librairie pour robot Zumo
zumo.cpp@12:d9c9ef63c5ff, 2022-01-10 (annotated)
- Committer:
- bouaziz
- Date:
- Mon Jan 10 23:00:47 2022 +0000
- Revision:
- 12:d9c9ef63c5ff
- Parent:
- 11:1082c5b3b418
amelioration lib Zumo polytech paris saclay
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bouaziz | 10:7935bbc4ebf1 | 1 | /* zumo Library |
bouaziz | 10:7935bbc4ebf1 | 2 | */ |
chris | 0:e6020bd04b45 | 3 | |
chris | 0:e6020bd04b45 | 4 | #include "mbed.h" |
bouaziz | 10:7935bbc4ebf1 | 5 | #include "zumo.h" |
chris | 0:e6020bd04b45 | 6 | |
bouaziz | 10:7935bbc4ebf1 | 7 | zumo::zumo(PinName nrst, PinName tx, PinName rx) : _nrst(nrst), _ser(tx, rx) { |
bouaziz | 11:1082c5b3b418 | 8 | _ser.baud(57600); |
chris | 0:e6020bd04b45 | 9 | reset(); |
chris | 0:e6020bd04b45 | 10 | } |
chris | 0:e6020bd04b45 | 11 | |
bouaziz | 10:7935bbc4ebf1 | 12 | zumo::zumo(): _nrst(p23), _ser(p9, p10){ |
bouaziz | 11:1082c5b3b418 | 13 | _ser.baud(57600); |
bouaziz | 10:7935bbc4ebf1 | 14 | } |
bouaziz | 10:7935bbc4ebf1 | 15 | |
bouaziz | 10:7935bbc4ebf1 | 16 | |
bouaziz | 10:7935bbc4ebf1 | 17 | void zumo::reset () { |
chris | 7:9b128cebb3c2 | 18 | } |
chris | 7:9b128cebb3c2 | 19 | |
chris | 7:9b128cebb3c2 | 20 | |
bouaziz | 10:7935bbc4ebf1 | 21 | void zumo::stop (void) { |
bouaziz | 11:1082c5b3b418 | 22 | _ser.putc(SEND_MOTOR_STOP); |
bouaziz | 11:1082c5b3b418 | 23 | _ser.getc(); |
bouaziz | 11:1082c5b3b418 | 24 | |
chris | 0:e6020bd04b45 | 25 | } |
chris | 0:e6020bd04b45 | 26 | |
bouaziz | 10:7935bbc4ebf1 | 27 | char zumo::speed(short spg,short spd){ |
bouaziz | 11:1082c5b3b418 | 28 | char v; |
bouaziz | 11:1082c5b3b418 | 29 | _ser.putc(SEND_MOTOR_SPEED); |
bouaziz | 10:7935bbc4ebf1 | 30 | _ser.putc(spg&0xff); |
bouaziz | 10:7935bbc4ebf1 | 31 | _ser.putc((spg>>8)&0xff); |
bouaziz | 10:7935bbc4ebf1 | 32 | _ser.putc(spd&0xff); |
bouaziz | 10:7935bbc4ebf1 | 33 | _ser.putc((spd>>8)&0xff); |
bouaziz | 10:7935bbc4ebf1 | 34 | v=_ser.getc(); |
bouaziz | 10:7935bbc4ebf1 | 35 | return v; |
chris | 0:e6020bd04b45 | 36 | } |
chris | 0:e6020bd04b45 | 37 | |
bouaziz | 12:d9c9ef63c5ff | 38 | // angle envoyé au centime de deg rot et sens trigo |
bouaziz | 12:d9c9ef63c5ff | 39 | char zumo::rotate_speed_gyro_wait (short spd,short rot){ |
bouaziz | 12:d9c9ef63c5ff | 40 | char v; |
bouaziz | 12:d9c9ef63c5ff | 41 | _ser.putc(SEND_ROTATE_GYRO); |
bouaziz | 12:d9c9ef63c5ff | 42 | _ser.putc(spd&0xff); |
bouaziz | 12:d9c9ef63c5ff | 43 | _ser.putc((spd>>8)&0xff); |
bouaziz | 12:d9c9ef63c5ff | 44 | _ser.putc(rot&0xff); |
bouaziz | 12:d9c9ef63c5ff | 45 | _ser.putc((rot>>8)&0xff); |
bouaziz | 12:d9c9ef63c5ff | 46 | v=_ser.getc(); |
bouaziz | 12:d9c9ef63c5ff | 47 | v=_ser.getc(); |
bouaziz | 12:d9c9ef63c5ff | 48 | return v; |
bouaziz | 12:d9c9ef63c5ff | 49 | } |
bouaziz | 12:d9c9ef63c5ff | 50 | |
bouaziz | 12:d9c9ef63c5ff | 51 | char zumo::Calibrate_line_sensor_wait (){ |
bouaziz | 12:d9c9ef63c5ff | 52 | char v; |
bouaziz | 12:d9c9ef63c5ff | 53 | _ser.putc(SEND_CALIBRE_LIGNE); |
bouaziz | 12:d9c9ef63c5ff | 54 | v=_ser.getc(); //accusé commande prise |
bouaziz | 12:d9c9ef63c5ff | 55 | v=_ser.getc(); // accusé commande terminée |
bouaziz | 12:d9c9ef63c5ff | 56 | return v; |
bouaziz | 12:d9c9ef63c5ff | 57 | } |
bouaziz | 12:d9c9ef63c5ff | 58 | |
bouaziz | 12:d9c9ef63c5ff | 59 | |
bouaziz | 12:d9c9ef63c5ff | 60 | char zumo::read_data_wait (char whatdata,char*tab){ |
bouaziz | 12:d9c9ef63c5ff | 61 | char v; |
bouaziz | 12:d9c9ef63c5ff | 62 | _ser.putc(SEND_ROTATE_GYRO); |
bouaziz | 12:d9c9ef63c5ff | 63 | _ser.putc(whatdata&0xff); |
bouaziz | 12:d9c9ef63c5ff | 64 | v=_ser.getc(); |
bouaziz | 12:d9c9ef63c5ff | 65 | /*#define DATA_SENSOR_GYRO 0x12 |
bouaziz | 12:d9c9ef63c5ff | 66 | #define DATA_SENSOR_MEANDIST 0x24 |
bouaziz | 12:d9c9ef63c5ff | 67 | #define DATA_SENSOR_LRDIST 0x38 |
bouaziz | 12:d9c9ef63c5ff | 68 | #define DATA_SENSOR_LIGNE 0x42 |
bouaziz | 12:d9c9ef63c5ff | 69 | #define DATA_SENSOR_LIGNE_5BIT 0x51 |
bouaziz | 12:d9c9ef63c5ff | 70 | #define DATA_SENSOR_OBSTACLE 0x62*/ |
bouaziz | 12:d9c9ef63c5ff | 71 | v=v&0x0F; |
bouaziz | 12:d9c9ef63c5ff | 72 | if(v!=0){ |
bouaziz | 12:d9c9ef63c5ff | 73 | for(char i=0;i<v;i++){ |
bouaziz | 12:d9c9ef63c5ff | 74 | tab[i]= _ser.getc(); |
bouaziz | 12:d9c9ef63c5ff | 75 | } |
bouaziz | 12:d9c9ef63c5ff | 76 | return 0x0A; |
bouaziz | 12:d9c9ef63c5ff | 77 | // retour de donnée |
bouaziz | 12:d9c9ef63c5ff | 78 | }else{ |
bouaziz | 12:d9c9ef63c5ff | 79 | return 0x0E; |
bouaziz | 12:d9c9ef63c5ff | 80 | } |
bouaziz | 12:d9c9ef63c5ff | 81 | } |
bouaziz | 12:d9c9ef63c5ff | 82 | |
bouaziz | 12:d9c9ef63c5ff | 83 | |
bouaziz | 12:d9c9ef63c5ff | 84 | // avance tout droit de dist mm limite de 3000mm |
bouaziz | 12:d9c9ef63c5ff | 85 | // avec speed positif (car calcul sens par dist) |
bouaziz | 12:d9c9ef63c5ff | 86 | // angle envoyé au centime de deg rot et sens trigo |
bouaziz | 12:d9c9ef63c5ff | 87 | char zumo::avance_wait(short spd, short dist){ |
bouaziz | 12:d9c9ef63c5ff | 88 | char v; |
bouaziz | 12:d9c9ef63c5ff | 89 | if(dist<=3000) { |
bouaziz | 12:d9c9ef63c5ff | 90 | _ser.putc(SEND_AVANCE); |
bouaziz | 12:d9c9ef63c5ff | 91 | _ser.putc(spd&0xff); |
bouaziz | 12:d9c9ef63c5ff | 92 | _ser.putc((spd>>8)&0xff); |
bouaziz | 12:d9c9ef63c5ff | 93 | _ser.putc(dist&0xff); |
bouaziz | 12:d9c9ef63c5ff | 94 | _ser.putc((dist>>8)&0xff); |
bouaziz | 12:d9c9ef63c5ff | 95 | v=_ser.getc(); |
bouaziz | 12:d9c9ef63c5ff | 96 | v=_ser.getc(); // 0x0B bloquant jusqu'à fin trajectoire |
bouaziz | 12:d9c9ef63c5ff | 97 | return v; |
bouaziz | 12:d9c9ef63c5ff | 98 | }else{ |
bouaziz | 12:d9c9ef63c5ff | 99 | return (0x0E); |
bouaziz | 12:d9c9ef63c5ff | 100 | } |
bouaziz | 12:d9c9ef63c5ff | 101 | } |
bouaziz | 12:d9c9ef63c5ff | 102 | |
bouaziz | 10:7935bbc4ebf1 | 103 | float zumo::battery() { |
bouaziz | 10:7935bbc4ebf1 | 104 | |
chris | 0:e6020bd04b45 | 105 | } |
chris | 0:e6020bd04b45 | 106 | |
chris | 0:e6020bd04b45 | 107 | |
bouaziz | 10:7935bbc4ebf1 | 108 | float zumo::line_position() { |
bouaziz | 10:7935bbc4ebf1 | 109 | |
chris | 0:e6020bd04b45 | 110 | } |
chris | 0:e6020bd04b45 | 111 | |
bouaziz | 10:7935bbc4ebf1 | 112 | void zumo::calibrated_sensors(unsigned short ltab[5]) { |
bouaziz | 10:7935bbc4ebf1 | 113 | |
chris | 0:e6020bd04b45 | 114 | } |
chris | 0:e6020bd04b45 | 115 | |
chris | 5:09fb0636207b | 116 | |
bouaziz | 10:7935bbc4ebf1 | 117 | void zumo::PID_start(int max_speed, int a, int b, int c, int d) { |
bouaziz | 10:7935bbc4ebf1 | 118 | |
chris | 0:e6020bd04b45 | 119 | } |
chris | 0:e6020bd04b45 | 120 | |
bouaziz | 10:7935bbc4ebf1 | 121 | void zumo::PID_stop() { |
bouaziz | 10:7935bbc4ebf1 | 122 | |
chris | 0:e6020bd04b45 | 123 | } |
chris | 0:e6020bd04b45 | 124 | |
chris | 0:e6020bd04b45 | 125 | |
chris | 0:e6020bd04b45 | 126 | |
bouaziz | 10:7935bbc4ebf1 | 127 | void zumo::leds(int val) { |
chris | 0:e6020bd04b45 | 128 | |
bouaziz | 10:7935bbc4ebf1 | 129 | } |
chris | 0:e6020bd04b45 | 130 |