File content as of revision 4:3f22193053d0:

#include "mbed.h"
#include "Servo.h"
#include "Rover.h"
#include "math.h"
#include "TOFs.h"
//#include "MX.h"
//#include "UARTSerial_half.h"

#define DEBUG true
#define READ true 

 
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);

//UARTSerial_half dxl_port(PB_9,PB_8,PF_13, 1000000); 

//int ID[5] = {1,2,3,4,5};
//MX mx_MotorChain(ID, sizeof(ID)/sizeof(int), 1000000, &dxl_port);  

int main() {
      
      
 /*           bool enableVal[5] = {1,1,1,1,1};
            mx_MotorChain.SyncTorqueEnable(enableVal);
            
            float Goal_position[5] = {100,20,150,60,75};
            mx_MotorChain.SyncSetGoal(Goal_position); */
      
    float velf_a;
    float vels_a;    
    float vela_a; 
    float velf_m = 0.0;
    float vels_m = 0.0;    
    float vela_m = 0.0;
    
    float front_vel_dx = 0.0;
    float front_vel_sx = 0.0;    
    float retro_vel_dx = 0.0;
    float retro_vel_sx = 0.0;
    float forward_vel = 0.0; 
    float pitch;
    
    float jointFront_d = 0.0;
    float jointRetro_d = 0.0;
    float time = 0.0;
    
     
    float roll;
    printf("Creo il rover \r\n");
    Rover rover;
    
    
    rover.initializeImu();
    rover.initializeTofs(); 

    
    Timer schedulerTimer;  

    schedulerTimer.start();     

    rover.setCentralJointsAngle(0.0,0.0);  //in radiants  (front, retro)
    rover.setWheelsVelocity(0.0,0.0 , 0, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration
    
    rover.startEthComunication();

    wait(1);
    
    double timeStabPrec = 0.0;
    double timeReadPrec = 0.0;   
    double timePrintPrec = 0.0;    

    timeStabPrec = schedulerTimer.read();
    timePrintPrec = schedulerTimer.read();    
    timeReadPrec = schedulerTimer.read();    

    float dtStab = 5.0/1000;
    float dtRead = 20.0/1000;   
    float dtPrint = 500.0/1000;
    float dtPassedStab = 0.0;
    float Kp=1.2; //0.6
    
    float pitch_d = 0.0;
    
    float frontDistance;
    float retroDistance;   
        
    while (1){
         
         time = schedulerTimer.read();
         

         if(time - timeStabPrec > dtStab){  
         
             //rover.acquireTofs(frontDistance, retroDistance); 
             
             rover.computeCentralJointsFromTofs();
             dtPassedStab = time - timeStabPrec; 
             timeStabPrec = time;                

             rover.calcImuAngles(pitch, roll, dtStab);

             velf_a = forward_vel;
             vels_a = -Kp*M_PI*(pitch_d-pitch)/180;
             vela_a = 0.0;
                          
             rover.setWheelsVelocity(velf_a, vels_a , vela_a, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration
             //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125); 
             
             forward_vel = (float)rover.get_forward_vel()/1000;
             pitch_d = (float)rover.get_pitch()/1000;
             
             jointFront_d = (float)rover.get_jointFront()/1000;
             jointRetro_d = (float)rover.get_jointRetro()/1000;
             
             //rover.setCentralJointsAngle(jointFront_d,jointRetro_d);  //in radiants  (front, retro)

             
         }
         
          if(READ && time - timeReadPrec > dtRead){
             
             timeReadPrec = time;             
             //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125); 
             rover.getRoverWheelsVelocity(front_vel_dx, front_vel_sx, retro_vel_dx, retro_vel_sx); 
             
 
         }

       
         if(DEBUG && time - timePrintPrec > dtPrint){
             timePrintPrec = time;
              
             printf("Ts: %4.2f \t M2-M1 : %2.1f \r\n",dtPassedStab*1000,frontDistance);
               
             //printf(" Ts: %4.2f \t Vfa: %4.4f \t Vfm %4.4f \t Vsa: %4.4f \t Vsm: %4.4f \t A: %4.4f \r\n",dtPassedStab*1000, velf_a, velf_m, vels_a, vels_m, amplitude);

             if(rover.getEthState() == 1){ 
                myled1 = true;
                myled2 = false;
             }else if(rover.getEthState() == 2){
                myled1 = true;
                myled2 = true;
             }else{
                myled1 = false;
                myled2 = false;
             }        
                    
             

         }
          rover.ethComunicationUpdate(schedulerTimer.read(), pitch, frontDistance, retroDistance, dtPassedStab*1000, velf_a);
          //rover.ethComunicationUpdate(schedulerTimer.read(), pitch, velf_m, vels_m, dtPassedStab*1000, vel_r);

             
        
    }       

}