PRISMA Lab
Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of
Hyfliers_Completo_testato
by 
Marco De Silva
main.cpp@3:fc26045926d9, 2019-11-06 (annotated)
- Committer:
- anfontanelli
- Date:
- Wed Nov 06 10:57:51 2019 +0000
- Revision:
- 3:fc26045926d9
- Parent:
- 1:ec61ea9f67de
- Child:
- 4:3f22193053d0
Who changed what in which revision?
| User | Revision | Line number | New 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 | 3:fc26045926d9 | 9 | #define DEBUG false |
| 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 | 3:fc26045926d9 | 37 | float vel_dx = 0.0; |
| anfontanelli | 3:fc26045926d9 | 38 | float vel_sx = 0.0; |
| anfontanelli | 3:fc26045926d9 | 39 | float vel_r = 0.0; |
| anfontanelli | 3:fc26045926d9 | 40 | float forward_vel = 0.0; |
| anfontanelli | 3:fc26045926d9 | 41 | float pitch; |
| anfontanelli | 3:fc26045926d9 | 42 | |
| anfontanelli | 3:fc26045926d9 | 43 | float jointFront_d = 0.0; |
| anfontanelli | 3:fc26045926d9 | 44 | float jointRetro_d = 0.0; |
| anfontanelli | 3:fc26045926d9 | 45 | float time = 0.0; |
| anfontanelli | 3:fc26045926d9 | 46 | |
| anfontanelli | 3:fc26045926d9 | 47 | |
| anfontanelli | 3:fc26045926d9 | 48 | float roll; |
| anfontanelli | 3:fc26045926d9 | 49 | printf("Creo il rover \r\n"); |
| anfontanelli | 3:fc26045926d9 | 50 | Rover rover; |
| anfontanelli | 3:fc26045926d9 | 51 | |
| anfontanelli | 3:fc26045926d9 | 52 | |
| anfontanelli | 3:fc26045926d9 | 53 | rover.initializeImu(); |
| anfontanelli | 3:fc26045926d9 | 54 | rover.initializeTofs(); |
| group-ST | 0:6e8275981824 | 55 | |
| anfontanelli | 3:fc26045926d9 | 56 | |
| anfontanelli | 3:fc26045926d9 | 57 | Timer schedulerTimer; |
| anfontanelli | 3:fc26045926d9 | 58 | |
| anfontanelli | 3:fc26045926d9 | 59 | schedulerTimer.start(); |
| anfontanelli | 3:fc26045926d9 | 60 | |
| anfontanelli | 3:fc26045926d9 | 61 | rover.setCentralJointsAngle(0.0,0.0); //in radiants (front, retro) |
| anfontanelli | 3:fc26045926d9 | 62 | rover.setWheelsVelocity(0.0,0.0 , 0, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration |
| anfontanelli | 3:fc26045926d9 | 63 | |
| anfontanelli | 3:fc26045926d9 | 64 | rover.startEthComunication(); |
| anfontanelli | 3:fc26045926d9 | 65 | |
| anfontanelli | 3:fc26045926d9 | 66 | wait(1); |
| anfontanelli | 3:fc26045926d9 | 67 | |
| anfontanelli | 3:fc26045926d9 | 68 | double timeStabPrec = 0.0; |
| anfontanelli | 3:fc26045926d9 | 69 | double timeReadPrec = 0.0; |
| anfontanelli | 3:fc26045926d9 | 70 | double timePrintPrec = 0.0; |
| anfontanelli | 3:fc26045926d9 | 71 | |
| anfontanelli | 3:fc26045926d9 | 72 | timeStabPrec = schedulerTimer.read(); |
| anfontanelli | 3:fc26045926d9 | 73 | timePrintPrec = schedulerTimer.read(); |
| anfontanelli | 3:fc26045926d9 | 74 | timeReadPrec = schedulerTimer.read(); |
| group-ST | 0:6e8275981824 | 75 | |
| anfontanelli | 3:fc26045926d9 | 76 | float dtStab = 5.0/1000; |
| anfontanelli | 3:fc26045926d9 | 77 | float dtRead = 20.0/1000; |
| anfontanelli | 3:fc26045926d9 | 78 | float dtPrint = 500.0/1000; |
| anfontanelli | 3:fc26045926d9 | 79 | float dtPassedStab = 0.0; |
| anfontanelli | 3:fc26045926d9 | 80 | float Kp=1.2; //0.6 |
| group-ST | 0:6e8275981824 | 81 | |
| anfontanelli | 3:fc26045926d9 | 82 | float pitch_d = 0.0; |
| group-ST | 0:6e8275981824 | 83 | |
| anfontanelli | 3:fc26045926d9 | 84 | float frontDistance; |
| anfontanelli | 3:fc26045926d9 | 85 | float retroDistance; |
| anfontanelli | 3:fc26045926d9 | 86 | |
| anfontanelli | 3:fc26045926d9 | 87 | while (1){ |
| anfontanelli | 3:fc26045926d9 | 88 | |
| anfontanelli | 3:fc26045926d9 | 89 | time = schedulerTimer.read(); |
| anfontanelli | 3:fc26045926d9 | 90 | |
| anfontanelli | 3:fc26045926d9 | 91 | |
| anfontanelli | 3:fc26045926d9 | 92 | if(time - timeStabPrec > dtStab){ |
| anfontanelli | 3:fc26045926d9 | 93 | |
| anfontanelli | 3:fc26045926d9 | 94 | //rover.acquireTofs(frontDistance, retroDistance); |
| anfontanelli | 3:fc26045926d9 | 95 | |
| anfontanelli | 3:fc26045926d9 | 96 | rover.computeCentralJointsFromTofs(); |
| anfontanelli | 3:fc26045926d9 | 97 | dtPassedStab = time - timeStabPrec; |
| anfontanelli | 3:fc26045926d9 | 98 | timeStabPrec = time; |
| anfontanelli | 3:fc26045926d9 | 99 | |
| anfontanelli | 3:fc26045926d9 | 100 | rover.calcImuAngles(pitch, roll, dtStab); |
| anfontanelli | 3:fc26045926d9 | 101 | |
| anfontanelli | 3:fc26045926d9 | 102 | velf_a = forward_vel; |
| anfontanelli | 3:fc26045926d9 | 103 | vels_a = -Kp*M_PI*(pitch_d-pitch)/180; |
| anfontanelli | 3:fc26045926d9 | 104 | vela_a = 0.0; |
| anfontanelli | 3:fc26045926d9 | 105 | |
| anfontanelli | 3:fc26045926d9 | 106 | rover.setWheelsVelocity(velf_a, vels_a , vela_a, 0.125, 10000); //velForward, velStabilization, velAsset, pipeRadious, acceleration |
| anfontanelli | 3:fc26045926d9 | 107 | //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125); |
| anfontanelli | 3:fc26045926d9 | 108 | |
| anfontanelli | 3:fc26045926d9 | 109 | forward_vel = (float)rover.get_forward_vel()/1000; |
| anfontanelli | 3:fc26045926d9 | 110 | pitch_d = (float)rover.get_pitch()/1000; |
| anfontanelli | 3:fc26045926d9 | 111 | |
| anfontanelli | 3:fc26045926d9 | 112 | jointFront_d = (float)rover.get_jointFront()/1000; |
| anfontanelli | 3:fc26045926d9 | 113 | jointRetro_d = (float)rover.get_jointRetro()/1000; |
| anfontanelli | 3:fc26045926d9 | 114 | |
| anfontanelli | 3:fc26045926d9 | 115 | //rover.setCentralJointsAngle(jointFront_d,jointRetro_d); //in radiants (front, retro) |
| anfontanelli | 3:fc26045926d9 | 116 | |
| anfontanelli | 3:fc26045926d9 | 117 | |
| anfontanelli | 3:fc26045926d9 | 118 | } |
| anfontanelli | 3:fc26045926d9 | 119 | |
| anfontanelli | 3:fc26045926d9 | 120 | if(READ && time - timeReadPrec > dtRead){ |
| anfontanelli | 3:fc26045926d9 | 121 | |
| anfontanelli | 3:fc26045926d9 | 122 | timeReadPrec = time; |
| anfontanelli | 3:fc26045926d9 | 123 | //rover.getRoverVelocity(velf_m, vels_m, vela_m, 0.125); |
| anfontanelli | 3:fc26045926d9 | 124 | rover.getRoverWheelsVelocity(vel_dx, vel_sx, vel_r); |
| anfontanelli | 3:fc26045926d9 | 125 | |
| anfontanelli | 3:fc26045926d9 | 126 | |
| anfontanelli | 3:fc26045926d9 | 127 | } |
| anfontanelli | 3:fc26045926d9 | 128 | |
| anfontanelli | 3:fc26045926d9 | 129 | |
| anfontanelli | 3:fc26045926d9 | 130 | if(DEBUG && time - timePrintPrec > dtPrint){ |
| anfontanelli | 3:fc26045926d9 | 131 | timePrintPrec = time; |
| anfontanelli | 3:fc26045926d9 | 132 | |
| anfontanelli | 3:fc26045926d9 | 133 | //printf("Ts: %4.2f \t M2-M1 : %2.1f \r\n",dtPassedStab*1000,frontDistance); |
| anfontanelli | 3:fc26045926d9 | 134 | |
| anfontanelli | 3:fc26045926d9 | 135 | //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 | 136 | |
| anfontanelli | 3:fc26045926d9 | 137 | if(rover.getEthState() == 1){ |
| anfontanelli | 3:fc26045926d9 | 138 | myled1 = true; |
| anfontanelli | 3:fc26045926d9 | 139 | myled2 = false; |
| anfontanelli | 3:fc26045926d9 | 140 | }else if(rover.getEthState() == 2){ |
| anfontanelli | 3:fc26045926d9 | 141 | myled1 = true; |
| anfontanelli | 3:fc26045926d9 | 142 | myled2 = true; |
| anfontanelli | 3:fc26045926d9 | 143 | }else{ |
| anfontanelli | 3:fc26045926d9 | 144 | myled1 = false; |
| anfontanelli | 3:fc26045926d9 | 145 | myled2 = false; |
| anfontanelli | 3:fc26045926d9 | 146 | } |
| anfontanelli | 3:fc26045926d9 | 147 | |
| anfontanelli | 3:fc26045926d9 | 148 | |
| anfontanelli | 3:fc26045926d9 | 149 | |
| anfontanelli | 3:fc26045926d9 | 150 | } |
| anfontanelli | 3:fc26045926d9 | 151 | rover.ethComunicationUpdate(schedulerTimer.read(), pitch, frontDistance, retroDistance, dtPassedStab*1000, vel_r); |
| anfontanelli | 3:fc26045926d9 | 152 | //rover.ethComunicationUpdate(schedulerTimer.read(), pitch, velf_m, vels_m, dtPassedStab*1000, vel_r); |
| anfontanelli | 3:fc26045926d9 | 153 | |
| anfontanelli | 3:fc26045926d9 | 154 | |
| anfontanelli | 3:fc26045926d9 | 155 | |
| anfontanelli | 3:fc26045926d9 | 156 | } |
| anfontanelli | 3:fc26045926d9 | 157 | |
| group-ST | 0:6e8275981824 | 158 | } |
| anfontanelli | 3:fc26045926d9 | 159 |