Diff: m3pi.cpp

Revision:

10:c3617fcf2c58

Parent:

9:074ce6197b51

--- a/m3pi.cpp	Mon Mar 19 13:06:40 2018 +0000
+++ b/m3pi.cpp	Thu Nov 12 22:37:20 2020 +0000
@@ -5,17 +5,17 @@
 
 m3pi::m3pi()
 {
-    _serial = new Serial(p9,p10);
+    _serial = new BufferedSerial(p9,p10);
     _reset = new DigitalOut(p8);
     _last_line_position = 0.0;
-    
+
     // initialise the arrays
     _bar_graph[0] = ' ';
     for (int i = 0; i < 6; i++) {
-        _bar_graph[i+1] = i;    
+        _bar_graph[i+1] = i;
     }
     for (int i = 0; i < 5; i++) {
-       _values[i]=0;  
+        _values[i]=0;
     }
 }
 
@@ -30,7 +30,7 @@
 
 void m3pi::init()
 {
-    _serial->baud(115200);
+    _serial->set_baud(115200);
     reset();                        // hard rest of 3pi
     stop();                         // stop motors
     lcd_clear();                    // clear LCD
@@ -38,28 +38,41 @@
 
 /////////////////////////////// serial slave commands ////////////////////////////////
 
-void m3pi::scan() 
+void m3pi::scan()
 {
     get_calibrated_values(_values);
 }
 
 void m3pi::get_signature(char *signature)
 {
-    _serial->putc(0x81);
-    _serial->gets(signature,7);
+    char buff[7];
+    //_serial->putc(0x81);
+    buff[1]=0x81;
+    _serial->write(buff,1);
+    //_serial->gets(signature,7);
+    _serial->read(buff,7);
 }
 
 void m3pi::get_raw_values(unsigned int *values)
 {
+        char buff[1];                       // send command
+        
     while (_serial->readable() ) {  // flush buffer
-     _serial->getc();   
+        //_serial->getc();
+        _serial->sync();
+        
     }
-    
+
     char vals[10];                     // array to receive 10 byte return message
-    _serial->putc(0x86);                // send command
+
+    //_serial->putc(0x86);
+    buff[0]=0x86;
+    _serial->write(buff,1);
 
     for (int i=0; i < 10; i++) {
-        vals[i] =  _serial->getc();   
+        //vals[i] =  _serial->getc();
+        _serial->read(buff,1);
+        vals[i] = buff[0];
     }
 
     for(int i=0; i<5; i++) {            // construct the 2-byte values
@@ -69,28 +82,44 @@
 
 void m3pi::get_calibrated_values(unsigned int *values)
 {
+    char buff[1];
+
     while (_serial->readable() ) {  // flush buffer
-     _serial->getc();   
+        //_serial->getc();
+        _serial->sync();
     }
-    
+
     char vals[10];                      // array to receive 10 byte return message
-    _serial->putc(0x87);                // send command
-        
+    //_serial->putc(0x87);                // send command
+    buff[0]=0x87;
+    _serial->write(buff,1);
+
     for (int i=0; i < 10; i++) {
-        vals[i] =  _serial->getc();   
+        //vals[i] =  _serial->getc();
+        _serial->read(buff,1);
+        vals[i] = buff[0];
     }
-    
+
     for(int i=0; i<5; i++) {            // construct the 2-byte values
         values[i] = (vals[2*i+1] << 8) | vals[2*i];
     }
-    
+
 }
 
 float m3pi::get_trimpot_value()
 {
-    _serial->putc(0xB0);
-    char lsb = _serial->getc();
-    char msb = _serial->getc();
+    char buff[1];
+
+    //_serial->putc(0xB0);
+    buff[0]=0xB0;
+    _serial->write(buff,1);
+
+    char lsb;// = _serial->getc();
+    _serial->read(buff,1);
+    lsb = buff[0];
+    char msb;// = _serial->getc();
+        _serial->read(buff,1);
+    msb = buff[0];
     // trimpot value in the range 0 - 1023
     float value = ( msb<<8 | lsb ) / 1023.0;
     return value;
@@ -99,9 +128,16 @@
 
 float m3pi::get_battery_voltage()
 {
-    _serial->putc(0xB1);
-    char lsb = _serial->getc();
-    char msb = _serial->getc();
+    char buff[1];
+    //_serial->putc(0xB1);
+    buff[0]=0xB1;
+    _serial->write(buff,1);
+    char lsb;// = _serial->getc();
+        _serial->read(buff,1);
+    lsb = buff[0];
+    char msb;// = _serial->getc();
+        _serial->read(buff,1);
+    msb = buff[0];
     // Battery in mV so convert to volts
     float voltage = ( msb<<8 | lsb ) / 1000.0;
     return voltage;
@@ -109,33 +145,55 @@
 
 void m3pi::play_music(const char notes[],int length)
 {
+    char buff[1];
+
     length = length < 0 ? 0 : length;
     length = length > 100 ? 100 : length;
 
-    _serial->putc(0xB3);
-    _serial->putc(length);
+    //_serial->putc(0xB3);
+    buff[0]=0xB3;
+    _serial->write(buff,1);
+
+    //_serial->putc(length);
+    buff[0]=length;
+    _serial->write(buff,1);
 
     for (int i = 0 ; i < length ; i++) {
-        _serial->putc(notes[i]);
+        //_serial->putc(notes[i]);
+        buff[0]=notes[i];
+        _serial->write(buff,1);
     }
 }
 
 void m3pi::calibrate()
 {
-    _serial->putc(0xB4);
+    char buff[1];
+    //_serial->putc(0xB4);
+    buff[0]=0xB4;
+    _serial->write(buff,1);
 }
 
 void m3pi::reset_calibration()
 {
-    _serial->putc(0xB5);
+    char buff[1];
+    //_serial->putc(0xB5);
+    buff[0]=0xB5;
+    _serial->write(buff,1);
 }
 
 float m3pi::get_line_position()
 {
-    _serial->putc(0xB6);
+    char buff[1];
+    //_serial->putc(0xB6);
+    buff[0]=0xB6;
+    _serial->write(buff,1);
 
-    char lsb = _serial->getc();
-    char msb = _serial->getc();
+    char lsb;// = _serial->getc();
+        _serial->read(buff,1);
+    lsb = buff[0];
+    char msb;// = _serial->getc();
+        _serial->read(buff,1);
+    msb = buff[0];
     int position = (msb<<8 | lsb);
 
     return float(position - 2000)/2000.0;
@@ -143,33 +201,55 @@
 
 void m3pi::lcd_clear()
 {
-    _serial->putc(0xB7);
+    char buff[1];
+    //_serial->putc(0xB7);
+    buff[0]=0xB7;
+    _serial->write(buff,1);
 }
 
 void m3pi::lcd_print(char text[],int length)
 {
+    char buff[1];
+
     length = length < 0 ? 0 : length;
     length = length > 8 ? 8 : length;
 
-    _serial->putc(0xB8);
-    _serial->putc(length);
+    //_serial->putc(0xB8);
+    buff[0]=0xB8;
+    _serial->write(buff,1);
+
+    //_serial->putc(length);
+    buff[0]=length;
+    _serial->write(buff,1);
 
     for (int i = 0 ; i < length ; i++) {
-        _serial->putc(text[i]);
+        //_serial->putc(text[i]);
+        buff[0]=text[i];
+        _serial->write(buff,1);
     }
 }
 
 void m3pi::lcd_goto_xy(int x, int y)
 {
-    _serial->putc(0xB9);
-    _serial->putc(x);
-    _serial->putc(y);
+    char buff[1];
+    //_serial->putc(0xB9);
+        buff[0]=0xB9;
+    _serial->write(buff,1);
+    //_serial->putc(x);
+        buff[0]=x;
+    _serial->write(buff,1);
+   //_serial->putc(y);
+        buff[0]=y;
+    _serial->write(buff,1);
 }
 
 void m3pi::auto_calibrate()
 {
-    _serial->putc(0xBA);
-    
+    char buff[1];
+    //_serial->putc(0xBA);
+        buff[0]=0xBA;
+    _serial->write(buff,1);
+
     while(1) {  // wait for serial response
         if (_serial->readable()) {
             break;
@@ -181,36 +261,54 @@
 
 void m3pi::left_motor(float speed)
 {
+    char buff[1];
     // check within bounds
     speed = speed > 1.0 ? 1.0 : speed;
     speed = speed < -1.0 ? -1.0 : speed;
 
     if (speed > 0.0) {  // forward
-        _serial->putc(0xC1);
+        //_serial->putc(0xC1);
+            buff[0]=0xC1;
+    _serial->write(buff,1);
         char s = char(127.0*speed);
-        _serial->putc(s);
+        //_serial->putc(s);
+            buff[0]=s;
+    _serial->write(buff,1);
     } else { // backward - speed is negative
-        _serial->putc(0xC2);
+        //_serial->putc(0xC2);
+            buff[0]=0xC2;
+    _serial->write(buff,1);
         char s = char(-127.0*speed);
-        _serial->putc(s);
+        //_serial->putc(s);
+            buff[0]=s;
+    _serial->write(buff,1);
     }
 
 }
 
 void m3pi::right_motor(float speed)
 {
+    char buff[1];
     // check within bounds
     speed = speed > 1.0 ? 1.0 : speed;
     speed = speed < -1.0 ? -1.0 : speed;
 
     if (speed > 0.0) {  // forward
-        _serial->putc(0xC5);
+        //_serial->putc(0xC5);
+            buff[0]=0xC5;
+    _serial->write(buff,1);
         char s = char(127.0*speed);
-        _serial->putc(s);
+        //_serial->putc(s);
+            buff[0]=s;
+    _serial->write(buff,1);
     } else { // backward - speed is negative
-        _serial->putc(0xC6);
+        //_serial->putc(0xC6);
+            buff[0]=0xC6;
+    _serial->write(buff,1);
         char s = char(-127.0*speed);
-        _serial->putc(s);
+        //_serial->putc(s);
+            buff[0]=s;
+    _serial->write(buff,1);
     }
 
 }
@@ -281,9 +379,15 @@
 
 void m3pi::display_signature(int x,int y)
 {
-    _serial->putc(0x81);
-    char buffer[7];  // including NULL terminator
-    _serial->gets(buffer,7);
+    char buffer[7]; // including NULL terminator
+    
+    //_serial->putc(0x81);
+        buffer[0]=0x81;
+    _serial->write(buffer,1);
+    
+    
+    //_serial->gets(buffer,7);
+    _serial->read(buffer,7);
 
     lcd_goto_xy(x,y);
     lcd_print(buffer,6);
@@ -293,31 +397,31 @@
 {
     // initialise array to ASCII '0'
     lcd_goto_xy(1,y);
-    
+
     char sensor_values[5];
-    
+
     // loop through sensor
     for (int sensor = 0 ; sensor < 5 ; sensor++) {
-        // get the value and put it in the correct bin 
+        // get the value and put it in the correct bin
         // (7 bins in the range 0 to 1000
-        char value = char(values[sensor]/(1000.0/7.0)); 
+        char value = char(values[sensor]/(1000.0/7.0));
         // use the bin to select the bar graph icon to display
         sensor_values[sensor] = _bar_graph[value];
-    }  
-    
+    }
+
     lcd_print(sensor_values,5);
 
 }
 
-void m3pi::display_data() 
+void m3pi::display_data()
 {
-    display_sensor_values(_values,1);   
-    
-    char buffer[8]={0};
+    display_sensor_values(_values,1);
+
+    char buffer[8]= {0};
     sprintf(buffer,"% .3f",_last_line_position);
     lcd_goto_xy(0,0);
-    lcd_print(buffer,6); 
-       
+    lcd_print(buffer,6);
+
 }
 
 unsigned int m3pi::get_sensor_array_value(unsigned int values[])
@@ -368,18 +472,20 @@
     return _last_line_position;
 }
 
-float m3pi::read_line() {
-    return calc_line_position(_values); 
+float m3pi::read_line()
+{
+    return calc_line_position(_values);
 }
 
 
+
 /////////////////////////////// private methods ////////////////////////////////
 
 void m3pi::reset()
 {
     // pulse the reset line (active-high)
     _reset->write(1);
-    wait_ms(100);
+    ThisThread::sleep_for(100ms);
     _reset->write(0);
-    wait_ms(100);
+    ThisThread::sleep_for(100ms);
 }
\ No newline at end of file