Ward Elder / m3pi_V6_Ward_copy

Library for Pololu m3pi line-following robot. Implements the serial slave commands. for MBED OS V6

m3pi.cpp@5:847e6cbd458b, 2017-06-29 (annotated)

Committer:
eencae
Date:
Thu Jun 29 09:25:07 2017 +0000
Revision:
5:847e6cbd458b
Parent:
4:0abe81f5d9fd
Child:
6:39daa09102a0
Changed code to loop while waiting for serial data to come back from 3pi. Ensures motors are not updated before sensor values arrive.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eencae 0:56320ef879a6 1 #include "m3pi.h"
eencae 0:56320ef879a6 2
eencae 0:56320ef879a6 3 ////////////////////////// constructor/destructor //////////////////////////////
eencae 0:56320ef879a6 4
eencae 0:56320ef879a6 5
eencae 0:56320ef879a6 6 m3pi::m3pi()
eencae 0:56320ef879a6 7 {
eencae 0:56320ef879a6 8 _serial = new Serial(p9,p10);
eencae 3:5015bc2d1cf8 9 _reset = new DigitalOut(p8);
eencae 2:26bf14f4dc84 10 _last_line_position = 0.0;
eencae 4:0abe81f5d9fd 11
eencae 4:0abe81f5d9fd 12 _bar_graph[0] = ' ';
eencae 4:0abe81f5d9fd 13 for (int i = 0; i < 6; i++) {
eencae 4:0abe81f5d9fd 14 _bar_graph[i+1] = i;
eencae 4:0abe81f5d9fd 15 }
eencae 0:56320ef879a6 16 }
eencae 0:56320ef879a6 17
eencae 0:56320ef879a6 18 m3pi::~m3pi()
eencae 0:56320ef879a6 19 {
eencae 0:56320ef879a6 20 delete _serial;
eencae 0:56320ef879a6 21 delete _reset;
eencae 3:5015bc2d1cf8 22
eencae 0:56320ef879a6 23 }
eencae 0:56320ef879a6 24
eencae 0:56320ef879a6 25 /////////////////////////////// public methods /////////////////////////////////
eencae 0:56320ef879a6 26
eencae 0:56320ef879a6 27 void m3pi::init()
eencae 0:56320ef879a6 28 {
eencae 0:56320ef879a6 29 _serial->baud(115200);
eencae 0:56320ef879a6 30 reset(); // hard rest of 3pi
eencae 0:56320ef879a6 31 stop(); // stop motors
eencae 0:56320ef879a6 32 lcd_clear(); // clear LCD
eencae 0:56320ef879a6 33 }
eencae 0:56320ef879a6 34
eencae 2:26bf14f4dc84 35 /////////////////////////////// serial slave commands ////////////////////////////////
eencae 2:26bf14f4dc84 36
eencae 2:26bf14f4dc84 37
eencae 0:56320ef879a6 38 void m3pi::get_signature(char *signature)
eencae 0:56320ef879a6 39 {
eencae 0:56320ef879a6 40 _serial->putc(0x81);
eencae 0:56320ef879a6 41 _serial->gets(signature,7);
eencae 0:56320ef879a6 42 }
eencae 0:56320ef879a6 43
eencae 0:56320ef879a6 44 void m3pi::get_raw_values(unsigned int *values)
eencae 0:56320ef879a6 45 {
eencae 1:5523d6d1feec 46 char vals[10]; // array to receive 10 byte return message
eencae 1:5523d6d1feec 47 _serial->putc(0x86); // send command
eencae 0:56320ef879a6 48
eencae 1:5523d6d1feec 49 int n=0;
eencae 1:5523d6d1feec 50 while ( _serial->readable() ) { // keep looping while data on rx line
eencae 1:5523d6d1feec 51 vals[n] = _serial->getc(); // read into array
eencae 1:5523d6d1feec 52 n++; // increment index
eencae 1:5523d6d1feec 53 }
eencae 1:5523d6d1feec 54
eencae 1:5523d6d1feec 55 for(int i=0; i<5; i++) { // construct the 2-byte values
eencae 1:5523d6d1feec 56 values[i] = (vals[2*i+1] << 8) | vals[2*i];
eencae 0:56320ef879a6 57 }
eencae 0:56320ef879a6 58 }
eencae 0:56320ef879a6 59
eencae 0:56320ef879a6 60 void m3pi::get_calibrated_values(unsigned int *values)
eencae 0:56320ef879a6 61 {
eencae 1:5523d6d1feec 62 char vals[10]; // array to receive 10 byte return message
eencae 1:5523d6d1feec 63 _serial->putc(0x87); // send command
eencae 5:847e6cbd458b 64
eencae 5:847e6cbd458b 65 while (_serial->readable() == 0) {
eencae 5:847e6cbd458b 66
eencae 0:56320ef879a6 67 }
eencae 5:847e6cbd458b 68
eencae 5:847e6cbd458b 69 for (int i=0; i < 10; i++) {
eencae 5:847e6cbd458b 70 vals[i] = _serial->getc();
eencae 5:847e6cbd458b 71 }
eencae 5:847e6cbd458b 72
eencae 1:5523d6d1feec 73 for(int i=0; i<5; i++) { // construct the 2-byte values
eencae 1:5523d6d1feec 74 values[i] = (vals[2*i+1] << 8) | vals[2*i];
eencae 1:5523d6d1feec 75 }
eencae 2:26bf14f4dc84 76
eencae 5:847e6cbd458b 77 //int n=0;
eencae 5:847e6cbd458b 78 //while ( _serial->readable() ) { // keep looping while data on rx line
eencae 5:847e6cbd458b 79 // vals[n] = _serial->getc(); // read into array
eencae 5:847e6cbd458b 80 // n++; // increment index
eencae 5:847e6cbd458b 81 //}
eencae 5:847e6cbd458b 82
eencae 5:847e6cbd458b 83 //for(int i=0; i<5; i++) { // construct the 2-byte values
eencae 5:847e6cbd458b 84 // values[i] = (vals[2*i+1] << 8) | vals[2*i];
eencae 5:847e6cbd458b 85 //}
eencae 5:847e6cbd458b 86
eencae 5:847e6cbd458b 87 //wait_ms(1);
eencae 0:56320ef879a6 88 }
eencae 0:56320ef879a6 89
eencae 0:56320ef879a6 90 float m3pi::get_trimpot_value()
eencae 0:56320ef879a6 91 {
eencae 0:56320ef879a6 92 _serial->putc(0xB0);
eencae 0:56320ef879a6 93 char lsb = _serial->getc();
eencae 0:56320ef879a6 94 char msb = _serial->getc();
eencae 0:56320ef879a6 95 // trimpot value in the range 0 - 1023
eencae 0:56320ef879a6 96 float value = ( msb<<8 | lsb ) / 1023.0;
eencae 0:56320ef879a6 97 return value;
eencae 0:56320ef879a6 98 }
eencae 0:56320ef879a6 99
eencae 0:56320ef879a6 100
eencae 0:56320ef879a6 101 float m3pi::get_battery_voltage()
eencae 0:56320ef879a6 102 {
eencae 0:56320ef879a6 103 _serial->putc(0xB1);
eencae 0:56320ef879a6 104 char lsb = _serial->getc();
eencae 0:56320ef879a6 105 char msb = _serial->getc();
eencae 0:56320ef879a6 106 // Battery in mV so convert to volts
eencae 0:56320ef879a6 107 float voltage = ( msb<<8 | lsb ) / 1000.0;
eencae 0:56320ef879a6 108 return voltage;
eencae 0:56320ef879a6 109 }
eencae 0:56320ef879a6 110
eencae 0:56320ef879a6 111 void m3pi::play_music(const char notes[],int length)
eencae 0:56320ef879a6 112 {
eencae 0:56320ef879a6 113 length = length < 0 ? 0 : length;
eencae 0:56320ef879a6 114 length = length > 100 ? 100 : length;
eencae 0:56320ef879a6 115
eencae 0:56320ef879a6 116 _serial->putc(0xB3);
eencae 0:56320ef879a6 117 _serial->putc(length);
eencae 0:56320ef879a6 118
eencae 0:56320ef879a6 119 for (int i = 0 ; i < length ; i++) {
eencae 0:56320ef879a6 120 _serial->putc(notes[i]);
eencae 0:56320ef879a6 121 }
eencae 0:56320ef879a6 122 }
eencae 0:56320ef879a6 123
eencae 0:56320ef879a6 124
eencae 0:56320ef879a6 125 void m3pi::calibrate()
eencae 0:56320ef879a6 126 {
eencae 3:5015bc2d1cf8 127 _serial->putc(0xB4);
eencae 0:56320ef879a6 128 }
eencae 0:56320ef879a6 129
eencae 0:56320ef879a6 130 void m3pi::reset_calibration()
eencae 0:56320ef879a6 131 {
eencae 0:56320ef879a6 132 _serial->putc(0xB5);
eencae 0:56320ef879a6 133 }
eencae 0:56320ef879a6 134
eencae 2:26bf14f4dc84 135 float m3pi::get_line_position()
eencae 0:56320ef879a6 136 {
eencae 0:56320ef879a6 137 _serial->putc(0xB6);
eencae 0:56320ef879a6 138
eencae 0:56320ef879a6 139 char lsb = _serial->getc();
eencae 0:56320ef879a6 140 char msb = _serial->getc();
eencae 0:56320ef879a6 141 int position = (msb<<8 | lsb);
eencae 0:56320ef879a6 142
eencae 2:26bf14f4dc84 143 return float(position - 2000)/2000.0;
eencae 0:56320ef879a6 144 }
eencae 0:56320ef879a6 145
eencae 0:56320ef879a6 146 void m3pi::lcd_clear()
eencae 0:56320ef879a6 147 {
eencae 0:56320ef879a6 148 _serial->putc(0xB7);
eencae 0:56320ef879a6 149 }
eencae 0:56320ef879a6 150
eencae 0:56320ef879a6 151 void m3pi::lcd_print(char text[],int length)
eencae 0:56320ef879a6 152 {
eencae 0:56320ef879a6 153 length = length < 0 ? 0 : length;
eencae 0:56320ef879a6 154 length = length > 8 ? 8 : length;
eencae 0:56320ef879a6 155
eencae 0:56320ef879a6 156 _serial->putc(0xB8);
eencae 0:56320ef879a6 157 _serial->putc(length);
eencae 0:56320ef879a6 158
eencae 0:56320ef879a6 159 for (int i = 0 ; i < length ; i++) {
eencae 0:56320ef879a6 160 _serial->putc(text[i]);
eencae 0:56320ef879a6 161 }
eencae 0:56320ef879a6 162 }
eencae 0:56320ef879a6 163
eencae 0:56320ef879a6 164 void m3pi::lcd_goto_xy(int x, int y)
eencae 0:56320ef879a6 165 {
eencae 0:56320ef879a6 166 _serial->putc(0xB9);
eencae 0:56320ef879a6 167 _serial->putc(x);
eencae 0:56320ef879a6 168 _serial->putc(y);
eencae 0:56320ef879a6 169 }
eencae 0:56320ef879a6 170
eencae 0:56320ef879a6 171 void m3pi::auto_calibrate()
eencae 0:56320ef879a6 172 {
eencae 0:56320ef879a6 173 _serial->putc(0xBA);
eencae 3:5015bc2d1cf8 174
eencae 3:5015bc2d1cf8 175 while(1) { // wait for serial response
eencae 0:56320ef879a6 176 if (_serial->readable()) {
eencae 0:56320ef879a6 177 break;
eencae 0:56320ef879a6 178 }
eencae 0:56320ef879a6 179 }
eencae 0:56320ef879a6 180 }
eencae 0:56320ef879a6 181
eencae 2:26bf14f4dc84 182 /////////////////////////////// motor methods ////////////////////////////////
eencae 2:26bf14f4dc84 183
eencae 0:56320ef879a6 184 void m3pi::left_motor(float speed)
eencae 0:56320ef879a6 185 {
eencae 0:56320ef879a6 186 // check within bounds
eencae 0:56320ef879a6 187 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 188 speed = speed < -1.0 ? -1.0 : speed;
eencae 0:56320ef879a6 189
eencae 0:56320ef879a6 190 if (speed > 0.0) { // forward
eencae 0:56320ef879a6 191 _serial->putc(0xC1);
eencae 0:56320ef879a6 192 char s = char(127.0*speed);
eencae 0:56320ef879a6 193 _serial->putc(s);
eencae 0:56320ef879a6 194 } else { // backward - speed is negative
eencae 0:56320ef879a6 195 _serial->putc(0xC2);
eencae 0:56320ef879a6 196 char s = char(-127.0*speed);
eencae 0:56320ef879a6 197 _serial->putc(s);
eencae 0:56320ef879a6 198 }
eencae 0:56320ef879a6 199
eencae 0:56320ef879a6 200 }
eencae 0:56320ef879a6 201
eencae 0:56320ef879a6 202 void m3pi::right_motor(float speed)
eencae 0:56320ef879a6 203 {
eencae 0:56320ef879a6 204 // check within bounds
eencae 0:56320ef879a6 205 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 206 speed = speed < -1.0 ? -1.0 : speed;
eencae 0:56320ef879a6 207
eencae 0:56320ef879a6 208 if (speed > 0.0) { // forward
eencae 0:56320ef879a6 209 _serial->putc(0xC5);
eencae 0:56320ef879a6 210 char s = char(127.0*speed);
eencae 0:56320ef879a6 211 _serial->putc(s);
eencae 0:56320ef879a6 212 } else { // backward - speed is negative
eencae 0:56320ef879a6 213 _serial->putc(0xC6);
eencae 0:56320ef879a6 214 char s = char(-127.0*speed);
eencae 0:56320ef879a6 215 _serial->putc(s);
eencae 0:56320ef879a6 216 }
eencae 0:56320ef879a6 217
eencae 0:56320ef879a6 218 }
eencae 0:56320ef879a6 219
eencae 0:56320ef879a6 220 // speeds from -1.0 to 1.0 (0 is stop)
eencae 0:56320ef879a6 221 void m3pi::motors(float left_speed,float right_speed)
eencae 0:56320ef879a6 222 {
eencae 0:56320ef879a6 223 left_motor(left_speed);
eencae 0:56320ef879a6 224 right_motor(right_speed);
eencae 0:56320ef879a6 225 }
eencae 0:56320ef879a6 226
eencae 0:56320ef879a6 227 void m3pi::stop()
eencae 0:56320ef879a6 228 {
eencae 0:56320ef879a6 229 left_motor(0.0);
eencae 0:56320ef879a6 230 right_motor(0.0);
eencae 0:56320ef879a6 231 }
eencae 0:56320ef879a6 232
eencae 0:56320ef879a6 233 // speed in range 0.0 to 1.0
eencae 0:56320ef879a6 234 void m3pi::forward(float speed)
eencae 0:56320ef879a6 235 {
eencae 0:56320ef879a6 236 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 237 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 238
eencae 0:56320ef879a6 239 left_motor(speed);
eencae 0:56320ef879a6 240 right_motor(speed);
eencae 0:56320ef879a6 241 }
eencae 0:56320ef879a6 242
eencae 0:56320ef879a6 243 // speed in range 0 to 1.0
eencae 0:56320ef879a6 244 void m3pi::reverse(float speed)
eencae 0:56320ef879a6 245 {
eencae 0:56320ef879a6 246 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 247 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 248
eencae 0:56320ef879a6 249 left_motor(-speed);
eencae 0:56320ef879a6 250 right_motor(-speed);
eencae 0:56320ef879a6 251 }
eencae 0:56320ef879a6 252
eencae 0:56320ef879a6 253 void m3pi::spin_right(float speed)
eencae 0:56320ef879a6 254 {
eencae 0:56320ef879a6 255 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 256 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 257
eencae 0:56320ef879a6 258 left_motor(speed);
eencae 0:56320ef879a6 259 right_motor(-speed);
eencae 0:56320ef879a6 260 }
eencae 0:56320ef879a6 261
eencae 0:56320ef879a6 262 void m3pi::spin_left(float speed)
eencae 0:56320ef879a6 263 {
eencae 0:56320ef879a6 264 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 265 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 266
eencae 0:56320ef879a6 267 left_motor(-speed);
eencae 0:56320ef879a6 268 right_motor(speed);
eencae 0:56320ef879a6 269 }
eencae 0:56320ef879a6 270
eencae 2:26bf14f4dc84 271 ////////////////////////////////////////////////////////////////////////////////
eencae 2:26bf14f4dc84 272
eencae 0:56320ef879a6 273 void m3pi::display_battery_voltage(int x,int y)
eencae 0:56320ef879a6 274 {
eencae 0:56320ef879a6 275 float voltage = get_battery_voltage();
eencae 0:56320ef879a6 276
eencae 0:56320ef879a6 277 char buffer[8];
eencae 0:56320ef879a6 278 sprintf(buffer,"%3.1f V",voltage);
eencae 0:56320ef879a6 279
eencae 0:56320ef879a6 280 lcd_goto_xy(x,y);
eencae 0:56320ef879a6 281 lcd_print(buffer,5);
eencae 0:56320ef879a6 282 }
eencae 0:56320ef879a6 283
eencae 0:56320ef879a6 284 void m3pi::display_signature(int x,int y)
eencae 0:56320ef879a6 285 {
eencae 0:56320ef879a6 286 _serial->putc(0x81);
eencae 0:56320ef879a6 287 char buffer[7]; // including NULL terminator
eencae 0:56320ef879a6 288 _serial->gets(buffer,7);
eencae 0:56320ef879a6 289
eencae 0:56320ef879a6 290 lcd_goto_xy(x,y);
eencae 0:56320ef879a6 291 lcd_print(buffer,6);
eencae 0:56320ef879a6 292 }
eencae 0:56320ef879a6 293
eencae 2:26bf14f4dc84 294 void m3pi::display_sensor_values(unsigned int values[],int y)
eencae 2:26bf14f4dc84 295 {
eencae 2:26bf14f4dc84 296 // initialise array to ASCII '0'
eencae 4:0abe81f5d9fd 297 lcd_goto_xy(1,y);
eencae 4:0abe81f5d9fd 298
eencae 4:0abe81f5d9fd 299 char sensor_values[5];
eencae 4:0abe81f5d9fd 300
eencae 4:0abe81f5d9fd 301 // loop through sensor
eencae 4:0abe81f5d9fd 302 for (int sensor = 0 ; sensor < 5 ; sensor++) {
eencae 4:0abe81f5d9fd 303 // get the value and put it in the correct bin
eencae 4:0abe81f5d9fd 304 // (7 bins in the range 0 to 1000
eencae 4:0abe81f5d9fd 305 char value = char(values[sensor]/(1000.0/7.0));
eencae 4:0abe81f5d9fd 306 // use the bin to select the bar graph icon to display
eencae 4:0abe81f5d9fd 307 sensor_values[sensor] = _bar_graph[value];
eencae 4:0abe81f5d9fd 308 }
eencae 4:0abe81f5d9fd 309
eencae 4:0abe81f5d9fd 310 lcd_print(sensor_values,5);
eencae 2:26bf14f4dc84 311
eencae 2:26bf14f4dc84 312 }
eencae 2:26bf14f4dc84 313
eencae 2:26bf14f4dc84 314 unsigned int m3pi::get_sensor_array_value(unsigned int values[])
eencae 2:26bf14f4dc84 315 {
eencae 2:26bf14f4dc84 316 unsigned int value = 0;
eencae 2:26bf14f4dc84 317
eencae 2:26bf14f4dc84 318 // loop through each bit, starting from PC4
eencae 2:26bf14f4dc84 319 for (int i = 4; i >= 0; i--) {
eencae 2:26bf14f4dc84 320
eencae 2:26bf14f4dc84 321 unsigned int weight = pow(2.0,4-i);
eencae 2:26bf14f4dc84 322
eencae 2:26bf14f4dc84 323 // check if over threshold
eencae 2:26bf14f4dc84 324 if (values[i] > 500) {
eencae 2:26bf14f4dc84 325 // add equivalent binary weight to value
eencae 2:26bf14f4dc84 326 value += weight;
eencae 2:26bf14f4dc84 327 }
eencae 2:26bf14f4dc84 328
eencae 2:26bf14f4dc84 329 }
eencae 2:26bf14f4dc84 330
eencae 2:26bf14f4dc84 331 return value;
eencae 2:26bf14f4dc84 332 }
eencae 2:26bf14f4dc84 333
eencae 2:26bf14f4dc84 334 float m3pi::calc_line_position(unsigned int values[])
eencae 2:26bf14f4dc84 335 {
eencae 2:26bf14f4dc84 336 // calculate weighted average
eencae 2:26bf14f4dc84 337 unsigned int value =
eencae 2:26bf14f4dc84 338 (0*values[0]+1e3*values[1]+2e3*values[2]+3e3*values[3]+4e3*values[4])/
eencae 2:26bf14f4dc84 339 (values[0]+values[1]+values[2]+values[3]+values[4]);
eencae 2:26bf14f4dc84 340
eencae 2:26bf14f4dc84 341 // scale to between -1.0 and 1.0
eencae 2:26bf14f4dc84 342 float position = (int(value) - 2000)/2000.0;
eencae 2:26bf14f4dc84 343
eencae 2:26bf14f4dc84 344 float is_on_line = false;
eencae 2:26bf14f4dc84 345
eencae 2:26bf14f4dc84 346 // loop through and check if any sensor reading is above the threshold
eencae 2:26bf14f4dc84 347 for (int i = 0; i<5; i++) {
eencae 2:26bf14f4dc84 348 if (values[i] > 500) {
eencae 2:26bf14f4dc84 349 is_on_line = true;
eencae 2:26bf14f4dc84 350 }
eencae 2:26bf14f4dc84 351 }
eencae 2:26bf14f4dc84 352
eencae 2:26bf14f4dc84 353 // update last line position if over line
eencae 2:26bf14f4dc84 354 if (is_on_line) {
eencae 2:26bf14f4dc84 355 _last_line_position = position;
eencae 2:26bf14f4dc84 356 }
eencae 2:26bf14f4dc84 357
eencae 2:26bf14f4dc84 358 // if not on line then the last line position will have the last value when over line
eencae 2:26bf14f4dc84 359 return _last_line_position;
eencae 2:26bf14f4dc84 360 }
eencae 2:26bf14f4dc84 361
eencae 0:56320ef879a6 362 /////////////////////////////// private methods ////////////////////////////////
eencae 0:56320ef879a6 363
eencae 0:56320ef879a6 364 void m3pi::reset()
eencae 0:56320ef879a6 365 {
eencae 0:56320ef879a6 366 // pulse the reset line (active-high)
eencae 3:5015bc2d1cf8 367 _reset->write(1);
eencae 3:5015bc2d1cf8 368 wait_ms(100);
eencae 0:56320ef879a6 369 _reset->write(0);
eencae 0:56320ef879a6 370 wait_ms(100);
eencae 0:56320ef879a6 371 }