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@9:074ce6197b51, 2018-03-19 (annotated)

Committer:
eencae
Date:
Mon Mar 19 13:06:40 2018 +0000
Revision:
9:074ce6197b51
Parent:
8:92167bd3eb44 
Added some simple functions to the library that do not require arrays to be passed by reference. Sensor values are stored in the class instead.

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