Jeanne Le Soudeer
/
PSL-VicTeam
fusion leopold sasa
Diff: main.cpp
- Revision:
- 1:c5e9c436f8d0
- Parent:
- 0:8762773cddf9
- Child:
- 2:223d6363315b
diff -r 8762773cddf9 -r c5e9c436f8d0 main.cpp --- a/main.cpp Fri Mar 27 07:46:40 2020 +0000 +++ b/main.cpp Fri Apr 24 07:58:54 2020 +0000 @@ -16,9 +16,9 @@ int i = 0; char last_read; -int indice_a_modifie = 0; +int indice_a_modifie = 1; char message[50]; -char message_vide = 0; +char message_vide = '0'; float front, lat, rot, T_front, T_lat = 0; /*typedef struct { @@ -36,58 +36,126 @@ alivenessLED = !alivenessLED; }*/ -void setup(){ +void setup() +{ //eventQueue.call_every(500, blinkCallback); coach.baud(115200); myax12a.SetMode(1); myax12b.SetMode(1); myax12c.SetMode(1); - } - -void asser(float Vx, float Vy, float gammaz){ +} + +void asser(float Vx, float Vy, float gammaz) +{ float wa = rapport_vitesse*(Vx + 100 * gammaz); float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz); float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz); myax12a.SetCRSpeed(wa); myax12b.SetCRSpeed(wb); myax12c.SetCRSpeed(wc); - } +} -void send_stat(){ - coach.printf("%f,%f,%f,%f,%f \n", front, lat, rot, front, lat ); - } +void send_stat() +{ + coach.printf("S%f,%f,%f,%f,%f \n", front, lat, rot, T_front, T_lat ); +} -void mise_a_zero_msg(){ - for(i=0;i<50;i++){//On réinitialise la chaîne de caractères - message[i] = message_vide;} +void mise_a_zero_msg() +{ + for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères + message[i] = message_vide; + } indice_a_modifie = 0; - } - -void reception_com(){ +} + +void reception_com() +{ if (coach.readable()) { last_read = coach.getc(); + /* if (last_read == 'A') { - indice_a_modifie = 0; + wait(0.05); + indice_a_modifie = 1; last_read = coach.getc(); - while ((last_read != ';')){ + while ((last_read != ';')) { message[indice_a_modifie] = last_read; + indice_a_modifie ++; coach.printf("|%i|%s|", indice_a_modifie, message); - indice_a_modifie ++; - last_read = coach.getc();} + last_read = coach.getc(); + } message[indice_a_modifie] = last_read; sscanf(message,"%f,%f,%f,%f,%f;", &front, &lat, &rot, &front, &lat ); mise_a_zero_msg(); - } - if (last_read == 'S') { + }*/ + switch(last_read){ + case 'Z': + front = 0; + lat = 0; + rot = 0; + T_front = 0; + T_lat = 0; + break; + + case 'a': + front ++; + break; + + case 'b': + front --; + break; + + case 'A': + front = front + 10; + break; + + case 'B': + front = front - 10; + break; + + case 'c': + lat ++; + break; + + case 'd': + lat --; + break; + + case 'C': + lat = lat + 10; + break; + + case 'D': + lat = lat - 10; + break; + + case 'e': + rot ++; + break; + + case 'f': + rot --; + break; + + case 'E': + rot = rot + 10; + break; + + case 'F': + rot = rot - 10; + break; + + case 'S' : send_stat(); - } - }} + } + } +} -int main(){ +int main() +{ setup(); - while (true){ + while (true) { reception_com(); asser(front, lat, rot); - } + } - } \ No newline at end of file +} \ No newline at end of file