Leopold Clement / Mbed 2 deprecated PSL-VicTeam

faut que Val répare le truc

Dependencies:   mbed AX12

main.cpp@0:8762773cddf9, 2020-03-27 (annotated)

Committer:
leocle
Date:
Fri Mar 27 07:46:40 2020 +0000
Revision:
0:8762773cddf9
Child:
1:c5e9c436f8d0
ne li pas les commandes;

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 0:8762773cddf9 19 int indice_a_modifie = 0;
leocle 0:8762773cddf9 20 char message[50];
leocle 0:8762773cddf9 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 0:8762773cddf9 39 void setup(){
leocle 0:8762773cddf9 40 //eventQueue.call_every(500, blinkCallback);
leocle 0:8762773cddf9 41 coach.baud(115200);
leocle 0:8762773cddf9 42 myax12a.SetMode(1);
leocle 0:8762773cddf9 43 myax12b.SetMode(1);
leocle 0:8762773cddf9 44 myax12c.SetMode(1);
leocle 0:8762773cddf9 45 }
leocle 0:8762773cddf9 46
leocle 0:8762773cddf9 47 void asser(float Vx, float Vy, float gammaz){
leocle 0:8762773cddf9 48 float wa = rapport_vitesse*(Vx + 100 * gammaz);
leocle 0:8762773cddf9 49 float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz);
leocle 0:8762773cddf9 50 float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz);
leocle 0:8762773cddf9 51 myax12a.SetCRSpeed(wa);
leocle 0:8762773cddf9 52 myax12b.SetCRSpeed(wb);
leocle 0:8762773cddf9 53 myax12c.SetCRSpeed(wc);
leocle 0:8762773cddf9 54 }
leocle 0:8762773cddf9 55
leocle 0:8762773cddf9 56 void send_stat(){
leocle 0:8762773cddf9 57 coach.printf("%f,%f,%f,%f,%f \n", front, lat, rot, front, lat );
leocle 0:8762773cddf9 58 }
leocle 0:8762773cddf9 59
leocle 0:8762773cddf9 60 void mise_a_zero_msg(){
leocle 0:8762773cddf9 61 for(i=0;i<50;i++){//On réinitialise la chaîne de caractères
leocle 0:8762773cddf9 62 message[i] = message_vide;}
leocle 0:8762773cddf9 63 indice_a_modifie = 0;
leocle 0:8762773cddf9 64 }
leocle 0:8762773cddf9 65
leocle 0:8762773cddf9 66 void reception_com(){
leocle 0:8762773cddf9 67 if (coach.readable()) {
leocle 0:8762773cddf9 68 last_read = coach.getc();
leocle 0:8762773cddf9 69 if (last_read == 'A') {
leocle 0:8762773cddf9 70 indice_a_modifie = 0;
leocle 0:8762773cddf9 71 last_read = coach.getc();
leocle 0:8762773cddf9 72 while ((last_read != ';')){
leocle 0:8762773cddf9 73 message[indice_a_modifie] = last_read;
leocle 0:8762773cddf9 74 coach.printf("|%i|%s|", indice_a_modifie, message);
leocle 0:8762773cddf9 75 indice_a_modifie ++;
leocle 0:8762773cddf9 76 last_read = coach.getc();}
leocle 0:8762773cddf9 77 message[indice_a_modifie] = last_read;
leocle 0:8762773cddf9 78 sscanf(message,"%f,%f,%f,%f,%f;", &front, &lat, &rot, &front, &lat );
leocle 0:8762773cddf9 79 mise_a_zero_msg();
leocle 0:8762773cddf9 80 }
leocle 0:8762773cddf9 81 if (last_read == 'S') {
leocle 0:8762773cddf9 82 send_stat();
leocle 0:8762773cddf9 83 }
leocle 0:8762773cddf9 84 }}
leocle 0:8762773cddf9 85
leocle 0:8762773cddf9 86 int main(){
leocle 0:8762773cddf9 87 setup();
leocle 0:8762773cddf9 88 while (true){
leocle 0:8762773cddf9 89 reception_com();
leocle 0:8762773cddf9 90 asser(front, lat, rot);
leocle 0:8762773cddf9 91 }
leocle 0:8762773cddf9 92
leocle 0:8762773cddf9 93 }