Project in progress

With a lot of inspiration from Boston Dynamics projects, I'm trying to make something great without million dollars.

• 17,182 views
• 143 respects

## Components and supplies

 Arduino Mega 2560 & Genuino Mega 2560
×1
×1
 Raspberry Pi 3 Model B
×1
 LIDAR Laser
×1
 Servos (Tower Pro MG996R)
×1
 GY-521 MPU-6050 3 Axis Gyroscope + Accelerometer Module For Arduino
×1
 xTion PRO live
×1

## Necessary tools and machines

 Amateur tools and machines and offcourse, a lot of work.

JQR Quadruped Robot is a DIY project with the main objective to build an autonomous, legged robot that will help people in many activities.

The project was born in the summer of 2014 after months dedicated to the study of notions concerning the motion of mammalian quadrupeds. This study was essential to understand the issues and benefits of a self-propelled robot over the use of wheels. A lot of thanks to Boston Dynamics!

The conclusions I have arrived at say that the advantage of the motion generated by legs over the motion generated by the wheels is equity.

JQR Quadruped Robot is a DIY project is intended to help with activities from the simplest to the most complex task:

• To help monitor elderly people or children when we are away. (via PC or smartphone)
• Let rescuers find/help peoples on dirty or impervious terrains.
• To explore and shoot videos of places on impervious terrains.
• To explore damaged areas due to earthquake or flooding.
• To have a "watchdog" monitor the house when you're away. (via PC or smartphone)
• To help operators check sewers and pipes.
• To study walking cycle, CG and general physics/electronics/robotic matters.

UPDATE 25/May 2019:

Here are the new results for the robot's eyes. I am trying many resolutions and many types of X and Y amplitude of the view. Truly an exciting subject! Currently the lateral width X of the view is 60 degrees while the vertical width Y is 50 degrees. in the lower resolution (400 total pixels) such scanning takes place in less than 6 seconds! (precisely 5.80 seconds); exceptional result considering that even with such a low resolution the robot will understand the shape of the objects. With medium resolutions (there are 3 types of medium resolutions) I get scans of 800/1000 pixels in a time frame that varies between 8.5 seconds to 12 seconds. With higher resolutions I get from 2000 up to 5000/6000 pixels (virtually there are no limits regarding the number of pixels) with waiting times ranging from 40 seconds upwards.

I attach an image to show you the various resolutions obtained. Currently the laser head is composed of a frame that mounts two servo motors and the laser itself. I'm designing a laser head that can tilt so that I always see the right angle even if the robot is tilted laterally or longitudinally (like the heads of owls so to speak); this would allow me to perform visual scans even while the robot is walking or balancing! (currently every 5/10 seconds of walking the robot is forced to stop to "update its view"; if it does not stop, the image it would get with the visual scan would be distorted and not consistent with the previous scan, causing various problems in the code that concerns the view.

Please save the image and zoom it a little :) (here can't see it good)

## Code

##### basic example codeArduino
```//tratta da 3056C

//e spinta zampe posteriori forse i polpacci si devono aprire prima!!

//LIBRERIA WIRE MODIFICATA! riferirsi al sito per twi.c e twi.h modificate e istruzioni!
//ALTRA LIBREIRA MODIFICATA : MPU6050_6Axis_MotionApps20.h
//VALORI UTILIZZATI : 0x02,   0x16,   0x02,   0x00, 0x03 // D_0_22 inv_set_fifo_rate

/*
prova a :

A) Se il robot sta perdendo l'equilibrio durante fase ricarica: BLOCCARE RICARICA E PENSARE AD EQUILIBRIO!!
Magari come inizia fase ricarica, il robot puo' alzare leggermente la zampa per vedere se perde l'equilibrio!
Se SI interrompe e la zampa,dato che era appena alzata, torna subito al suo posto!
Se NO allora continua la fase ricarica!

B) la zampa che ha appena caricato NON deve essere utilizzata per finire l'equilibrio fatto in pre ricarica

C) Provare a mettere il peso del robot nella zampa che ha appena ricaricato in fase di spinta!

E) RIDURRE VALORE NUMERICO ANGOLI ROLL E PITCH!
ora sono troppo alti per l'inclinazione reale!
RISULTATO:
SE SI RIDUCE IN 90/M_PI, I GRADI AUMENTANO E DIMINUISCONO MENO DURANTE INCLINAZIONE!
Primo pensiero che viene e' che, con le attuali routine, si perde di precisione nell'
equilibrio del robot! ma ci sarebbe da approfondire il discorso e magari provare
un valore tra 180 e 90, diciamo 135 per trovare un compromesso che ci soddisfi piu' dell'
equilibrio settato con 180/M_PI

D) e considera anche che potresti fare che una zampa quando
troppo allungata (tipo zampe posteriori in spinta) allora
riduce o proprio non fa movimento equilibrio!

*/

/*
fare una routine che equilibria il robot, SOLO(?) in fase di ricarica, utilizzando i sensori tattili
e cercando di portare, se ad esempio si sta caricando zampa 1, tale zampa a un leggerissimo
contatto con il terreno rispetto alle altre zampe.
Sarebbe gia' pronta per caricare senza rischio di far cadere il robot
posso gia' usare i tasti set target su processing!

EQUILIBRIO ZAMPA 1:
A) prima si agisce lateralmente, facendo inclinare JQR a sinistra aspettando che la pressione
della zampa 1 entri nel "leggerissimo"

B) Se l'inclinazione laterale non basta, parte l'inclinazione indietro. A fine corsa si dovrebbe
aver ottenuto la zampa 1 con pressione nel terreno "leggerissima"

A e B possono anche partire e avanzare insieme eh!

oppure:
ALZARE LEGGERMENTE (braccio+avambraccio) LA ZAMPA OPPOSTA (quindi zampa 4) COME
QUANDO SI RICARICA MA SOLTANTO DI POCHISSIMO, IL TANTO CHE BASTA PER FARE
UNA PICCOLA CADUTA DALLA PARTE OPPOSTA ALLA ZAMPA CHE DEVE RICARICARE (LA 1)
*/

#include "MemoryFree.h"
//#include <SoftwareSerial.h>
//#include "avr/wdt.h" WATCHDOG

//--------------------------------- GYRO
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
//#include "NBWire.h"
#endif

MPU6050 accelgyro(0x69); //serve? cmq prova anche con 69

MPU6050 mpu;

//#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
// packet structure for InvenSense teapot demo
//uint8_t teapotPacket[14] = { '\$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
mpuInterrupt = true;
}

// called this way, it uses the default address 0x40
// you can also call it with a different address you want

// Depending on your servo make, the pulse width min and max may vary, you
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!

// ZAMPA 1 -AVANTI DESTRA
//AVAMBRACCIO 1
#define SERVOMIN_ava1  200 //180 - 630
#define SERVOMAX_ava1  650

//BRACCIO 1 -- ORA E' SPALLA ZAMPA 4
#define SERVOMIN_bra1  165 //180
#define SERVOMAX_bra1  615 // 640

//SPALLA 1
#define SERVOMIN_spa1  180 //330-600 piu aumenta piu la spalla si allarga
#define SERVOMAX_spa1  630

//-------------------------------------------------------------------

// ZAMPA 2 -AVANTI SINISTRA
//AVAMBRACCIO 2
#define SERVOMIN_ava2  150 //era 190 640 ora devo usare 160 610 alzandolo si abbassa l'avambraccio
#define SERVOMAX_ava2  600

//BRACCIO 2
#define SERVOMIN_bra2  165 //braccio deve andare leggerm avanti riducendo il valore!
#define SERVOMAX_bra2  615

//SPALLA 2
#define SERVOMIN_spa2  100 //120
#define SERVOMAX_spa2  550 //570

//-------------------------------------------------------------------

// ZAMPA 3 -DIETRO DESTRA
//AVAMBRACCIO 3
#define SERVOMIN_ava3  150
#define SERVOMAX_ava3  600

//BRACCIO 3
#define SERVOMIN_bra3  150       //140-590
#define SERVOMAX_bra3  600

//SPALLA 3
#define SERVOMIN_spa3  70 //90-540
#define SERVOMAX_spa3  520

//-------------------------------------------------------------------

// ZAMPA 4 -DIETRO SINISTRA
//AVAMBRACCIO 4
#define SERVOMIN_ava4  150
#define SERVOMAX_ava4  600 //prova abbassandolo

//BRACCIO 4
#define SERVOMIN_bra4  130 //164-600 alzando il valore torna indietro
#define SERVOMAX_bra4  580

//SPALLA 4
#define SERVOMIN_spa4  90 //90-540
#define SERVOMAX_spa4  540

// our servo # counter
uint8_t servonum = 0;

//---------------------------- SENSORI TATTILI -----------------------------
int fsrAnalogPin1 = 0; // FSR is connected to analog 0
int fsrAnalogPin2 = 1; // FSR is connected to analog 1
int fsrAnalogPin3 = 2; // FSR is connected to analog 2
int fsrAnalogPin4 = 3; // FSR is connected to analog 3
//--------------------------------------------------------------------------
//per camminata furtiva
int fase_mov_zampe = 0, fase_mov_zampa2 = 0, fase_mov_zampa3 = 0, fase_mov_zampa4 = 0;
String equilibrio_str = "";

//per camminata test (furtiva 2)
int routine_equilibrio = 1;// 1 --> on / 0 --> off routine equilibrio
int target_x = 0, target_y = 0;
int valore_tar_sin2 = 0,valore_tar_sin4 = 0, valore_tar_des1 = 0, valore_tar_des3 = 0;
int valore_tar_sin2b = 0,valore_tar_sin4b = 0, valore_tar_des1b = 0, valore_tar_des3b = 0;
int valore_tar_des1_SOMMA = 0, valore_tar_des3_SOMMA = 0, valore_tar_sin2_SOMMA = 0, valore_tar_sin4_SOMMA = 0;
int valore_tar_des1_FIN = 0, valore_tar_des3_FIN = 0, valore_tar_sin2_FIN = 0, valore_tar_sin4_FIN = 0;
int valore_vel_des1 = 0, valore_vel_des3 = 0, valore_vel_sin2 = 0, valore_vel_sin4 = 0;
int valore_vel_des1b = 0, valore_vel_des3b = 0, valore_vel_sin2b = 0, valore_vel_sin4b = 0;
int valore_vel_des1_FIN = 0, valore_vel_des3_FIN = 0, valore_vel_sin2_FIN = 0, valore_vel_sin4_FIN = 0;
int dist_angolo_roll_con_target_x;
int dist_angolo_pitch_con_target_y;
int velocita_mov, velocita_movb;
int flag_kfin = 0;
int pillozzo = 0;
int pillozzoB = 0;
int timer_raggiung_target_x = 0;
int timer_raggiung_target_y = 0;
int timer_generico = 0, timer_generico2 = 0, timer_generico3 = 0;
//int timer_generico3 = 0;
int durata_timer = 0;
int target_x_appo = 0;
int target_y_appo = 0;
int tempo_attesa_appo = 0;
int durata_spinte = 0;
int timer_check_assi = 0;
int fase_equilibrio = 0;
int inclinazione_a = 0; //da togliere
int zona_velocita = 0;
int fase_equilibrio_B = 0, fase_equilibrio_C = 0;
int zampa1_no_equilibrio = 0, zampa2_no_equilibrio = 0, zampa3_no_equilibrio = 0, zampa4_no_equilibrio = 0;
int speed_zona_velocita1 , speed_zona_velocita2;
int riflessi_massimi = 1;
int spinta_zampe = 0;

int targ_crono_vel_equilib = 0;

int pulselengthz;
int pulselengthzb;
int pulselengthzc;

int new_pos_braccio1;
int new_pos_braccio2;
int new_pos_avambraccio1;
int new_pos_avambraccio2;

int new_pos_spalla1;
int new_pos_spalla2;
int new_pos_spalla3;
int new_pos_spalla4;
int new_pos_braccio4;
int new_pos_avambraccio3;
int new_pos_avambraccio4;

//-------------------- CONFIGURAZIONE INIZIALE ZAMPE ><
//-------------------------------ZAMPA NUMERO 1 - AVANTI DESTRA---------------------------
//AVAMBRACCIO 1
int pos_avambraccio1 = 60; // 140=MAX (avambr tutto aperto)//     60
// 0=MIN  (avambr tutto chiuso)
// 70=MED
//BRACCIO 1
int pos_braccio1 = 115; // 140=MAX(braccio tutto dietro //   115
// 0=MIN (braccio tutto avanti)
// 70=MED
//SPALLA 1
int pos_spalla1 = 60; //120=MAX (spalla tutta chiusa)  95 -valore minore chiude ascella
// 0=MIN (spalla tutta aperta)
//60 = MED

//-------------------------------ZAMPA NUMERO 2 - AVANTI SINISTRA---------------------------
//AVAMBRACCIO 2
int pos_avambraccio2 = 60; // 140=MAX (avambr tutto aperto)           60
// 0=MIN  (avambr tutto chiuso)
// 70=MED
//BRACCIO 2
int pos_braccio2 = 115; // 140=MAX(braccio tuto dietro                115
// 0=MIN (braccio tutto avanti)
// 70=MED
//SPALLA 2
int pos_spalla2 = 60; // 120 =MAX (spalla tutta chiusa)
//  0 =MIN (spalla tutta aperta)
//  60 = MED

//-------------------------------ZAMPA NUMERO 3 - DIETRO DESTRA---------------------------
//AVAMBRACCIO 3
int pos_avambraccio3 = 60; // 140=MAX (avambr tutto aperto)        60
// 0=MIN  (avambr tutto chiuso)
// 70=MED   OK 60

//BRACCIO 3
int pos_braccio3 = 35; // 140=MAX(braccio tutto dietro             35
// 0=MIN (braccio tutto avanti)
// 70=MED
//SPALLA 3
int pos_spalla3 = 60; //120=MAX (spalla tutta chiusa)
// 0=MIN (spalla tutta aperta)
//60 = MED

//-------------------------------ZAMPA NUMERO 4 - DIETRO SINISTRA---------------------------
//AVAMBRACCIO 4
int pos_avambraccio4 = 60; // 140=MAX (avambr tutto aperto)         60
// 0=MIN  (avambr tutto chiuso)
// 70=MED
//BRACCIO 4
int pos_braccio4 = 35; // 140=MAX(braccio tutto dietro              35
// 0=MIN (braccio tutto avanti)
// 70=MED
//SPALLA 4
int pos_spalla4 = 60; //120=MAX (spalla tutta chiusa)
// 0=MIN (spalla tutta aperta)
//60 = MED

/*
//-------------------- CONFIGURAZIONE INIZIALE A MO DI SPRINTER IN POSIZIONE
//-------------------------------ZAMPA NUMERO 1 - AVANTI DESTRA---------------------------
//AVAMBRACCIO 1
int pos_avambraccio1 = 60; // 140=MAX (avambr tutto aperto)//             80
// 0=MIN  (avambr tutto chiuso)
// 70=MED
//BRACCIO 1
int pos_braccio1 = 130; // 140=MAX(braccio tutto dietro //                  90
// 0=MIN (braccio tutto avanti)
// 70=MED
//SPALLA 1
int pos_spalla1 = 60; //160=MAX (spalla tutta chiusa)  85
// 0=MIN (spalla tutta aperta)
//90 = MED

//-------------------------------ZAMPA NUMERO 2 - AVANTI SINISTRA---------------------------
//AVAMBRACCIO 2
int pos_avambraccio2 = 65; // 140=MAX (avambr tutto aperto)
// 0=MIN  (avambr tutto chiuso)
// 70=MED
//BRACCIO 2
int pos_braccio2 = 105; // 140=MAX(braccio tuto dietro 60
// 0=MIN (braccio tutto avanti)
// 70=MED
//SPALLA 2
int pos_spalla2 = 60; //160=MAX (spalla tutta chiusa)
// 0=MIN (spalla tutta aperta)
//90 = MED

//-------------------------------ZAMPA NUMERO 3 - DIETRO DESTRA---------------------------
//AVAMBRACCIO 3
int pos_avambraccio3 = 55; // 140=MAX (avambr tutto aperto) 6
// 0=MIN  (avambr tutto chiuso)
// 70=MED

//BRACCIO 3
int pos_braccio3 = 25; // 140=MAX(braccio tutto dietro 60
// 0=MIN (braccio tutto avanti)
// 70=MED
//SPALLA 3
int pos_spalla3 = 60; //160=MAX (spalla tutta chiusa)
// 0=MIN (spalla tutta aperta)
//90 = MED

//-------------------------------ZAMPA NUMERO 4 - DIETRO SINISTRA---------------------------
//AVAMBRACCIO 4
int pos_avambraccio4 = 65; // 140=MAX (avambr tutto aperto) 60
// 0=MIN  (avambr tutto chiuso)
// 70=MED
//BRACCIO 4
int pos_braccio4 = 45; // 140=MAX(braccio tutto dietro  60             45
// 0=MIN (braccio tutto avanti)
// 70=MED
//SPALLA 4
int pos_spalla4 = 60; //160=MAX (spalla tutta chiusa)
// 0=MIN (spalla tutta aperta)
//90 = MED
*/

//int ginolillo = 0;
int pos_avambraccio1_fin = pos_avambraccio1;
int pos_braccio1_fin = pos_braccio1;
int pos_spalla1_fin = pos_spalla1;
int pos_avambraccio2_fin = pos_avambraccio2;
int pos_braccio2_fin = pos_braccio2;
int pos_spalla2_fin = pos_spalla2;
int pos_avambraccio3_fin = pos_avambraccio3;
int pos_braccio3_fin = pos_braccio3;
int pos_spalla3_fin = pos_spalla3;
int pos_avambraccio4_fin = pos_avambraccio4;
int pos_braccio4_fin = pos_braccio4;
int pos_spalla4_fin = pos_spalla4;

int appo_pos_braccio1_fin, appo_pos_avambraccio1_fin, appo_pos_spalla1_fin;
int appo_pos_braccio2_fin, appo_pos_avambraccio2_fin, appo_pos_spalla2_fin;
int appo_pos_braccio3_fin, appo_pos_avambraccio3_fin, appo_pos_spalla3_fin;
int appo_pos_braccio4_fin, appo_pos_avambraccio4_fin, appo_pos_spalla4_fin;
int ok_leg_back_position = 0;

int val_stand_attesa = 2;
int timer_bra1 = 0, attesa_bra1 = val_stand_attesa;
int timer_ava1 = 0, attesa_ava1 = val_stand_attesa;
int timer_spa1 = 0, attesa_spa1 = val_stand_attesa;
int timer_bra2 = 0, attesa_bra2 = val_stand_attesa;
int timer_ava2 = 0, attesa_ava2 = val_stand_attesa;
int timer_spa2 = 0, attesa_spa2 = val_stand_attesa;
int timer_bra3 = 0, attesa_bra3 = val_stand_attesa;
int timer_ava3 = 0, attesa_ava3 = val_stand_attesa;
int timer_spa3 = 0, attesa_spa3 = val_stand_attesa;
int timer_bra4 = 0, attesa_bra4 = val_stand_attesa;
int timer_ava4 = 0, attesa_ava4 = val_stand_attesa;
int timer_spa4 = 0, attesa_spa4 = val_stand_attesa;

int moto_zampa1 = 0; // 0 = zampa eseguira' codice - 10 = zampa disabilitata
int moto_zampa2 = 0;
int moto_zampa3 = 0;
int moto_zampa4 = 0;

//---------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------
//fai ricarica zampa3! prendi dati posizione zampe nel momento in cui ricarica
//la zampa 3 e poi crea la scenetta per provare nuova ricarica (che devi ancora fare!)

int start = 7; //7;      //0= AVVIENE SOLO IL SETTAGGIO INIZIALE - 5= MOVIMENTI ZAMPE TROTTO -7= MOVIMENTO FURTIVO - 10= TEST
int velocita_spinta; //12
//---------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------

int camminata_test = 0;
int incomingByte;
int reset_saltelli = 0;
int timer_ricarica1 = 0, timer_ricarica2 = 0, timer_ricarica3 = 0, timer_ricarica4 = 0;
int posiz_appo = 0;
int posiz_appo2 = 0;

// 0= spinta /  1= ricarica  / 10 = disabilitata spinta
int ok_zampa1 = 0;
int ok_zampa2 = 0;
int ok_zampa3 = 0;
int ok_zampa4 = 0;
int pausa = 0;
int movimento_on = -1;
int movimento_on_flag = 0;//serve nell'attesa prima dello show!
int angolo_rotta_target = 0;
int secondi;
int fase = 0;
String risposta_direzione;

// angolo Z - YAWN BUSSOLA
int angolo_rotta_attuale_reale = 0;
int angolo_rotta_attuale = 0;
int conto_rotta = 0;
int angolo_rotta_attuale_appo1 = 0, angolo_rotta_attuale_appo2 = 0, angolo_rotta_attuale_appo3 = 0;

// angolo X - LATERAL ROLL
float angolo_roll_reale = 0;
int angolo_roll_attuale = 0;
int conto_roll = 0;
int angolo_roll_appo1 = 0, angolo_roll_appo2 = 0, angolo_roll_appo3 = 0;

// angolo Y - PITCH
int angolo_pitch_reale = 0;
int angolo_pitch_attuale = 0;
int conto_pitch = 0;
int angolo_pitch_appo1 = 0, angolo_pitch_appo2 = 0, angolo_pitch_appo3 = 0;

int z; //asse z gyro -yaw- RAW (bussola)
int x; //asse x gyro -roll- RAW
int y; //asse y gyro -pitch- RAW

//------------------------------------ ARRAY VISTA

byte array_vista[461];
int i = 1;

int timer_for = 0;

int secondi_test;
int decimi_test;
int centesimi_test;
int index = 0;

//____________________________________________SETTING INIZIALE_________________________________________
void setup()
{

//Memorizzazione INIZIALE attuale posizione zampe
pos_braccio2_fin = pos_braccio2;
pos_avambraccio2_fin = pos_avambraccio2;

for(i = 1; i <= 461; i = i + 1)
{
array_vista[i] = 0;
}

i = 1; //pronta per dopo!

/*
if(timer_for == 0)
{
for(i = 1; i < 121; i = i + 1)
{
//Serial.println(myPins[i]);
array_vista[i] = -1;

}
timer_for = 1;
}
*/

//wdt_enable(WDTO_2S); WATCHDOG
//-------------------------- GYRO
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(200, true); //o 400????
#endif
Serial.begin(115200); //115200
//Serial.begin(57600);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
mpu.initialize();
// load and configure the DMP
//Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); //1788 // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
//Serial.println(F("DMP ready! Waiting for first interrupt..."));
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {

}
// configure LED for output
//pinMode(LED_PIN, OUTPUT);

pwm.begin();
pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

//30 170    ---  10 150 (se invertito)
//DEVI CONVERTIRE PER OTTENERE DI POTER ORDINARE MOVIMENTI DA 0 a 140

//************************************************************************************** ZAMPA 1 SETUP
//SETTAGGIO INIZIALE AVAMBRACCIO - 0
//pos_avambraccio1 = pos_avambraccio1 + 10; //+10 perche' e'in mirror se no sarebbe +30
new_pos_avambraccio1 = 180 - pos_avambraccio1;
pulselengthz = map(new_pos_avambraccio1, 0, 180, SERVOMIN_ava1, SERVOMAX_ava1);
pwm.setPWM(0, 0, pulselengthz);

//SETTAGGIO INIZIALE BRACCIO - 1
//pos_braccio1 = pos_braccio1 + 10; //+10 perche' e'in mirror se no sarebbe +30
//new_pos_braccio1 = 180 - pos_braccio1;
pulselengthzb = map(pos_braccio1, 0, 180, SERVOMIN_bra1, SERVOMAX_bra1);
pwm.setPWM(1, 0, pulselengthzb);

//SETTAGGIO INIZIALE SPALLA - 2
/*
new_pos_spalla1 = 180 - pos_spalla1;
pulselengthzc = map(new_pos_spalla1, 0, 180, SERVOMIN_spa1, SERVOMAX_spa1);
pwm.setPWM(2, 0, pulselengthzc);
*/
//SETTAGGIO INIZIALE SPALLA - 2
new_pos_spalla1 = 180 - pos_spalla1;
pulselengthzc = map(new_pos_spalla1, 0, 180, SERVOMIN_spa1, SERVOMAX_spa1);
pwm.setPWM(2, 0, pulselengthzc);

//************************************************************************************** ZAMPA 2 SETUP
//SETTAGGIO INIZIALE AVAMBRACCIO - 4
//pos_avambraccio2 = pos_avambraccio2 + 30; //se fosse in mirror sarebbe +10
new_pos_avambraccio2 = 180 - pos_avambraccio2;
pulselengthz = map(new_pos_avambraccio2, 0, 180, SERVOMIN_ava2, SERVOMAX_ava2);
pwm.setPWM(4, 0, pulselengthz);

//SETTAGGIO INIZIALE BRACCIO - 6
//pos_braccio2 = pos_braccio2 + 10; //se fosse in mirror sarebbe +10
new_pos_braccio2 = 180 - pos_braccio2;
pulselengthzb = map(new_pos_braccio2, 0, 180, SERVOMIN_bra2, SERVOMAX_bra2);
pwm.setPWM(6, 0, pulselengthzb);

//SETTAGGIO INIZIALE SPALLA - 7
new_pos_spalla2 = 180 - pos_spalla2;
pulselengthzc = map(new_pos_spalla2, 0, 180, SERVOMIN_spa2, SERVOMAX_spa2);
pwm.setPWM(7, 0, pulselengthzc);

//************************************************************************************** ZAMPA 3 SETUP
//SETTAGGIO INIZIALE AVAMBRACCIO - 8
pos_avambraccio3 = pos_avambraccio3 + 0;// e' mirror quindi + 10
new_pos_avambraccio3 = 180 - pos_avambraccio3;
pulselengthz = map(new_pos_avambraccio3, 0, 180, SERVOMIN_ava3, SERVOMAX_ava3);
pwm.setPWM(8, 0, pulselengthz);

//SETTAGGIO INIZIALE BRACCIO - 9
//pos_braccio3 = pos_braccio3 + 30;//non e' mirror quindi + 30
//new_pos_braccio3 = 190 - pos_braccio3;
pulselengthzb = map(pos_braccio3, 0, 180, SERVOMIN_bra3, SERVOMAX_bra3);
pwm.setPWM(9, 0, pulselengthzb);

//SETTAGGIO INIZIALE SPALLA - 10
new_pos_spalla3 = 180 - pos_spalla3;
pulselengthzc = map(new_pos_spalla3, 0, 180, SERVOMIN_spa3, SERVOMAX_spa3);
pwm.setPWM(10, 0, pulselengthzc);

//************************************************************************************** ZAMPA 4 SETUP
//SETTAGGIO INIZIALE AVAMBRACCIO - 12
//pos_avambraccio4 = pos_avambraccio4 + 10;//e' mirror quindi + 10
//new_pos_avambraccio4 = 180 - pos_avambraccio4;
new_pos_avambraccio4 = pos_avambraccio4;
//new_pos_avambraccio4 = pos_avambraccio4;
pulselengthz = map(new_pos_avambraccio4, 0, 180, SERVOMIN_ava4, SERVOMAX_ava4);
pwm.setPWM(12, 0, pulselengthz);

//SETTAGGIO INIZIALE BRACCIO - 13
//pos_braccio4 = pos_braccio4 + 30;//non e' mirror quindi + 30
new_pos_braccio4 = 180 - pos_braccio4;
pulselengthzb = map(new_pos_braccio4, 0, 180, SERVOMIN_bra4, SERVOMAX_bra4);
pwm.setPWM(13, 0, pulselengthzb);

//SETTAGGIO INIZIALE SPALLA - 15
//new_pos_spalla4 =  180 - pos_spalla4;
new_pos_spalla4 = pos_spalla4;
pulselengthzc = map(new_pos_spalla4, 0, 180, SERVOMIN_spa4, SERVOMAX_spa4);
pwm.setPWM(15, 0, pulselengthzc);

}
//______________________________________________________________________________________________________

int flag_memorizz_coord_bra1, flag_memorizz_coord_bra2, flag_memorizz_coord_bra3, flag_memorizz_coord_bra4;

int flag_memorizz_coord_avambra1, flag_memorizz_coord_avambra2, flag_memorizz_coord_avambra3, flag_memorizz_coord_avambra4;

int flag_memorizz_coord_zampa = 0;

// VARIABILI ALTEZZA ROBOT
int routine_altezza = 0;
int step_altezza = 5; //di quanto deve diminuire altezza robot
int flag_altezza = 0, flag_altezzaB = 0; //setup
int fine_altezz_zampa1 = 0, fine_altezz_zampa2 = 0, fine_altezz_zampa3 = 0, fine_altezz_zampa4 = 0;
int alt_posizione_bra1 , alt_posizione_bra2, alt_posizione_bra3, alt_posizione_bra4;
int alt_posizione_avambra1, alt_posizione_avambra2, alt_posizione_avambra3, alt_posizione_avambra4;
int flag_altezza_serial = 0;
int pos_avambraccio1_alt_targ, pos_avambraccio2_alt_targ, pos_avambraccio3_alt_targ, pos_avambraccio4_alt_targ;
int modo_altezza1 = 0, modo_altezza2 = 0, modo_altezza3 = 0, modo_altezza4 = 0;
int timer_generico_equilibrio = 0;
int flag_generico1 = 0;

int pausa_reflex = 1;
int speed_reflex = 1; //per il braccio sara' 1 mentre per l'avambr sara' speed_reflex * 2

int cilla = 0;
int equilibrio_off = 0;

int crono_spinte = 0;
int lentezza_spinte = 1;
int no_mov_zampa1 = 0, no_mov_zampa2 = 0;
int altezza_robot_totale = 0;
int altezza_robot_zampa1 = 0, altezza_robot_zampa2 = 0, altezza_robot_zampa3 = 0, altezza_robot_zampa4 = 0;
int timerk = 0;
int flag_pippo = 0;
int flag_incomingByte = 1;
int flag_equilib_zampe = 0;

//____________________________________________LOOP PRINCIPALE___________________________________________
void loop()
{

//ram_rimasta = freeRam();

Serial.print(angolo_rotta_attuale);  // rotta attuale
Serial.print(",");
Serial.print(angolo_rotta_target);  // rotta target
Serial.print(",");
Serial.print(risposta_direzione);  // risposta direzione
Serial.print(",");
Serial.print(movimento_on,DEC); //movimento on
Serial.print(",");
Serial.print(angolo_roll_attuale);  // x roll
Serial.print(",");
Serial.print(angolo_pitch_attuale);  // y pitch
Serial.print(",");
Serial.print(fase_mov_zampe,DEC);  // fase_mov_zampe ---> fase movimento furtivo
Serial.print(",");
Serial.print(equilibrio_str);  // equilibrio_str
Serial.print(",");
Serial.print(target_x);  // target_x
Serial.print(",");
Serial.print(target_y);  // target_y
Serial.print(",");

Serial.print(velocita_mov);  //
Serial.print(",");

Serial.print(flag_kfin);  //
Serial.print(",");

Serial.print(valore_vel_sin2_FIN);  //
Serial.print(",");

Serial.print(valore_vel_sin4_FIN);  //
Serial.print(",");

Serial.print(valore_vel_des1_FIN);  //
Serial.print(",");

Serial.print(valore_vel_des3_FIN);  //
Serial.print(",");
//----------------
Serial.print(valore_tar_sin2_SOMMA);  // era valore_tar_sin2_FIN = valore target per esempio 105 (posizione da raggiungere)
Serial.print(",");
Serial.print(valore_tar_sin4_SOMMA);  //
Serial.print(",");
Serial.print(valore_tar_des1_SOMMA);  //
Serial.print(",");
Serial.print(valore_tar_des3_SOMMA);  //
Serial.print(",");

Serial.print(pos_braccio1_fin);  //
Serial.print(",");

Serial.print(pos_avambraccio1_fin);  //
Serial.print(",");

Serial.print(valore_tar_des1_FIN);  //
Serial.print(",");

Serial.print(",");

Serial.print(",");

Serial.print(",");

Serial.print(",");

/*
Serial.print(fase_equilibrio);  //
Serial.print(",");

Serial.print(timer_target_x_raggiunto);  //
Serial.print(",");

Serial.print(angolo_roll_attuale);  //
Serial.print(",");
*/
Serial.print(velocita_mov);  //
Serial.print(",");
Serial.print(valore_tar_sin2);  //
Serial.print(",");
Serial.print(dist_angolo_roll_con_target_x);  //
Serial.print(",");
Serial.print(inclinazione_a);  //
Serial.print(",");

Serial.print(fase_equilibrio);  //
Serial.print(",");
Serial.print(timer_check_assi);  //
Serial.print(",");
Serial.print(zona_velocita);  //
Serial.print(",");
//Serial.print(ok_zampa2);  //
//Serial.print(",");

Serial.print(array_vista[0]);
Serial.print(",");
Serial.print(array_vista[1]);
Serial.print(",");
Serial.print(array_vista[2]);
Serial.print(",");
Serial.print(array_vista[3]);
Serial.print(",");
Serial.print(array_vista[4]);
Serial.print(",");
Serial.print(array_vista[5]);
Serial.print(",");
Serial.print(array_vista[6]);
Serial.print(",");
Serial.print(array_vista[7]);
Serial.print(",");
Serial.print(array_vista[8]);
Serial.print(",");
Serial.print(array_vista[9]);
Serial.print(",");

Serial.print(array_vista[10]);
Serial.print(",");
Serial.print(array_vista[11]);
Serial.print(",");
Serial.print(array_vista[12]);
Serial.print(",");
Serial.print(array_vista[13]);
Serial.print(",");
Serial.print(array_vista[14]);
Serial.print(",");
Serial.print(array_vista[15]);
Serial.print(",");
Serial.print(array_vista[16]);
Serial.print(",");
Serial.print(array_vista[17]);
Serial.print(",");
Serial.print(array_vista[18]);
Serial.print(",");
Serial.print(array_vista[19]); //54a variabile spedita
Serial.print(",");
Serial.print(ok_zampa3);
Serial.print(",");
Serial.print(pos_avambraccio1_fin);
Serial.print(",");
Serial.print(pos_braccio3_fin);
Serial.print(",");
Serial.print(pos_avambraccio3_fin);
Serial.print(",");

Serial.print(pos_braccio2_fin);
Serial.print(",");
Serial.print(pos_avambraccio2_fin);
Serial.print(",");
Serial.print(pos_braccio4_fin);
Serial.print(",");
Serial.print(pos_avambraccio4_fin);
Serial.print(",");

//Serial.print(",");
Serial.println();
//Serial.print(ram_rimasta);

//---------------------------------------------------------------------------------- GYRO ENGINE
// if programming failed, don't try to do anything

// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {

// other program behavior stuff here
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)

if ((mpuIntStatus & 0x10) || fifoCount == 1024) //1024
//if ((mpuIntStatus & 0x10) || fifoCount == 1024) //1024
{
// reset so we can continue cleanly
mpu.resetFIFO();

//Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02)
{
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
z = ypr[0] * 180/M_PI; //yawn
x = ypr[1] * 270/M_PI; //roll SE SI RIDUCE IN 90/M_PI, I GRADI AUMENTANO MENO DURANTE INCLINAZIONE
y = ypr[2] * 270/M_PI; //pitch SE SI RIDUCE IN 90M_PI, I GRADI AUMENTANO MENO DURANTE INCLINAZIONE
//--------------------------------------------- valore asse Y - PITCH
//angolo_pitch_reale = y;
angolo_pitch_attuale = y;
//--------------------------------------------- valore asse X - LATERAL ROLL
//angolo_roll_reale = x;
angolo_roll_attuale = x;
//--------------------------------------------- valore asse Z - YAWN BUSSOLA (trasformato in 360)
if (z <= 0 && z >= -179)
{
angolo_rotta_attuale = (z + 361) % 360;
}
if (z >= 0 && z <= 179)
{
angolo_rotta_attuale = (z + 360) % 360;
}

#endif
}//fine else if (mpuIntStatus & 0x02) {

//------------------------------------------------- COMANDI DA TASTIERA E SERIALI DA PROCESSING
if (Serial.available() > 0)
{
if (incomingByte == 'p')
{
if (pausa == 0)
{
pausa = 1;
}
else
{
pausa = 0;
}
}

if (incomingByte == 'q')
{
flag_incomingByte = 0;
} //fine if (incomingByte == 'q')

if (flag_incomingByte == 0)
{
if (movimento_on == 0 || movimento_on == -1)
{
movimento_on = 1;
movimento_on_flag = 0;
flag_incomingByte = 1;
}
}// fine if (flag_incomingByte == 0)

if (flag_incomingByte == 0)
{
if (movimento_on == 1)
{
movimento_on = -1;
movimento_on_flag = 0;
...

```

## Schematics

#### Author

##### aldoz
• 1 project
• 35 followers

• Just inspiration. by Boston Dynamics

April 26, 2017

#### Members who respect this project

and 135 others

See similar projects
you might like

#### OpenCat

Project in progress by Team Petoi

• 216,754 views
• 1,568 respects

#### MeArm Robot Arm - Your Robot - V1.0

Project tutorial by Benjamin Gray

• 19,302 views
• 34 respects

#### Mike's Robot Dog

Project in progress by Mike Rigsby

• 10,865 views
• 55 respects

#### Autonomous Home Assistant Robot

Project tutorial by Juan Miguel Jimeno

• 7,942 views
• 67 respects

#### DIY 3-Axis CNC VMC

Project tutorial by Amit Nandi (BigWiz)

• 12,163 views