JQR Quadruped Autonomous Robot ** UPDATE 25/NOVEMBER/2019 **

//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 OUTPUT_READABLE_YAWPITCHROLL
//#define OUTPUT_READABLE_QUATERNION

//#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// 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
void dmpDataReady() {
    mpuInterrupt = true;
}



#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);

// 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
    int fsrReading1; // the analog reading from the FSR resistor divider
    int fsrReading2; // the analog reading from the FSR resistor divider
    int fsrReading3; // the analog reading from the FSR resistor divider
    int fsrReading4; // the analog reading from the FSR resistor divider
    int fsrReading1_check = 0, fsrReading2_check = 0, fsrReading3_check = 0, fsrReading4_check = 0;
//--------------------------------------------------------------------------
//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 loading_scansione = 0;
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)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        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..."));
        dmpReady = true;
        // 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;    
  
     int flag_fine_raddrizzamento1 = 0, flag_fine_raddrizzamento2 = 0, flag_fine_raddrizzamento3 = 0, flag_fine_raddrizzamento4 = 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(fsrReading1);  // 
  Serial.print(",");

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

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

  Serial.print(fsrReading4);  // 
  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

    if (!dmpReady) return;
    // 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;  
        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // 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) 
  {
   incomingByte = Serial.read();
   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;
...

This file has been truncated, please download it to see its full contents.