Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of Hyfliers_Completo_testato by
main.cpp
- Committer:
- anfontanelli
- Date:
- 2020-01-08
- Revision:
- 4:3f22193053d0
- Parent:
- 3:fc26045926d9
- Child:
- 6:584653235830
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); } }