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@2:26bf14f4dc84, 2017-04-07 (annotated)

Committer:
eencae
Date:
Fri Apr 07 18:22:35 2017 +0000
Revision:
2:26bf14f4dc84
Parent:
1:5523d6d1feec
Child:
3:5015bc2d1cf8
Added local line calculating method.;

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 2:26bf14f4dc84 12
eencae 2:26bf14f4dc84 13 _last_line_position = 0.0;
eencae 2:26bf14f4dc84 14
eencae 0:56320ef879a6 15 }
eencae 0:56320ef879a6 16
eencae 0:56320ef879a6 17 m3pi::~m3pi()
eencae 0:56320ef879a6 18 {
eencae 0:56320ef879a6 19 delete _serial;
eencae 0:56320ef879a6 20 delete _reset;
eencae 0:56320ef879a6 21 delete _button;
eencae 0:56320ef879a6 22 delete _leds;
eencae 0:56320ef879a6 23 }
eencae 0:56320ef879a6 24
eencae 0:56320ef879a6 25 /////////////////////////////// public methods /////////////////////////////////
eencae 0:56320ef879a6 26
eencae 0:56320ef879a6 27 void m3pi::write_leds(int val)
eencae 0:56320ef879a6 28 {
eencae 0:56320ef879a6 29 // check within limits
eencae 0:56320ef879a6 30 val = val > 255 ? 255 : val;
eencae 0:56320ef879a6 31 val = val < 0 ? 0 : val;
eencae 0:56320ef879a6 32
eencae 0:56320ef879a6 33 _leds->write(val);
eencae 0:56320ef879a6 34 }
eencae 0:56320ef879a6 35
eencae 0:56320ef879a6 36 void m3pi::init()
eencae 0:56320ef879a6 37 {
eencae 0:56320ef879a6 38 _serial->baud(115200);
eencae 0:56320ef879a6 39 reset(); // hard rest of 3pi
eencae 0:56320ef879a6 40 stop(); // stop motors
eencae 0:56320ef879a6 41 lcd_clear(); // clear LCD
eencae 0:56320ef879a6 42 write_leds(0); // turn off LEDs
eencae 0:56320ef879a6 43 _button->mode(PullUp); // turn pull-up on
eencae 0:56320ef879a6 44 }
eencae 0:56320ef879a6 45
eencae 2:26bf14f4dc84 46 /////////////////////////////// serial slave commands ////////////////////////////////
eencae 2:26bf14f4dc84 47
eencae 2:26bf14f4dc84 48
eencae 0:56320ef879a6 49 void m3pi::get_signature(char *signature)
eencae 0:56320ef879a6 50 {
eencae 0:56320ef879a6 51 _serial->putc(0x81);
eencae 0:56320ef879a6 52 _serial->gets(signature,7);
eencae 0:56320ef879a6 53 }
eencae 0:56320ef879a6 54
eencae 0:56320ef879a6 55 void m3pi::get_raw_values(unsigned int *values)
eencae 0:56320ef879a6 56 {
eencae 1:5523d6d1feec 57 char vals[10]; // array to receive 10 byte return message
eencae 1:5523d6d1feec 58 _serial->putc(0x86); // send command
eencae 0:56320ef879a6 59
eencae 1:5523d6d1feec 60 int n=0;
eencae 1:5523d6d1feec 61 while ( _serial->readable() ) { // keep looping while data on rx line
eencae 1:5523d6d1feec 62 vals[n] = _serial->getc(); // read into array
eencae 1:5523d6d1feec 63 n++; // increment index
eencae 1:5523d6d1feec 64 }
eencae 1:5523d6d1feec 65
eencae 1:5523d6d1feec 66 for(int i=0; i<5; i++) { // construct the 2-byte values
eencae 1:5523d6d1feec 67 values[i] = (vals[2*i+1] << 8) | vals[2*i];
eencae 0:56320ef879a6 68 }
eencae 0:56320ef879a6 69 }
eencae 0:56320ef879a6 70
eencae 0:56320ef879a6 71 void m3pi::get_calibrated_values(unsigned int *values)
eencae 0:56320ef879a6 72 {
eencae 1:5523d6d1feec 73 char vals[10]; // array to receive 10 byte return message
eencae 1:5523d6d1feec 74 _serial->putc(0x87); // send command
eencae 0:56320ef879a6 75
eencae 1:5523d6d1feec 76 int n=0;
eencae 1:5523d6d1feec 77 while ( _serial->readable() ) { // keep looping while data on rx line
eencae 1:5523d6d1feec 78 vals[n] = _serial->getc(); // read into array
eencae 1:5523d6d1feec 79 n++; // increment index
eencae 0:56320ef879a6 80 }
eencae 0:56320ef879a6 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 0:56320ef879a6 86 }
eencae 0:56320ef879a6 87
eencae 0:56320ef879a6 88 float m3pi::get_trimpot_value()
eencae 0:56320ef879a6 89 {
eencae 0:56320ef879a6 90 _serial->putc(0xB0);
eencae 0:56320ef879a6 91 char lsb = _serial->getc();
eencae 0:56320ef879a6 92 char msb = _serial->getc();
eencae 0:56320ef879a6 93 // trimpot value in the range 0 - 1023
eencae 0:56320ef879a6 94 float value = ( msb<<8 | lsb ) / 1023.0;
eencae 0:56320ef879a6 95 return value;
eencae 0:56320ef879a6 96 }
eencae 0:56320ef879a6 97
eencae 0:56320ef879a6 98
eencae 0:56320ef879a6 99 float m3pi::get_battery_voltage()
eencae 0:56320ef879a6 100 {
eencae 0:56320ef879a6 101 _serial->putc(0xB1);
eencae 0:56320ef879a6 102 char lsb = _serial->getc();
eencae 0:56320ef879a6 103 char msb = _serial->getc();
eencae 0:56320ef879a6 104 // Battery in mV so convert to volts
eencae 0:56320ef879a6 105 float voltage = ( msb<<8 | lsb ) / 1000.0;
eencae 0:56320ef879a6 106 return voltage;
eencae 0:56320ef879a6 107 }
eencae 0:56320ef879a6 108
eencae 0:56320ef879a6 109 void m3pi::play_music(const char notes[],int length)
eencae 0:56320ef879a6 110 {
eencae 0:56320ef879a6 111 length = length < 0 ? 0 : length;
eencae 0:56320ef879a6 112 length = length > 100 ? 100 : length;
eencae 0:56320ef879a6 113
eencae 0:56320ef879a6 114 _serial->putc(0xB3);
eencae 0:56320ef879a6 115 _serial->putc(length);
eencae 0:56320ef879a6 116
eencae 0:56320ef879a6 117 for (int i = 0 ; i < length ; i++) {
eencae 0:56320ef879a6 118 _serial->putc(notes[i]);
eencae 0:56320ef879a6 119 }
eencae 0:56320ef879a6 120 }
eencae 0:56320ef879a6 121
eencae 0:56320ef879a6 122
eencae 0:56320ef879a6 123 void m3pi::calibrate()
eencae 0:56320ef879a6 124 {
eencae 0:56320ef879a6 125 reset_calibration();
eencae 0:56320ef879a6 126
eencae 0:56320ef879a6 127 lcd_goto_xy(0,0);
eencae 0:56320ef879a6 128 lcd_print("Place on",8);
eencae 0:56320ef879a6 129 lcd_goto_xy(0,1);
eencae 0:56320ef879a6 130 lcd_print(" line ",8);
eencae 0:56320ef879a6 131
eencae 2:26bf14f4dc84 132 wait(0.5);
eencae 0:56320ef879a6 133
eencae 0:56320ef879a6 134 lcd_clear();
eencae 0:56320ef879a6 135 lcd_goto_xy(0,0);
eencae 0:56320ef879a6 136 lcd_print(" Press ",8);
eencae 0:56320ef879a6 137 lcd_goto_xy(0,1);
eencae 0:56320ef879a6 138 lcd_print("to begin",8);
eencae 0:56320ef879a6 139
eencae 0:56320ef879a6 140 while( read_button() ) {
eencae 0:56320ef879a6 141 // loop while waiting for button to be press
eencae 0:56320ef879a6 142 }
eencae 0:56320ef879a6 143
eencae 2:26bf14f4dc84 144 wait(0.5);
eencae 0:56320ef879a6 145
eencae 0:56320ef879a6 146 lcd_clear();
eencae 0:56320ef879a6 147 lcd_goto_xy(0,0);
eencae 0:56320ef879a6 148 lcd_print("Reading ",8);
eencae 0:56320ef879a6 149 lcd_goto_xy(0,1);
eencae 0:56320ef879a6 150 lcd_print("Sensors ",8);
eencae 0:56320ef879a6 151
eencae 1:5523d6d1feec 152 spin_right(0.2);
eencae 0:56320ef879a6 153
eencae 0:56320ef879a6 154 char led_val = 0;
eencae 0:56320ef879a6 155 Timer timer;
eencae 0:56320ef879a6 156 timer.start();
eencae 0:56320ef879a6 157
eencae 2:26bf14f4dc84 158 while (timer.read() < 5.0) {
eencae 2:26bf14f4dc84 159
eencae 0:56320ef879a6 160 write_leds(led_val++);
eencae 0:56320ef879a6 161
eencae 0:56320ef879a6 162 if (led_val > 255) {
eencae 0:56320ef879a6 163 led_val = 0;
eencae 0:56320ef879a6 164 }
eencae 0:56320ef879a6 165
eencae 0:56320ef879a6 166 _serial->putc(0xB4);
eencae 1:5523d6d1feec 167 wait_ms(25);
eencae 0:56320ef879a6 168 }
eencae 0:56320ef879a6 169
eencae 0:56320ef879a6 170 timer.stop();
eencae 0:56320ef879a6 171
eencae 0:56320ef879a6 172 write_leds(255);
eencae 0:56320ef879a6 173 stop();
eencae 0:56320ef879a6 174
eencae 0:56320ef879a6 175 lcd_clear();
eencae 0:56320ef879a6 176 lcd_goto_xy(0,0);
eencae 1:5523d6d1feec 177 lcd_print(" Done ",8);
eencae 0:56320ef879a6 178
eencae 0:56320ef879a6 179 while( read_button() ) {
eencae 0:56320ef879a6 180 // loop while waiting for button to be press
eencae 0:56320ef879a6 181 }
eencae 0:56320ef879a6 182
eencae 0:56320ef879a6 183 lcd_clear();
eencae 0:56320ef879a6 184 write_leds(0);
eencae 2:26bf14f4dc84 185 wait(0.5);
eencae 0:56320ef879a6 186 }
eencae 0:56320ef879a6 187
eencae 0:56320ef879a6 188 void m3pi::reset_calibration()
eencae 0:56320ef879a6 189 {
eencae 0:56320ef879a6 190 _serial->putc(0xB5);
eencae 0:56320ef879a6 191 wait_ms(50);
eencae 0:56320ef879a6 192 }
eencae 0:56320ef879a6 193
eencae 2:26bf14f4dc84 194 float m3pi::get_line_position()
eencae 0:56320ef879a6 195 {
eencae 0:56320ef879a6 196 _serial->putc(0xB6);
eencae 0:56320ef879a6 197
eencae 0:56320ef879a6 198 char lsb = _serial->getc();
eencae 0:56320ef879a6 199 char msb = _serial->getc();
eencae 0:56320ef879a6 200 int position = (msb<<8 | lsb);
eencae 0:56320ef879a6 201
eencae 2:26bf14f4dc84 202 return float(position - 2000)/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 2:26bf14f4dc84 245 /////////////////////////////// motor methods ////////////////////////////////
eencae 2:26bf14f4dc84 246
eencae 2:26bf14f4dc84 247
eencae 0:56320ef879a6 248 void m3pi::left_motor(float speed)
eencae 0:56320ef879a6 249 {
eencae 0:56320ef879a6 250 // check within bounds
eencae 0:56320ef879a6 251 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 252 speed = speed < -1.0 ? -1.0 : speed;
eencae 0:56320ef879a6 253
eencae 0:56320ef879a6 254 if (speed > 0.0) { // forward
eencae 0:56320ef879a6 255 _serial->putc(0xC1);
eencae 0:56320ef879a6 256 char s = char(127.0*speed);
eencae 0:56320ef879a6 257 _serial->putc(s);
eencae 0:56320ef879a6 258 } else { // backward - speed is negative
eencae 0:56320ef879a6 259 _serial->putc(0xC2);
eencae 0:56320ef879a6 260 char s = char(-127.0*speed);
eencae 0:56320ef879a6 261 _serial->putc(s);
eencae 0:56320ef879a6 262 }
eencae 0:56320ef879a6 263
eencae 0:56320ef879a6 264 }
eencae 0:56320ef879a6 265
eencae 0:56320ef879a6 266 void m3pi::right_motor(float speed)
eencae 0:56320ef879a6 267 {
eencae 0:56320ef879a6 268 // check within bounds
eencae 0:56320ef879a6 269 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 270 speed = speed < -1.0 ? -1.0 : speed;
eencae 0:56320ef879a6 271
eencae 0:56320ef879a6 272 if (speed > 0.0) { // forward
eencae 0:56320ef879a6 273 _serial->putc(0xC5);
eencae 0:56320ef879a6 274 char s = char(127.0*speed);
eencae 0:56320ef879a6 275 _serial->putc(s);
eencae 0:56320ef879a6 276 } else { // backward - speed is negative
eencae 0:56320ef879a6 277 _serial->putc(0xC6);
eencae 0:56320ef879a6 278 char s = char(-127.0*speed);
eencae 0:56320ef879a6 279 _serial->putc(s);
eencae 0:56320ef879a6 280 }
eencae 0:56320ef879a6 281
eencae 0:56320ef879a6 282 }
eencae 0:56320ef879a6 283
eencae 0:56320ef879a6 284 // speeds from -1.0 to 1.0 (0 is stop)
eencae 0:56320ef879a6 285 void m3pi::motors(float left_speed,float right_speed)
eencae 0:56320ef879a6 286 {
eencae 0:56320ef879a6 287 left_motor(left_speed);
eencae 0:56320ef879a6 288 right_motor(right_speed);
eencae 0:56320ef879a6 289 }
eencae 0:56320ef879a6 290
eencae 0:56320ef879a6 291
eencae 0:56320ef879a6 292 void m3pi::stop()
eencae 0:56320ef879a6 293 {
eencae 0:56320ef879a6 294 left_motor(0.0);
eencae 0:56320ef879a6 295 right_motor(0.0);
eencae 0:56320ef879a6 296 }
eencae 0:56320ef879a6 297
eencae 0:56320ef879a6 298 // speed in range 0.0 to 1.0
eencae 0:56320ef879a6 299 void m3pi::forward(float speed)
eencae 0:56320ef879a6 300 {
eencae 0:56320ef879a6 301 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 302 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 303
eencae 0:56320ef879a6 304 left_motor(speed);
eencae 0:56320ef879a6 305 right_motor(speed);
eencae 0:56320ef879a6 306 }
eencae 0:56320ef879a6 307
eencae 0:56320ef879a6 308 // speed in range 0 to 1.0
eencae 0:56320ef879a6 309 void m3pi::reverse(float speed)
eencae 0:56320ef879a6 310 {
eencae 0:56320ef879a6 311 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 312 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 313
eencae 0:56320ef879a6 314 left_motor(-speed);
eencae 0:56320ef879a6 315 right_motor(-speed);
eencae 0:56320ef879a6 316 }
eencae 0:56320ef879a6 317
eencae 0:56320ef879a6 318 void m3pi::spin_right(float speed)
eencae 0:56320ef879a6 319 {
eencae 0:56320ef879a6 320 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 321 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 322
eencae 0:56320ef879a6 323 left_motor(speed);
eencae 0:56320ef879a6 324 right_motor(-speed);
eencae 0:56320ef879a6 325 }
eencae 0:56320ef879a6 326
eencae 0:56320ef879a6 327 void m3pi::spin_left(float speed)
eencae 0:56320ef879a6 328 {
eencae 0:56320ef879a6 329 speed = speed > 1.0 ? 1.0 : speed;
eencae 0:56320ef879a6 330 speed = speed < 0.0 ? 0.0 : speed;
eencae 0:56320ef879a6 331
eencae 0:56320ef879a6 332 left_motor(-speed);
eencae 0:56320ef879a6 333 right_motor(speed);
eencae 0:56320ef879a6 334 }
eencae 0:56320ef879a6 335
eencae 2:26bf14f4dc84 336 ////////////////////////////////////////////////////////////////////////////////
eencae 2:26bf14f4dc84 337
eencae 2:26bf14f4dc84 338
eencae 0:56320ef879a6 339 int m3pi::read_button()
eencae 0:56320ef879a6 340 {
eencae 0:56320ef879a6 341 return _button->read();
eencae 0:56320ef879a6 342 }
eencae 0:56320ef879a6 343
eencae 0:56320ef879a6 344 void m3pi::display_battery_voltage(int x,int y)
eencae 0:56320ef879a6 345 {
eencae 0:56320ef879a6 346 float voltage = get_battery_voltage();
eencae 0:56320ef879a6 347
eencae 0:56320ef879a6 348 char buffer[8];
eencae 0:56320ef879a6 349 sprintf(buffer,"%3.1f V",voltage);
eencae 0:56320ef879a6 350
eencae 0:56320ef879a6 351 lcd_goto_xy(x,y);
eencae 0:56320ef879a6 352 lcd_print(buffer,5);
eencae 0:56320ef879a6 353 }
eencae 0:56320ef879a6 354
eencae 0:56320ef879a6 355 void m3pi::display_signature(int x,int y)
eencae 0:56320ef879a6 356 {
eencae 0:56320ef879a6 357 _serial->putc(0x81);
eencae 0:56320ef879a6 358 char buffer[7]; // including NULL terminator
eencae 0:56320ef879a6 359 _serial->gets(buffer,7);
eencae 0:56320ef879a6 360
eencae 0:56320ef879a6 361 lcd_goto_xy(x,y);
eencae 0:56320ef879a6 362 lcd_print(buffer,6);
eencae 0:56320ef879a6 363 }
eencae 0:56320ef879a6 364
eencae 2:26bf14f4dc84 365 void m3pi::display_sensor_values(unsigned int values[],int y)
eencae 2:26bf14f4dc84 366 {
eencae 2:26bf14f4dc84 367 // initialise array to ASCII '0'
eencae 2:26bf14f4dc84 368 char bin[5]= {'0','0','0','0','0'};
eencae 2:26bf14f4dc84 369
eencae 2:26bf14f4dc84 370 // loop through and if above threshold then sent to ASCII '1'
eencae 2:26bf14f4dc84 371 for (int i=0; i<5; i++) {
eencae 2:26bf14f4dc84 372 if (values[i] > 500) {
eencae 2:26bf14f4dc84 373 bin[i] = '1';
eencae 2:26bf14f4dc84 374 }
eencae 2:26bf14f4dc84 375 }
eencae 2:26bf14f4dc84 376
eencae 2:26bf14f4dc84 377 lcd_goto_xy(2,y);
eencae 2:26bf14f4dc84 378 lcd_print(bin,5);
eencae 2:26bf14f4dc84 379
eencae 2:26bf14f4dc84 380 }
eencae 2:26bf14f4dc84 381
eencae 2:26bf14f4dc84 382 unsigned int m3pi::get_sensor_array_value(unsigned int values[])
eencae 2:26bf14f4dc84 383 {
eencae 2:26bf14f4dc84 384
eencae 2:26bf14f4dc84 385 unsigned int value = 0;
eencae 2:26bf14f4dc84 386
eencae 2:26bf14f4dc84 387 // loop through each bit, starting from PC4
eencae 2:26bf14f4dc84 388 for (int i = 4; i >= 0; i--) {
eencae 2:26bf14f4dc84 389
eencae 2:26bf14f4dc84 390 unsigned int weight = pow(2.0,4-i);
eencae 2:26bf14f4dc84 391
eencae 2:26bf14f4dc84 392 // check if over threshold
eencae 2:26bf14f4dc84 393 if (values[i] > 500) {
eencae 2:26bf14f4dc84 394 // add equivalent binary weight to value
eencae 2:26bf14f4dc84 395 value += weight;
eencae 2:26bf14f4dc84 396 }
eencae 2:26bf14f4dc84 397
eencae 2:26bf14f4dc84 398 }
eencae 2:26bf14f4dc84 399
eencae 2:26bf14f4dc84 400 return value;
eencae 2:26bf14f4dc84 401 }
eencae 2:26bf14f4dc84 402
eencae 2:26bf14f4dc84 403 float m3pi::calc_line_position(unsigned int values[])
eencae 2:26bf14f4dc84 404 {
eencae 2:26bf14f4dc84 405 // calculate weighted average
eencae 2:26bf14f4dc84 406 unsigned int value =
eencae 2:26bf14f4dc84 407 (0*values[0]+1e3*values[1]+2e3*values[2]+3e3*values[3]+4e3*values[4])/
eencae 2:26bf14f4dc84 408 (values[0]+values[1]+values[2]+values[3]+values[4]);
eencae 2:26bf14f4dc84 409
eencae 2:26bf14f4dc84 410 // scale to between -1.0 and 1.0
eencae 2:26bf14f4dc84 411 float position = (int(value) - 2000)/2000.0;
eencae 2:26bf14f4dc84 412
eencae 2:26bf14f4dc84 413 float is_on_line = false;
eencae 2:26bf14f4dc84 414 write_leds(0x0);
eencae 2:26bf14f4dc84 415
eencae 2:26bf14f4dc84 416 // loop through and check if any sensor reading is above the threshold
eencae 2:26bf14f4dc84 417 for (int i = 0; i<5; i++) {
eencae 2:26bf14f4dc84 418 if (values[i] > 500) {
eencae 2:26bf14f4dc84 419 is_on_line = true;
eencae 2:26bf14f4dc84 420 write_leds(0xFF);
eencae 2:26bf14f4dc84 421 }
eencae 2:26bf14f4dc84 422 }
eencae 2:26bf14f4dc84 423
eencae 2:26bf14f4dc84 424 // update last line position if over line
eencae 2:26bf14f4dc84 425 if (is_on_line) {
eencae 2:26bf14f4dc84 426 _last_line_position = position;
eencae 2:26bf14f4dc84 427 }
eencae 2:26bf14f4dc84 428
eencae 2:26bf14f4dc84 429 // if not on line then the last line position will have the last value when over line
eencae 2:26bf14f4dc84 430 return _last_line_position;
eencae 2:26bf14f4dc84 431 }
eencae 2:26bf14f4dc84 432
eencae 0:56320ef879a6 433 /////////////////////////////// private methods ////////////////////////////////
eencae 0:56320ef879a6 434
eencae 0:56320ef879a6 435 void m3pi::reset()
eencae 0:56320ef879a6 436 {
eencae 0:56320ef879a6 437 // pulse the reset line (active-high)
eencae 0:56320ef879a6 438 _reset->write(0);
eencae 0:56320ef879a6 439 wait_ms(100);
eencae 0:56320ef879a6 440 _reset->write(1);
eencae 0:56320ef879a6 441 wait_ms(100);
eencae 0:56320ef879a6 442 }