Craig Evans / m3pi

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

Dependents:   3pi_Example_2 3pi_Lab1_Task2_Example1 3pi_Lab2_Task1_Example1 3pi_Line_Follow ... more

m3pi.cpp@3:5015bc2d1cf8, 2017-05-25 (annotated)

Committer:
eencae
Date:
Thu May 25 10:18:43 2017 +0000
Revision:
3:5015bc2d1cf8
Parent:
2:26bf14f4dc84
Child:
4:0abe81f5d9fd
Remove m3pi shield specific methods (button and LEDs) as custom shield does not have these.

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