Craig Evans / m3pi

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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

m3pi.cpp@1:5523d6d1feec, 2017-04-05 (annotated)

Committer:
eencae
Date:
Wed Apr 05 10:25:42 2017 +0000
Revision:
1:5523d6d1feec
Parent:
0:56320ef879a6
Child:
2:26bf14f4dc84
Added few extra message, fixed issue with reading sensor values into array.

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