James Heavey / 3875_Individualproject

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:   3875_DISSERTATION

m3pi.cpp@11:b550167f5278, 2020-02-13 (annotated)

Committer:
jamesheavey
Date:
Thu Feb 13 16:08:34 2020 +0000
Revision:
11:b550167f5278
Parent:
10:0dec82280040 
Changed the library so that read_line only uses the middle 3 sensors preventing right turn error when straight is available

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 =
jamesheavey 11:b550167f5278 347 (0*values[1]+1000*values[2]+2000*values[3])/
jamesheavey 11:b550167f5278 348 (values[1]+values[2]+values[3]);
eencae 2:26bf14f4dc84 349
eencae 2:26bf14f4dc84 350 // scale to between -1.0 and 1.0
jamesheavey 11:b550167f5278 351 float position = (int(value) - 1000)/1000.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
jamesheavey 11:b550167f5278 356 for (int i = 1; i<4; 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
jamesheavey 10:0dec82280040 375 unsigned int * m3pi::get_sensors() {
jamesheavey 10:0dec82280040 376 return _values;
jamesheavey 10:0dec82280040 377 }
jamesheavey 10:0dec82280040 378
eencae 9:074ce6197b51 379
eencae 0:56320ef879a6 380 /////////////////////////////// private methods ////////////////////////////////
eencae 0:56320ef879a6 381
eencae 0:56320ef879a6 382 void m3pi::reset()
eencae 0:56320ef879a6 383 {
eencae 0:56320ef879a6 384 // pulse the reset line (active-high)
eencae 3:5015bc2d1cf8 385 _reset->write(1);
eencae 3:5015bc2d1cf8 386 wait_ms(100);
eencae 0:56320ef879a6 387 _reset->write(0);
eencae 0:56320ef879a6 388 wait_ms(100);
eencae 0:56320ef879a6 389 }