PRISMA Lab / Mbed OS Hyfliers_Completo_testato

Clone per testare Mbed studio

Dependencies:   PwmIn IONMcMotor MPU6050 Eigen ROVER

Fork of Hyfliers_Completo_testato by Marco De Silva

main.cpp@4:3f22193053d0, 2020-01-08 (annotated)

Committer:
anfontanelli
Date:
Wed Jan 08 11:05:36 2020 +0000
Revision:
4:3f22193053d0
Parent:
3:fc26045926d9
Child:
6:584653235830
release bad

Who changed what in which revision?

UserRevisionLine numberNew contents of line
anfontanelli 3:fc26045926d9 1 #include "mbed.h"
anfontanelli 3:fc26045926d9 2 #include "Servo.h"
anfontanelli 3:fc26045926d9 3 #include "Rover.h"
anfontanelli 3:fc26045926d9 4 #include "math.h"
anfontanelli 3:fc26045926d9 5 #include "TOFs.h"
anfontanelli 3:fc26045926d9 6 //#include "MX.h"
anfontanelli 3:fc26045926d9 7 //#include "UARTSerial_half.h"
group-ST 0:6e8275981824 8
anfontanelli 4:3f22193053d0 9 #define DEBUG true
anfontanelli 3:fc26045926d9 10 #define READ true
anfontanelli 3:fc26045926d9 11
anfontanelli 3:fc26045926d9 12
anfontanelli 3:fc26045926d9 13 DigitalOut myled1(LED1);
anfontanelli 3:fc26045926d9 14 DigitalOut myled2(LED2);
anfontanelli 3:fc26045926d9 15
anfontanelli 3:fc26045926d9 16 //UARTSerial_half dxl_port(PB_9,PB_8,PF_13, 1000000);
anfontanelli 3:fc26045926d9 17
anfontanelli 3:fc26045926d9 18 //int ID[5] = {1,2,3,4,5};
anfontanelli 3:fc26045926d9 19 //MX mx_MotorChain(ID, sizeof(ID)/sizeof(int), 1000000, &dxl_port);
group-ST 0:6e8275981824 20
anfontanelli 3:fc26045926d9 21 int main() {
anfontanelli 3:fc26045926d9 22
anfontanelli 3:fc26045926d9 23
anfontanelli 3:fc26045926d9 24 /* bool enableVal[5] = {1,1,1,1,1};
anfontanelli 3:fc26045926d9 25 mx_MotorChain.SyncTorqueEnable(enableVal);
anfontanelli 3:fc26045926d9 26
anfontanelli 3:fc26045926d9 27 float Goal_position[5] = {100,20,150,60,75};
anfontanelli 3:fc26045926d9 28 mx_MotorChain.SyncSetGoal(Goal_position); */
anfontanelli 3:fc26045926d9 29
anfontanelli 3:fc26045926d9 30 float velf_a;
anfontanelli 3:fc26045926d9 31 float vels_a;
anfontanelli 3:fc26045926d9 32 float vela_a;
anfontanelli 3:fc26045926d9 33 float velf_m = 0.0;
anfontanelli 3:fc26045926d9 34 float vels_m = 0.0;
anfontanelli 3:fc26045926d9 35 float vela_m = 0.0;
anfontanelli 3:fc26045926d9 36
anfontanelli 4:3f22193053d0 37 float front_vel_dx = 0.0;
anfontanelli 4:3f22193053d0 38 float front_vel_sx = 0.0;
anfontanelli 4:3f22193053d0 39 float retro_vel_dx = 0.0;
anfontanelli 4:3f22193053d0 40 float retro_vel_sx = 0.0;
anfontanelli 3:fc26045926d9 41 float forward_vel = 0.0;
anfontanelli 3:fc26045926d9 42 float pitch;
anfontanelli 3:fc26045926d9 43
anfontanelli 3:fc26045926d9 44 float jointFront_d = 0.0;
anfontanelli 3:fc26045926d9 45 float jointRetro_d = 0.0;
anfontanelli 3:fc26045926d9 46 float time = 0.0;
anfontanelli 3:fc26045926d9 47
anfontanelli 3:fc26045926d9 48
anfontanelli 3:fc26045926d9 49 float roll;
anfontanelli 3:fc26045926d9 50 printf("Creo il rover \r\n");
anfontanelli 3:fc26045926d9 51 Rover rover;
anfontanelli 3:fc26045926d9 52
anfontanelli 3:fc26045926d9 53
anfontanelli 3:fc26045926d9 54 rover.initializeImu();
anfontanelli 3:fc26045926d9 55 rover.initializeTofs();
group-ST 0:6e8275981824 56
anfontanelli 3:fc26045926d9 57
anfontanelli 3:fc26045926d9 58 Timer schedulerTimer;
anfontanelli 3:fc26045926d9 59
anfontanelli 3:fc26045926d9 60 schedulerTimer.start();
anfontanelli 3:fc26045926d9 61
anfontanelli 3:fc26045926d9 62 rover.setCentralJointsAngle(0.0,0.0); //in radiants (front, retro)
anfontanelli 3:fc26045926d9 63 rover.setWheelsVelocity(0.0,0.0 , 0, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration
anfontanelli 3:fc26045926d9 64
anfontanelli 3:fc26045926d9 65 rover.startEthComunication();
anfontanelli 3:fc26045926d9 66
anfontanelli 3:fc26045926d9 67 wait(1);
anfontanelli 3:fc26045926d9 68
anfontanelli 3:fc26045926d9 69 double timeStabPrec = 0.0;
anfontanelli 3:fc26045926d9 70 double timeReadPrec = 0.0;
anfontanelli 3:fc26045926d9 71 double timePrintPrec = 0.0;
anfontanelli 3:fc26045926d9 72
anfontanelli 3:fc26045926d9 73 timeStabPrec = schedulerTimer.read();
anfontanelli 3:fc26045926d9 74 timePrintPrec = schedulerTimer.read();
anfontanelli 3:fc26045926d9 75 timeReadPrec = schedulerTimer.read();
group-ST 0:6e8275981824 76
anfontanelli 3:fc26045926d9 77 float dtStab = 5.0/1000;
anfontanelli 3:fc26045926d9 78 float dtRead = 20.0/1000;
anfontanelli 3:fc26045926d9 79 float dtPrint = 500.0/1000;
anfontanelli 3:fc26045926d9 80 float dtPassedStab = 0.0;
anfontanelli 3:fc26045926d9 81 float Kp=1.2; //0.6
group-ST 0:6e8275981824 82
anfontanelli 3:fc26045926d9 83 float pitch_d = 0.0;
group-ST 0:6e8275981824 84
anfontanelli 3:fc26045926d9 85 float frontDistance;
anfontanelli 3:fc26045926d9 86 float retroDistance;
anfontanelli 3:fc26045926d9 87
anfontanelli 3:fc26045926d9 88 while (1){
anfontanelli 3:fc26045926d9 89
anfontanelli 3:fc26045926d9 90 time = schedulerTimer.read();
anfontanelli 3:fc26045926d9 91
anfontanelli 3:fc26045926d9 92
anfontanelli 3:fc26045926d9 93 if(time - timeStabPrec > dtStab){
anfontanelli 3:fc26045926d9 94
anfontanelli 3:fc26045926d9 95 //rover.acquireTofs(frontDistance, retroDistance);
anfontanelli 3:fc26045926d9 96
anfontanelli 3:fc26045926d9 97 rover.computeCentralJointsFromTofs();
anfontanelli 3:fc26045926d9 98 dtPassedStab = time - timeStabPrec;
anfontanelli 3:fc26045926d9 99 timeStabPrec = time;
anfontanelli 3:fc26045926d9 100
anfontanelli 3:fc26045926d9 101 rover.calcImuAngles(pitch, roll, dtStab);
anfontanelli 3:fc26045926d9 102
anfontanelli 3:fc26045926d9 103 velf_a = forward_vel;
anfontanelli 3:fc26045926d9 104 vels_a = -Kp*M_PI*(pitch_d-pitch)/180;
anfontanelli 3:fc26045926d9 105 vela_a = 0.0;
anfontanelli 3:fc26045926d9 106
anfontanelli 3:fc26045926d9 107 rover.setWheelsVelocity(velf_a, vels_a , vela_a, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration
anfontanelli 3:fc26045926d9 108 //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125);
anfontanelli 3:fc26045926d9 109
anfontanelli 3:fc26045926d9 110 forward_vel = (float)rover.get_forward_vel()/1000;
anfontanelli 3:fc26045926d9 111 pitch_d = (float)rover.get_pitch()/1000;
anfontanelli 3:fc26045926d9 112
anfontanelli 3:fc26045926d9 113 jointFront_d = (float)rover.get_jointFront()/1000;
anfontanelli 3:fc26045926d9 114 jointRetro_d = (float)rover.get_jointRetro()/1000;
anfontanelli 3:fc26045926d9 115
anfontanelli 3:fc26045926d9 116 //rover.setCentralJointsAngle(jointFront_d,jointRetro_d); //in radiants (front, retro)
anfontanelli 3:fc26045926d9 117
anfontanelli 3:fc26045926d9 118
anfontanelli 3:fc26045926d9 119 }
anfontanelli 3:fc26045926d9 120
anfontanelli 3:fc26045926d9 121 if(READ && time - timeReadPrec > dtRead){
anfontanelli 3:fc26045926d9 122
anfontanelli 3:fc26045926d9 123 timeReadPrec = time;
anfontanelli 3:fc26045926d9 124 //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125);
anfontanelli 4:3f22193053d0 125 rover.getRoverWheelsVelocity(front_vel_dx, front_vel_sx, retro_vel_dx, retro_vel_sx);
anfontanelli 3:fc26045926d9 126
anfontanelli 3:fc26045926d9 127
anfontanelli 3:fc26045926d9 128 }
anfontanelli 3:fc26045926d9 129
anfontanelli 3:fc26045926d9 130
anfontanelli 3:fc26045926d9 131 if(DEBUG && time - timePrintPrec > dtPrint){
anfontanelli 3:fc26045926d9 132 timePrintPrec = time;
anfontanelli 3:fc26045926d9 133
anfontanelli 4:3f22193053d0 134 printf("Ts: %4.2f \t M2-M1 : %2.1f \r\n",dtPassedStab*1000,frontDistance);
anfontanelli 3:fc26045926d9 135
anfontanelli 3:fc26045926d9 136 //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);
anfontanelli 3:fc26045926d9 137
anfontanelli 3:fc26045926d9 138 if(rover.getEthState() == 1){
anfontanelli 3:fc26045926d9 139 myled1 = true;
anfontanelli 3:fc26045926d9 140 myled2 = false;
anfontanelli 3:fc26045926d9 141 }else if(rover.getEthState() == 2){
anfontanelli 3:fc26045926d9 142 myled1 = true;
anfontanelli 3:fc26045926d9 143 myled2 = true;
anfontanelli 3:fc26045926d9 144 }else{
anfontanelli 3:fc26045926d9 145 myled1 = false;
anfontanelli 3:fc26045926d9 146 myled2 = false;
anfontanelli 3:fc26045926d9 147 }
anfontanelli 3:fc26045926d9 148
anfontanelli 3:fc26045926d9 149
anfontanelli 3:fc26045926d9 150
anfontanelli 3:fc26045926d9 151 }
anfontanelli 4:3f22193053d0 152 rover.ethComunicationUpdate(schedulerTimer.read(), pitch, frontDistance, retroDistance, dtPassedStab*1000, velf_a);
anfontanelli 3:fc26045926d9 153 //rover.ethComunicationUpdate(schedulerTimer.read(), pitch, velf_m, vels_m, dtPassedStab*1000, vel_r);
anfontanelli 3:fc26045926d9 154
anfontanelli 3:fc26045926d9 155
anfontanelli 3:fc26045926d9 156
anfontanelli 3:fc26045926d9 157 }
anfontanelli 3:fc26045926d9 158
group-ST 0:6e8275981824 159 }
anfontanelli 3:fc26045926d9 160