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@6:39daa09102a0, 2017-07-05 (annotated)

Committer:
eencae
Date:
Wed Jul 05 10:05:46 2017 +0000
Revision:
6:39daa09102a0
Parent:
5:847e6cbd458b
Child:
7:b4fc73882a2a
Slight change in get calibrated values.

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