librairie pour robot Zumo
Revision 12:d9c9ef63c5ff, committed 2022-01-10
- Comitter:
- bouaziz
- Date:
- Mon Jan 10 23:00:47 2022 +0000
- Parent:
- 11:1082c5b3b418
- Commit message:
- amelioration lib Zumo polytech paris saclay
Changed in this revision
--- a/readme.txt Mon Jan 10 08:59:37 2022 +0000 +++ b/readme.txt Mon Jan 10 23:00:47 2022 +0000 @@ -9,3 +9,21 @@ speed(g,d) fonction permettant d'envoyer une vitesse au robot vers le 32U4 + + +SEND_ROTATE_GYRO +rotate_speed_gyro_wait + envoi 2octets vitesse (absolu) + de 2 octets angle au centieme de Deg + attente reponse 0x0A que data prise en compte + puis attendre 0x0B que la rotation s'est terminée + +SEND_AVANCE +char zumo::avance_wait (short dist) +avec speed 2 octets donné absolu car sign de dist définit le sens du deplacement +dist sur 2 octets signés en mm de déplacement + + +SEND_CALIBRE_LIGNE 0x60 +Calibrer les capteurs sur la ligne +Attendre le retour de 0x0A puis 0x0B que le calibrage est terminé. +
--- a/zumo.cpp Mon Jan 10 08:59:37 2022 +0000 +++ b/zumo.cpp Mon Jan 10 23:00:47 2022 +0000 @@ -35,6 +35,71 @@ return v; } +// angle envoyé au centime de deg rot et sens trigo +char zumo::rotate_speed_gyro_wait (short spd,short rot){ + char v; + _ser.putc(SEND_ROTATE_GYRO); + _ser.putc(spd&0xff); + _ser.putc((spd>>8)&0xff); + _ser.putc(rot&0xff); + _ser.putc((rot>>8)&0xff); + v=_ser.getc(); + v=_ser.getc(); + return v; +} + +char zumo::Calibrate_line_sensor_wait (){ + char v; + _ser.putc(SEND_CALIBRE_LIGNE); + v=_ser.getc(); //accusé commande prise + v=_ser.getc(); // accusé commande terminée + return v; +} + + +char zumo::read_data_wait (char whatdata,char*tab){ + char v; + _ser.putc(SEND_ROTATE_GYRO); + _ser.putc(whatdata&0xff); + v=_ser.getc(); +/*#define DATA_SENSOR_GYRO 0x12 +#define DATA_SENSOR_MEANDIST 0x24 +#define DATA_SENSOR_LRDIST 0x38 +#define DATA_SENSOR_LIGNE 0x42 +#define DATA_SENSOR_LIGNE_5BIT 0x51 +#define DATA_SENSOR_OBSTACLE 0x62*/ + v=v&0x0F; + if(v!=0){ + for(char i=0;i<v;i++){ + tab[i]= _ser.getc(); + } + return 0x0A; + // retour de donnée + }else{ + return 0x0E; + } +} + + +// avance tout droit de dist mm limite de 3000mm + // avec speed positif (car calcul sens par dist) + // angle envoyé au centime de deg rot et sens trigo +char zumo::avance_wait(short spd, short dist){ + char v; + if(dist<=3000) { + _ser.putc(SEND_AVANCE); + _ser.putc(spd&0xff); + _ser.putc((spd>>8)&0xff); + _ser.putc(dist&0xff); + _ser.putc((dist>>8)&0xff); + v=_ser.getc(); + v=_ser.getc(); // 0x0B bloquant jusqu'à fin trajectoire + return v; + }else{ + return (0x0E); + } +} + float zumo::battery() { } @@ -48,10 +113,6 @@ } -char zumo::sensor_auto_calibrate() { - -} - void zumo::PID_start(int max_speed, int a, int b, int c, int d) {
--- a/zumo.h Mon Jan 10 08:59:37 2022 +0000 +++ b/zumo.h Mon Jan 10 23:00:47 2022 +0000 @@ -26,9 +26,23 @@ #include "mbed.h" #include "platform.h" - +// commandes pour le ZUMO #define SEND_MOTOR_SPEED 0xA4 #define SEND_MOTOR_STOP 0xA0 +#define SEND_ROTATE_GYRO 0x94 +#define SEND_AVANCE 0x84 +#define SEND_CALIBRE_LIGNE 0x60 +#define READ_SENSOR 0x71 + + + +// sous commande de la commande READ_SENSOR +#define DATA_SENSOR_GYRO 0x12 +#define DATA_SENSOR_MEANDIST 0x24 +#define DATA_SENSOR_LRDIST 0x38 +#define DATA_SENSOR_LIGNE 0x42 +#define DATA_SENSOR_LIGNE_5BIT 0x51 +#define DATA_SENSOR_OBSTACLE 0x62 @@ -135,11 +149,22 @@ * */ char speed(short spg,short spd); - - /** Calibrate the sensors. This turns the robot left then right, looking for a line - * + + // speed vitesse absolue + // rot exprim au 1/100eme de deg 45.01 sera 4501 + char rotate_speed_gyro_wait (short spd,short rot); + + // avance tout droit de dist mm + // avec speed positif (car calcul sens par dist) + // angle envoyé au centime de deg rot et sens trigo + char avance_wait(short spd, short dist); + + /** Calibrate the sensors. */ - char sensor_auto_calibrate (void); + char Calibrate_line_sensor_wait (); + + char read_data_wait (char whatdata,char*tab); + void PID_start(int max_speed, int a, int b, int c, int d);