Ward Elder / m3pi_V6_Ward_copy

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

m3pi.cpp@10:c3617fcf2c58, 2020-11-12 (annotated)

Committer:
wardelder
Date:
Thu Nov 12 22:37:20 2020 +0000
Revision:
10:c3617fcf2c58
Parent:
9:074ce6197b51 
Wards MBED OS6 changes;

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 {
wardelder 10:c3617fcf2c58 8 _serial = new BufferedSerial(p9,p10);
eencae 3:5015bc2d1cf8 9 _reset = new DigitalOut(p8);
eencae 2:26bf14f4dc84 10 _last_line_position = 0.0;
wardelder 10:c3617fcf2c58 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++) {
wardelder 10:c3617fcf2c58 15 _bar_graph[i+1] = i;
eencae 4:0abe81f5d9fd 16 }
eencae 9:074ce6197b51 17 for (int i = 0; i < 5; i++) {
wardelder 10:c3617fcf2c58 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 {
wardelder 10:c3617fcf2c58 33 _serial->set_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
wardelder 10:c3617fcf2c58 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 {
wardelder 10:c3617fcf2c58 48 char buff[7];
wardelder 10:c3617fcf2c58 49 //_serial->putc(0x81);
wardelder 10:c3617fcf2c58 50 buff[1]=0x81;
wardelder 10:c3617fcf2c58 51 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 52 //_serial->gets(signature,7);
wardelder 10:c3617fcf2c58 53 _serial->read(buff,7);
eencae 0:56320ef879a6 54 }
eencae 0:56320ef879a6 55
eencae 0:56320ef879a6 56 void m3pi::get_raw_values(unsigned int *values)
eencae 0:56320ef879a6 57 {
wardelder 10:c3617fcf2c58 58 char buff[1]; // send command
wardelder 10:c3617fcf2c58 59
eencae 8:92167bd3eb44 60 while (_serial->readable() ) { // flush buffer
wardelder 10:c3617fcf2c58 61 //_serial->getc();
wardelder 10:c3617fcf2c58 62 _serial->sync();
wardelder 10:c3617fcf2c58 63
eencae 8:92167bd3eb44 64 }
wardelder 10:c3617fcf2c58 65
eencae 1:5523d6d1feec 66 char vals[10]; // array to receive 10 byte return message
wardelder 10:c3617fcf2c58 67
wardelder 10:c3617fcf2c58 68 //_serial->putc(0x86);
wardelder 10:c3617fcf2c58 69 buff[0]=0x86;
wardelder 10:c3617fcf2c58 70 _serial->write(buff,1);
eencae 0:56320ef879a6 71
eencae 8:92167bd3eb44 72 for (int i=0; i < 10; i++) {
wardelder 10:c3617fcf2c58 73 //vals[i] = _serial->getc();
wardelder 10:c3617fcf2c58 74 _serial->read(buff,1);
wardelder 10:c3617fcf2c58 75 vals[i] = buff[0];
eencae 1:5523d6d1feec 76 }
eencae 1:5523d6d1feec 77
eencae 1:5523d6d1feec 78 for(int i=0; i<5; i++) { // construct the 2-byte values
eencae 1:5523d6d1feec 79 values[i] = (vals[2*i+1] << 8) | vals[2*i];
eencae 0:56320ef879a6 80 }
eencae 0:56320ef879a6 81 }
eencae 0:56320ef879a6 82
eencae 0:56320ef879a6 83 void m3pi::get_calibrated_values(unsigned int *values)
eencae 0:56320ef879a6 84 {
wardelder 10:c3617fcf2c58 85 char buff[1];
wardelder 10:c3617fcf2c58 86
eencae 7:b4fc73882a2a 87 while (_serial->readable() ) { // flush buffer
wardelder 10:c3617fcf2c58 88 //_serial->getc();
wardelder 10:c3617fcf2c58 89 _serial->sync();
eencae 7:b4fc73882a2a 90 }
wardelder 10:c3617fcf2c58 91
eencae 1:5523d6d1feec 92 char vals[10]; // array to receive 10 byte return message
wardelder 10:c3617fcf2c58 93 //_serial->putc(0x87); // send command
wardelder 10:c3617fcf2c58 94 buff[0]=0x87;
wardelder 10:c3617fcf2c58 95 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 96
eencae 5:847e6cbd458b 97 for (int i=0; i < 10; i++) {
wardelder 10:c3617fcf2c58 98 //vals[i] = _serial->getc();
wardelder 10:c3617fcf2c58 99 _serial->read(buff,1);
wardelder 10:c3617fcf2c58 100 vals[i] = buff[0];
eencae 5:847e6cbd458b 101 }
wardelder 10:c3617fcf2c58 102
eencae 1:5523d6d1feec 103 for(int i=0; i<5; i++) { // construct the 2-byte values
eencae 1:5523d6d1feec 104 values[i] = (vals[2*i+1] << 8) | vals[2*i];
eencae 1:5523d6d1feec 105 }
wardelder 10:c3617fcf2c58 106
eencae 0:56320ef879a6 107 }
eencae 0:56320ef879a6 108
eencae 0:56320ef879a6 109 float m3pi::get_trimpot_value()
eencae 0:56320ef879a6 110 {
wardelder 10:c3617fcf2c58 111 char buff[1];
wardelder 10:c3617fcf2c58 112
wardelder 10:c3617fcf2c58 113 //_serial->putc(0xB0);
wardelder 10:c3617fcf2c58 114 buff[0]=0xB0;
wardelder 10:c3617fcf2c58 115 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 116
wardelder 10:c3617fcf2c58 117 char lsb;// = _serial->getc();
wardelder 10:c3617fcf2c58 118 _serial->read(buff,1);
wardelder 10:c3617fcf2c58 119 lsb = buff[0];
wardelder 10:c3617fcf2c58 120 char msb;// = _serial->getc();
wardelder 10:c3617fcf2c58 121 _serial->read(buff,1);
wardelder 10:c3617fcf2c58 122 msb = buff[0];
eencae 0:56320ef879a6 123 // trimpot value in the range 0 - 1023
eencae 0:56320ef879a6 124 float value = ( msb<<8 | lsb ) / 1023.0;
eencae 0:56320ef879a6 125 return value;
eencae 0:56320ef879a6 126 }
eencae 0:56320ef879a6 127
eencae 0:56320ef879a6 128
eencae 0:56320ef879a6 129 float m3pi::get_battery_voltage()
eencae 0:56320ef879a6 130 {
wardelder 10:c3617fcf2c58 131 char buff[1];
wardelder 10:c3617fcf2c58 132 //_serial->putc(0xB1);
wardelder 10:c3617fcf2c58 133 buff[0]=0xB1;
wardelder 10:c3617fcf2c58 134 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 135 char lsb;// = _serial->getc();
wardelder 10:c3617fcf2c58 136 _serial->read(buff,1);
wardelder 10:c3617fcf2c58 137 lsb = buff[0];
wardelder 10:c3617fcf2c58 138 char msb;// = _serial->getc();
wardelder 10:c3617fcf2c58 139 _serial->read(buff,1);
wardelder 10:c3617fcf2c58 140 msb = buff[0];
eencae 0:56320ef879a6 141 // Battery in mV so convert to volts
eencae 0:56320ef879a6 142 float voltage = ( msb<<8 | lsb ) / 1000.0;
eencae 0:56320ef879a6 143 return voltage;
eencae 0:56320ef879a6 144 }
eencae 0:56320ef879a6 145
eencae 0:56320ef879a6 146 void m3pi::play_music(const char notes[],int length)
eencae 0:56320ef879a6 147 {
wardelder 10:c3617fcf2c58 148 char buff[1];
wardelder 10:c3617fcf2c58 149
eencae 0:56320ef879a6 150 length = length < 0 ? 0 : length;
eencae 0:56320ef879a6 151 length = length > 100 ? 100 : length;
eencae 0:56320ef879a6 152
wardelder 10:c3617fcf2c58 153 //_serial->putc(0xB3);
wardelder 10:c3617fcf2c58 154 buff[0]=0xB3;
wardelder 10:c3617fcf2c58 155 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 156
wardelder 10:c3617fcf2c58 157 //_serial->putc(length);
wardelder 10:c3617fcf2c58 158 buff[0]=length;
wardelder 10:c3617fcf2c58 159 _serial->write(buff,1);
eencae 0:56320ef879a6 160
eencae 0:56320ef879a6 161 for (int i = 0 ; i < length ; i++) {
wardelder 10:c3617fcf2c58 162 //_serial->putc(notes[i]);
wardelder 10:c3617fcf2c58 163 buff[0]=notes[i];
wardelder 10:c3617fcf2c58 164 _serial->write(buff,1);
eencae 0:56320ef879a6 165 }
eencae 0:56320ef879a6 166 }
eencae 0:56320ef879a6 167
eencae 0:56320ef879a6 168 void m3pi::calibrate()
eencae 0:56320ef879a6 169 {
wardelder 10:c3617fcf2c58 170 char buff[1];
wardelder 10:c3617fcf2c58 171 //_serial->putc(0xB4);
wardelder 10:c3617fcf2c58 172 buff[0]=0xB4;
wardelder 10:c3617fcf2c58 173 _serial->write(buff,1);
eencae 0:56320ef879a6 174 }
eencae 0:56320ef879a6 175
eencae 0:56320ef879a6 176 void m3pi::reset_calibration()
eencae 0:56320ef879a6 177 {
wardelder 10:c3617fcf2c58 178 char buff[1];
wardelder 10:c3617fcf2c58 179 //_serial->putc(0xB5);
wardelder 10:c3617fcf2c58 180 buff[0]=0xB5;
wardelder 10:c3617fcf2c58 181 _serial->write(buff,1);
eencae 0:56320ef879a6 182 }
eencae 0:56320ef879a6 183
eencae 2:26bf14f4dc84 184 float m3pi::get_line_position()
eencae 0:56320ef879a6 185 {
wardelder 10:c3617fcf2c58 186 char buff[1];
wardelder 10:c3617fcf2c58 187 //_serial->putc(0xB6);
wardelder 10:c3617fcf2c58 188 buff[0]=0xB6;
wardelder 10:c3617fcf2c58 189 _serial->write(buff,1);
eencae 0:56320ef879a6 190
wardelder 10:c3617fcf2c58 191 char lsb;// = _serial->getc();
wardelder 10:c3617fcf2c58 192 _serial->read(buff,1);
wardelder 10:c3617fcf2c58 193 lsb = buff[0];
wardelder 10:c3617fcf2c58 194 char msb;// = _serial->getc();
wardelder 10:c3617fcf2c58 195 _serial->read(buff,1);
wardelder 10:c3617fcf2c58 196 msb = buff[0];
eencae 0:56320ef879a6 197 int position = (msb<<8 | lsb);
eencae 0:56320ef879a6 198
eencae 2:26bf14f4dc84 199 return float(position - 2000)/2000.0;
eencae 0:56320ef879a6 200 }
eencae 0:56320ef879a6 201
eencae 0:56320ef879a6 202 void m3pi::lcd_clear()
eencae 0:56320ef879a6 203 {
wardelder 10:c3617fcf2c58 204 char buff[1];
wardelder 10:c3617fcf2c58 205 //_serial->putc(0xB7);
wardelder 10:c3617fcf2c58 206 buff[0]=0xB7;
wardelder 10:c3617fcf2c58 207 _serial->write(buff,1);
eencae 0:56320ef879a6 208 }
eencae 0:56320ef879a6 209
eencae 0:56320ef879a6 210 void m3pi::lcd_print(char text[],int length)
eencae 0:56320ef879a6 211 {
wardelder 10:c3617fcf2c58 212 char buff[1];
wardelder 10:c3617fcf2c58 213
eencae 0:56320ef879a6 214 length = length < 0 ? 0 : length;
eencae 0:56320ef879a6 215 length = length > 8 ? 8 : length;
eencae 0:56320ef879a6 216
wardelder 10:c3617fcf2c58 217 //_serial->putc(0xB8);
wardelder 10:c3617fcf2c58 218 buff[0]=0xB8;
wardelder 10:c3617fcf2c58 219 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 220
wardelder 10:c3617fcf2c58 221 //_serial->putc(length);
wardelder 10:c3617fcf2c58 222 buff[0]=length;
wardelder 10:c3617fcf2c58 223 _serial->write(buff,1);
eencae 0:56320ef879a6 224
eencae 0:56320ef879a6 225 for (int i = 0 ; i < length ; i++) {
wardelder 10:c3617fcf2c58 226 //_serial->putc(text[i]);
wardelder 10:c3617fcf2c58 227 buff[0]=text[i];
wardelder 10:c3617fcf2c58 228 _serial->write(buff,1);
eencae 0:56320ef879a6 229 }
eencae 0:56320ef879a6 230 }
eencae 0:56320ef879a6 231
eencae 0:56320ef879a6 232 void m3pi::lcd_goto_xy(int x, int y)
eencae 0:56320ef879a6 233 {
wardelder 10:c3617fcf2c58 234 char buff[1];
wardelder 10:c3617fcf2c58 235 //_serial->putc(0xB9);
wardelder 10:c3617fcf2c58 236 buff[0]=0xB9;
wardelder 10:c3617fcf2c58 237 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 238 //_serial->putc(x);
wardelder 10:c3617fcf2c58 239 buff[0]=x;
wardelder 10:c3617fcf2c58 240 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 241 //_serial->putc(y);
wardelder 10:c3617fcf2c58 242 buff[0]=y;
wardelder 10:c3617fcf2c58 243 _serial->write(buff,1);
eencae 0:56320ef879a6 244 }
eencae 0:56320ef879a6 245
eencae 0:56320ef879a6 246 void m3pi::auto_calibrate()
eencae 0:56320ef879a6 247 {
wardelder 10:c3617fcf2c58 248 char buff[1];
wardelder 10:c3617fcf2c58 249 //_serial->putc(0xBA);
wardelder 10:c3617fcf2c58 250 buff[0]=0xBA;
wardelder 10:c3617fcf2c58 251 _serial->write(buff,1);
wardelder 10:c3617fcf2c58 252
eencae 3:5015bc2d1cf8 253 while(1) { // wait for serial response
eencae 0:56320ef879a6 254 if (_serial->readable()) {
eencae 0:56320ef879a6 255 break;
eencae 0:56320ef879a6 256 }
eencae 0:56320ef879a6 257 }
eencae 0:56320ef879a6 258 }
eencae 0:56320ef879a6 259
eencae 2:26bf14f4dc84 260 /////////////////////////////// motor methods ////////////////////////////////
eencae 2:26bf14f4dc84 261
eencae 0:56320ef879a6 262 void m3pi::left_motor(float speed)
eencae 0:56320ef879a6 263 {
wardelder 10:c3617fcf2c58 264 char buff[1];
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
wardelder 10:c3617fcf2c58 270 //_serial->putc(0xC1);
wardelder 10:c3617fcf2c58 271 buff[0]=0xC1;
wardelder 10:c3617fcf2c58 272 _serial->write(buff,1);
eencae 0:56320ef879a6 273 char s = char(127.0*speed);
wardelder 10:c3617fcf2c58 274 //_serial->putc(s);
wardelder 10:c3617fcf2c58 275 buff[0]=s;
wardelder 10:c3617fcf2c58 276 _serial->write(buff,1);
eencae 0:56320ef879a6 277 } else { // backward - speed is negative
wardelder 10:c3617fcf2c58 278 //_serial->putc(0xC2);
wardelder 10:c3617fcf2c58 279 buff[0]=0xC2;
wardelder 10:c3617fcf2c58 280 _serial->write(buff,1);
eencae 0:56320ef879a6 281 char s = char(-127.0*speed);
wardelder 10:c3617fcf2c58 282 //_serial->putc(s);
wardelder 10:c3617fcf2c58 283 buff[0]=s;
wardelder 10:c3617fcf2c58 284 _serial->write(buff,1);
eencae 0:56320ef879a6 285 }
eencae 0:56320ef879a6 286
eencae 0:56320ef879a6 287 }
eencae 0:56320ef879a6 288
eencae 0:56320ef879a6 289 void m3pi::right_motor(float speed)
eencae 0:56320ef879a6 290 {
wardelder 10:c3617fcf2c58 291 char buff[1];
eencae 0:56320ef879a6 292 // check within bounds
eencae 0:56320ef879a6 293 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 294 speed = speed < -1.0 ? -1.0 : speed;
eencae 0:56320ef879a6 295
eencae 0:56320ef879a6 296 if (speed > 0.0) { // forward
wardelder 10:c3617fcf2c58 297 //_serial->putc(0xC5);
wardelder 10:c3617fcf2c58 298 buff[0]=0xC5;
wardelder 10:c3617fcf2c58 299 _serial->write(buff,1);
eencae 0:56320ef879a6 300 char s = char(127.0*speed);
wardelder 10:c3617fcf2c58 301 //_serial->putc(s);
wardelder 10:c3617fcf2c58 302 buff[0]=s;
wardelder 10:c3617fcf2c58 303 _serial->write(buff,1);
eencae 0:56320ef879a6 304 } else { // backward - speed is negative
wardelder 10:c3617fcf2c58 305 //_serial->putc(0xC6);
wardelder 10:c3617fcf2c58 306 buff[0]=0xC6;
wardelder 10:c3617fcf2c58 307 _serial->write(buff,1);
eencae 0:56320ef879a6 308 char s = char(-127.0*speed);
wardelder 10:c3617fcf2c58 309 //_serial->putc(s);
wardelder 10:c3617fcf2c58 310 buff[0]=s;
wardelder 10:c3617fcf2c58 311 _serial->write(buff,1);
eencae 0:56320ef879a6 312 }
eencae 0:56320ef879a6 313
eencae 0:56320ef879a6 314 }
eencae 0:56320ef879a6 315
eencae 0:56320ef879a6 316 // speeds from -1.0 to 1.0 (0 is stop)
eencae 0:56320ef879a6 317 void m3pi::motors(float left_speed,float right_speed)
eencae 0:56320ef879a6 318 {
eencae 0:56320ef879a6 319 left_motor(left_speed);
eencae 0:56320ef879a6 320 right_motor(right_speed);
eencae 0:56320ef879a6 321 }
eencae 0:56320ef879a6 322
eencae 0:56320ef879a6 323 void m3pi::stop()
eencae 0:56320ef879a6 324 {
eencae 0:56320ef879a6 325 left_motor(0.0);
eencae 0:56320ef879a6 326 right_motor(0.0);
eencae 0:56320ef879a6 327 }
eencae 0:56320ef879a6 328
eencae 0:56320ef879a6 329 // speed in range 0.0 to 1.0
eencae 0:56320ef879a6 330 void m3pi::forward(float speed)
eencae 0:56320ef879a6 331 {
eencae 0:56320ef879a6 332 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 333 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 334
eencae 0:56320ef879a6 335 left_motor(speed);
eencae 0:56320ef879a6 336 right_motor(speed);
eencae 0:56320ef879a6 337 }
eencae 0:56320ef879a6 338
eencae 0:56320ef879a6 339 // speed in range 0 to 1.0
eencae 0:56320ef879a6 340 void m3pi::reverse(float speed)
eencae 0:56320ef879a6 341 {
eencae 0:56320ef879a6 342 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 343 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 344
eencae 0:56320ef879a6 345 left_motor(-speed);
eencae 0:56320ef879a6 346 right_motor(-speed);
eencae 0:56320ef879a6 347 }
eencae 0:56320ef879a6 348
eencae 0:56320ef879a6 349 void m3pi::spin_right(float speed)
eencae 0:56320ef879a6 350 {
eencae 0:56320ef879a6 351 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 352 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 353
eencae 0:56320ef879a6 354 left_motor(speed);
eencae 0:56320ef879a6 355 right_motor(-speed);
eencae 0:56320ef879a6 356 }
eencae 0:56320ef879a6 357
eencae 0:56320ef879a6 358 void m3pi::spin_left(float speed)
eencae 0:56320ef879a6 359 {
eencae 0:56320ef879a6 360 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 361 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 362
eencae 0:56320ef879a6 363 left_motor(-speed);
eencae 0:56320ef879a6 364 right_motor(speed);
eencae 0:56320ef879a6 365 }
eencae 0:56320ef879a6 366
eencae 2:26bf14f4dc84 367 ////////////////////////////////////////////////////////////////////////////////
eencae 2:26bf14f4dc84 368
eencae 0:56320ef879a6 369 void m3pi::display_battery_voltage(int x,int y)
eencae 0:56320ef879a6 370 {
eencae 0:56320ef879a6 371 float voltage = get_battery_voltage();
eencae 0:56320ef879a6 372
eencae 0:56320ef879a6 373 char buffer[8];
eencae 0:56320ef879a6 374 sprintf(buffer,"%3.1f V",voltage);
eencae 0:56320ef879a6 375
eencae 0:56320ef879a6 376 lcd_goto_xy(x,y);
eencae 0:56320ef879a6 377 lcd_print(buffer,5);
eencae 0:56320ef879a6 378 }
eencae 0:56320ef879a6 379
eencae 0:56320ef879a6 380 void m3pi::display_signature(int x,int y)
eencae 0:56320ef879a6 381 {
wardelder 10:c3617fcf2c58 382 char buffer[7]; // including NULL terminator
wardelder 10:c3617fcf2c58 383
wardelder 10:c3617fcf2c58 384 //_serial->putc(0x81);
wardelder 10:c3617fcf2c58 385 buffer[0]=0x81;
wardelder 10:c3617fcf2c58 386 _serial->write(buffer,1);
wardelder 10:c3617fcf2c58 387
wardelder 10:c3617fcf2c58 388
wardelder 10:c3617fcf2c58 389 //_serial->gets(buffer,7);
wardelder 10:c3617fcf2c58 390 _serial->read(buffer,7);
eencae 0:56320ef879a6 391
eencae 0:56320ef879a6 392 lcd_goto_xy(x,y);
eencae 0:56320ef879a6 393 lcd_print(buffer,6);
eencae 0:56320ef879a6 394 }
eencae 0:56320ef879a6 395
eencae 2:26bf14f4dc84 396 void m3pi::display_sensor_values(unsigned int values[],int y)
eencae 2:26bf14f4dc84 397 {
eencae 2:26bf14f4dc84 398 // initialise array to ASCII '0'
eencae 4:0abe81f5d9fd 399 lcd_goto_xy(1,y);
wardelder 10:c3617fcf2c58 400
eencae 4:0abe81f5d9fd 401 char sensor_values[5];
wardelder 10:c3617fcf2c58 402
eencae 4:0abe81f5d9fd 403 // loop through sensor
eencae 4:0abe81f5d9fd 404 for (int sensor = 0 ; sensor < 5 ; sensor++) {
wardelder 10:c3617fcf2c58 405 // get the value and put it in the correct bin
eencae 4:0abe81f5d9fd 406 // (7 bins in the range 0 to 1000
wardelder 10:c3617fcf2c58 407 char value = char(values[sensor]/(1000.0/7.0));
eencae 4:0abe81f5d9fd 408 // use the bin to select the bar graph icon to display
eencae 4:0abe81f5d9fd 409 sensor_values[sensor] = _bar_graph[value];
wardelder 10:c3617fcf2c58 410 }
wardelder 10:c3617fcf2c58 411
eencae 4:0abe81f5d9fd 412 lcd_print(sensor_values,5);
eencae 2:26bf14f4dc84 413
eencae 2:26bf14f4dc84 414 }
eencae 2:26bf14f4dc84 415
wardelder 10:c3617fcf2c58 416 void m3pi::display_data()
eencae 9:074ce6197b51 417 {
wardelder 10:c3617fcf2c58 418 display_sensor_values(_values,1);
wardelder 10:c3617fcf2c58 419
wardelder 10:c3617fcf2c58 420 char buffer[8]= {0};
eencae 9:074ce6197b51 421 sprintf(buffer,"% .3f",_last_line_position);
eencae 9:074ce6197b51 422 lcd_goto_xy(0,0);
wardelder 10:c3617fcf2c58 423 lcd_print(buffer,6);
wardelder 10:c3617fcf2c58 424
eencae 9:074ce6197b51 425 }
eencae 9:074ce6197b51 426
eencae 2:26bf14f4dc84 427 unsigned int m3pi::get_sensor_array_value(unsigned int values[])
eencae 2:26bf14f4dc84 428 {
eencae 2:26bf14f4dc84 429 unsigned int value = 0;
eencae 2:26bf14f4dc84 430
eencae 2:26bf14f4dc84 431 // loop through each bit, starting from PC4
eencae 2:26bf14f4dc84 432 for (int i = 4; i >= 0; i--) {
eencae 2:26bf14f4dc84 433
eencae 2:26bf14f4dc84 434 unsigned int weight = pow(2.0,4-i);
eencae 2:26bf14f4dc84 435
eencae 2:26bf14f4dc84 436 // check if over threshold
eencae 2:26bf14f4dc84 437 if (values[i] > 500) {
eencae 2:26bf14f4dc84 438 // add equivalent binary weight to value
eencae 2:26bf14f4dc84 439 value += weight;
eencae 2:26bf14f4dc84 440 }
eencae 2:26bf14f4dc84 441
eencae 2:26bf14f4dc84 442 }
eencae 2:26bf14f4dc84 443
eencae 2:26bf14f4dc84 444 return value;
eencae 2:26bf14f4dc84 445 }
eencae 2:26bf14f4dc84 446
eencae 2:26bf14f4dc84 447 float m3pi::calc_line_position(unsigned int values[])
eencae 2:26bf14f4dc84 448 {
eencae 2:26bf14f4dc84 449 // calculate weighted average
eencae 2:26bf14f4dc84 450 unsigned int value =
eencae 2:26bf14f4dc84 451 (0*values[0]+1e3*values[1]+2e3*values[2]+3e3*values[3]+4e3*values[4])/
eencae 2:26bf14f4dc84 452 (values[0]+values[1]+values[2]+values[3]+values[4]);
eencae 2:26bf14f4dc84 453
eencae 2:26bf14f4dc84 454 // scale to between -1.0 and 1.0
eencae 2:26bf14f4dc84 455 float position = (int(value) - 2000)/2000.0;
eencae 2:26bf14f4dc84 456
eencae 2:26bf14f4dc84 457 float is_on_line = false;
eencae 2:26bf14f4dc84 458
eencae 2:26bf14f4dc84 459 // loop through and check if any sensor reading is above the threshold
eencae 2:26bf14f4dc84 460 for (int i = 0; i<5; i++) {
eencae 2:26bf14f4dc84 461 if (values[i] > 500) {
eencae 2:26bf14f4dc84 462 is_on_line = true;
eencae 2:26bf14f4dc84 463 }
eencae 2:26bf14f4dc84 464 }
eencae 2:26bf14f4dc84 465
eencae 2:26bf14f4dc84 466 // update last line position if over line
eencae 2:26bf14f4dc84 467 if (is_on_line) {
eencae 2:26bf14f4dc84 468 _last_line_position = position;
eencae 2:26bf14f4dc84 469 }
eencae 2:26bf14f4dc84 470
eencae 2:26bf14f4dc84 471 // if not on line then the last line position will have the last value when over line
eencae 2:26bf14f4dc84 472 return _last_line_position;
eencae 2:26bf14f4dc84 473 }
eencae 2:26bf14f4dc84 474
wardelder 10:c3617fcf2c58 475 float m3pi::read_line()
wardelder 10:c3617fcf2c58 476 {
wardelder 10:c3617fcf2c58 477 return calc_line_position(_values);
eencae 9:074ce6197b51 478 }
eencae 9:074ce6197b51 479
eencae 9:074ce6197b51 480
wardelder 10:c3617fcf2c58 481
eencae 0:56320ef879a6 482 /////////////////////////////// private methods ////////////////////////////////
eencae 0:56320ef879a6 483
eencae 0:56320ef879a6 484 void m3pi::reset()
eencae 0:56320ef879a6 485 {
eencae 0:56320ef879a6 486 // pulse the reset line (active-high)
eencae 3:5015bc2d1cf8 487 _reset->write(1);
wardelder 10:c3617fcf2c58 488 ThisThread::sleep_for(100ms);
eencae 0:56320ef879a6 489 _reset->write(0);
wardelder 10:c3617fcf2c58 490 ThisThread::sleep_for(100ms);
eencae 0:56320ef879a6 491 }