zumo.cpp

00001 /* zumo Library
00002 */
00003 
00004 #include "mbed.h"
00005 #include "zumo.h"
00006 
00007 zumo::zumo(PinName nrst, PinName tx, PinName rx) : _nrst(nrst), _ser(tx, rx)  {
00008     _ser.baud(57600);
00009     reset();
00010 }
00011 
00012 zumo::zumo():  _nrst(p23), _ser(p9, p10){
00013         _ser.baud(57600);
00014     }   
00015 
00016 
00017 void zumo::reset () {
00018 }
00019 
00020 
00021 void zumo::stop (void) {
00022     _ser.putc(SEND_MOTOR_STOP);
00023     _ser.getc();
00024     
00025 }
00026 
00027 char zumo::speed(short spg,short spd){
00028     char v;
00029     _ser.putc(SEND_MOTOR_SPEED);
00030     _ser.putc(spg&0xff);
00031     _ser.putc((spg>>8)&0xff);
00032     _ser.putc(spd&0xff);
00033     _ser.putc((spd>>8)&0xff);
00034     v=_ser.getc();
00035     return v;
00036 }
00037 
00038 // angle envoyé au centime de deg  rot  et sens trigo
00039 char zumo::rotate_speed_gyro_wait (short spd,short rot){
00040     char v;
00041     _ser.putc(SEND_ROTATE_GYRO);
00042     _ser.putc(spd&0xff);
00043     _ser.putc((spd>>8)&0xff);
00044     _ser.putc(rot&0xff);
00045     _ser.putc((rot>>8)&0xff);
00046     v=_ser.getc();
00047     v=_ser.getc();
00048     return v;
00049 }
00050 
00051 char zumo::Calibrate_line_sensor_wait (){
00052     char v;
00053     _ser.putc(SEND_CALIBRE_LIGNE);
00054     v=_ser.getc(); //accusé commande prise
00055     v=_ser.getc(); // accusé commande terminée
00056     return v;
00057 }
00058 
00059 
00060 char zumo::read_data_wait (char whatdata,char*tab){
00061     char v;
00062     _ser.putc(SEND_ROTATE_GYRO);
00063     _ser.putc(whatdata&0xff);
00064     v=_ser.getc();
00065 /*#define DATA_SENSOR_GYRO    0x12
00066 #define DATA_SENSOR_MEANDIST  0x24
00067 #define DATA_SENSOR_LRDIST   0x38
00068 #define DATA_SENSOR_LIGNE 0x42
00069 #define DATA_SENSOR_LIGNE_5BIT 0x51
00070 #define DATA_SENSOR_OBSTACLE 0x62*/
00071     v=v&0x0F;
00072     if(v!=0){
00073         for(char i=0;i<v;i++){
00074                 tab[i]= _ser.getc();
00075         }
00076         return 0x0A;
00077     // retour de donnée
00078     }else{
00079         return 0x0E;
00080     }   
00081 }
00082 
00083 
00084 // avance tout droit de dist mm  limite de 3000mm
00085  // avec speed positif (car calcul sens par dist)
00086  // angle envoyé au centime de deg  rot  et sens trigo
00087 char zumo::avance_wait(short spd, short dist){
00088     char v;
00089     if(dist<=3000) {
00090             _ser.putc(SEND_AVANCE);
00091             _ser.putc(spd&0xff);
00092             _ser.putc((spd>>8)&0xff);
00093             _ser.putc(dist&0xff);
00094             _ser.putc((dist>>8)&0xff);
00095             v=_ser.getc();
00096             v=_ser.getc(); // 0x0B  bloquant jusqu'à fin trajectoire
00097             return v;
00098     }else{
00099         return (0x0E);
00100     }
00101 }
00102 
00103 float zumo::battery() {
00104    
00105 }
00106 
00107 
00108 float zumo::line_position() {
00109     
00110 }
00111 
00112 void zumo::calibrated_sensors(unsigned short ltab[5]) {
00113     
00114 }
00115 
00116 
00117 void zumo::PID_start(int max_speed, int a, int b, int c, int d) {
00118     
00119 }
00120 
00121 void zumo::PID_stop() {
00122     
00123 }
00124 
00125 
00126 
00127 void zumo::leds(int val) {
00128 
00129 }
00130