Leopold Clement / Mbed 2 deprecated PSL-VicTeam

faut que Val répare le truc

Dependencies:   mbed AX12

main.cpp@1:c5e9c436f8d0, 2020-04-24 (annotated)

Committer:
leocle
Date:
Fri Apr 24 07:58:54 2020 +0000
Revision:
1:c5e9c436f8d0
Parent:
0:8762773cddf9
Child:
2:223d6363315b
version avec valeur des caracteres

Who changed what in which revision?

UserRevisionLine numberNew contents of line
leocle 0:8762773cddf9 1
leocle 0:8762773cddf9 2 /* #include <mbed_events.h> */
leocle 0:8762773cddf9 3 #include "mbed.h"
leocle 0:8762773cddf9 4 #include "AX12.h"
leocle 0:8762773cddf9 5
leocle 0:8762773cddf9 6 DigitalOut alivenessLED(LED1);
leocle 0:8762773cddf9 7 DigitalOut testLed(LED2);
leocle 0:8762773cddf9 8
leocle 0:8762773cddf9 9 Serial coach(D1, D0);
leocle 0:8762773cddf9 10
leocle 0:8762773cddf9 11 const float rapport_vitesse = 0.019383809*0.026315;
leocle 0:8762773cddf9 12 const float root3 = 1.7321;
leocle 0:8762773cddf9 13 AX12 myax12a (PC_4, PC_5, 2);
leocle 0:8762773cddf9 14 AX12 myax12b (PC_4, PC_5, 3);
leocle 0:8762773cddf9 15 AX12 myax12c (PC_4, PC_5, 4);
leocle 0:8762773cddf9 16
leocle 0:8762773cddf9 17 int i = 0;
leocle 0:8762773cddf9 18 char last_read;
leocle 1:c5e9c436f8d0 19 int indice_a_modifie = 1;
leocle 0:8762773cddf9 20 char message[50];
leocle 1:c5e9c436f8d0 21 char message_vide = '0';
leocle 0:8762773cddf9 22 float front, lat, rot, T_front, T_lat = 0;
leocle 0:8762773cddf9 23
leocle 0:8762773cddf9 24 /*typedef struct {
leocle 0:8762773cddf9 25 float front;
leocle 0:8762773cddf9 26 float lat;
leocle 0:8762773cddf9 27 float rot ;
leocle 0:8762773cddf9 28 } dep;
leocle 0:8762773cddf9 29 typedef struct {
leocle 0:8762773cddf9 30 float front;
leocle 0:8762773cddf9 31 float lat;
leocle 0:8762773cddf9 32 } tire;*/
leocle 0:8762773cddf9 33 /*
leocle 0:8762773cddf9 34 void blinkCallback(void)
leocle 0:8762773cddf9 35 {
leocle 0:8762773cddf9 36 alivenessLED = !alivenessLED;
leocle 0:8762773cddf9 37 }*/
leocle 0:8762773cddf9 38
leocle 1:c5e9c436f8d0 39 void setup()
leocle 1:c5e9c436f8d0 40 {
leocle 0:8762773cddf9 41 //eventQueue.call_every(500, blinkCallback);
leocle 0:8762773cddf9 42 coach.baud(115200);
leocle 0:8762773cddf9 43 myax12a.SetMode(1);
leocle 0:8762773cddf9 44 myax12b.SetMode(1);
leocle 0:8762773cddf9 45 myax12c.SetMode(1);
leocle 1:c5e9c436f8d0 46 }
leocle 1:c5e9c436f8d0 47
leocle 1:c5e9c436f8d0 48 void asser(float Vx, float Vy, float gammaz)
leocle 1:c5e9c436f8d0 49 {
leocle 0:8762773cddf9 50 float wa = rapport_vitesse*(Vx + 100 * gammaz);
leocle 0:8762773cddf9 51 float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz);
leocle 0:8762773cddf9 52 float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz);
leocle 0:8762773cddf9 53 myax12a.SetCRSpeed(wa);
leocle 0:8762773cddf9 54 myax12b.SetCRSpeed(wb);
leocle 0:8762773cddf9 55 myax12c.SetCRSpeed(wc);
leocle 1:c5e9c436f8d0 56 }
leocle 0:8762773cddf9 57
leocle 1:c5e9c436f8d0 58 void send_stat()
leocle 1:c5e9c436f8d0 59 {
leocle 1:c5e9c436f8d0 60 coach.printf("S%f,%f,%f,%f,%f \n", front, lat, rot, T_front, T_lat );
leocle 1:c5e9c436f8d0 61 }
leocle 0:8762773cddf9 62
leocle 1:c5e9c436f8d0 63 void mise_a_zero_msg()
leocle 1:c5e9c436f8d0 64 {
leocle 1:c5e9c436f8d0 65 for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
leocle 1:c5e9c436f8d0 66 message[i] = message_vide;
leocle 1:c5e9c436f8d0 67 }
leocle 0:8762773cddf9 68 indice_a_modifie = 0;
leocle 1:c5e9c436f8d0 69 }
leocle 1:c5e9c436f8d0 70
leocle 1:c5e9c436f8d0 71 void reception_com()
leocle 1:c5e9c436f8d0 72 {
leocle 0:8762773cddf9 73 if (coach.readable()) {
leocle 0:8762773cddf9 74 last_read = coach.getc();
leocle 1:c5e9c436f8d0 75 /*
leocle 0:8762773cddf9 76 if (last_read == 'A') {
leocle 1:c5e9c436f8d0 77 wait(0.05);
leocle 1:c5e9c436f8d0 78 indice_a_modifie = 1;
leocle 0:8762773cddf9 79 last_read = coach.getc();
leocle 1:c5e9c436f8d0 80 while ((last_read != ';')) {
leocle 0:8762773cddf9 81 message[indice_a_modifie] = last_read;
leocle 1:c5e9c436f8d0 82 indice_a_modifie ++;
leocle 0:8762773cddf9 83 coach.printf("|%i|%s|", indice_a_modifie, message);
leocle 1:c5e9c436f8d0 84 last_read = coach.getc();
leocle 1:c5e9c436f8d0 85 }
leocle 0:8762773cddf9 86 message[indice_a_modifie] = last_read;
leocle 0:8762773cddf9 87 sscanf(message,"%f,%f,%f,%f,%f;", &front, &lat, &rot, &front, &lat );
leocle 0:8762773cddf9 88 mise_a_zero_msg();
leocle 1:c5e9c436f8d0 89 }*/
leocle 1:c5e9c436f8d0 90 switch(last_read){
leocle 1:c5e9c436f8d0 91 case 'Z':
leocle 1:c5e9c436f8d0 92 front = 0;
leocle 1:c5e9c436f8d0 93 lat = 0;
leocle 1:c5e9c436f8d0 94 rot = 0;
leocle 1:c5e9c436f8d0 95 T_front = 0;
leocle 1:c5e9c436f8d0 96 T_lat = 0;
leocle 1:c5e9c436f8d0 97 break;
leocle 1:c5e9c436f8d0 98
leocle 1:c5e9c436f8d0 99 case 'a':
leocle 1:c5e9c436f8d0 100 front ++;
leocle 1:c5e9c436f8d0 101 break;
leocle 1:c5e9c436f8d0 102
leocle 1:c5e9c436f8d0 103 case 'b':
leocle 1:c5e9c436f8d0 104 front --;
leocle 1:c5e9c436f8d0 105 break;
leocle 1:c5e9c436f8d0 106
leocle 1:c5e9c436f8d0 107 case 'A':
leocle 1:c5e9c436f8d0 108 front = front + 10;
leocle 1:c5e9c436f8d0 109 break;
leocle 1:c5e9c436f8d0 110
leocle 1:c5e9c436f8d0 111 case 'B':
leocle 1:c5e9c436f8d0 112 front = front - 10;
leocle 1:c5e9c436f8d0 113 break;
leocle 1:c5e9c436f8d0 114
leocle 1:c5e9c436f8d0 115 case 'c':
leocle 1:c5e9c436f8d0 116 lat ++;
leocle 1:c5e9c436f8d0 117 break;
leocle 1:c5e9c436f8d0 118
leocle 1:c5e9c436f8d0 119 case 'd':
leocle 1:c5e9c436f8d0 120 lat --;
leocle 1:c5e9c436f8d0 121 break;
leocle 1:c5e9c436f8d0 122
leocle 1:c5e9c436f8d0 123 case 'C':
leocle 1:c5e9c436f8d0 124 lat = lat + 10;
leocle 1:c5e9c436f8d0 125 break;
leocle 1:c5e9c436f8d0 126
leocle 1:c5e9c436f8d0 127 case 'D':
leocle 1:c5e9c436f8d0 128 lat = lat - 10;
leocle 1:c5e9c436f8d0 129 break;
leocle 1:c5e9c436f8d0 130
leocle 1:c5e9c436f8d0 131 case 'e':
leocle 1:c5e9c436f8d0 132 rot ++;
leocle 1:c5e9c436f8d0 133 break;
leocle 1:c5e9c436f8d0 134
leocle 1:c5e9c436f8d0 135 case 'f':
leocle 1:c5e9c436f8d0 136 rot --;
leocle 1:c5e9c436f8d0 137 break;
leocle 1:c5e9c436f8d0 138
leocle 1:c5e9c436f8d0 139 case 'E':
leocle 1:c5e9c436f8d0 140 rot = rot + 10;
leocle 1:c5e9c436f8d0 141 break;
leocle 1:c5e9c436f8d0 142
leocle 1:c5e9c436f8d0 143 case 'F':
leocle 1:c5e9c436f8d0 144 rot = rot - 10;
leocle 1:c5e9c436f8d0 145 break;
leocle 1:c5e9c436f8d0 146
leocle 1:c5e9c436f8d0 147 case 'S' :
leocle 0:8762773cddf9 148 send_stat();
leocle 1:c5e9c436f8d0 149 }
leocle 1:c5e9c436f8d0 150 }
leocle 1:c5e9c436f8d0 151 }
leocle 0:8762773cddf9 152
leocle 1:c5e9c436f8d0 153 int main()
leocle 1:c5e9c436f8d0 154 {
leocle 0:8762773cddf9 155 setup();
leocle 1:c5e9c436f8d0 156 while (true) {
leocle 0:8762773cddf9 157 reception_com();
leocle 0:8762773cddf9 158 asser(front, lat, rot);
leocle 1:c5e9c436f8d0 159 }
leocle 0:8762773cddf9 160
leocle 1:c5e9c436f8d0 161 }