Diff: m3pi.cpp

Revision:

2:26bf14f4dc84

Parent:

1:5523d6d1feec

Child:

3:5015bc2d1cf8

--- a/m3pi.cpp	Wed Apr 05 10:25:42 2017 +0000
+++ b/m3pi.cpp	Fri Apr 07 18:22:35 2017 +0000
@@ -9,6 +9,9 @@
     _reset = new DigitalOut(p23);
     _button = new DigitalIn(p21);
     _leds = new BusOut(p20,p19,p18,p17,p16,p15,p14,p13);
+
+    _last_line_position = 0.0;
+
 }
 
 m3pi::~m3pi()
@@ -40,6 +43,9 @@
     _button->mode(PullUp);           // turn pull-up on
 }
 
+/////////////////////////////// serial slave commands ////////////////////////////////
+
+
 void m3pi::get_signature(char *signature)
 {
     _serial->putc(0x81);
@@ -76,7 +82,7 @@
     for(int i=0; i<5; i++) {            // construct the 2-byte values
         values[i] = (vals[2*i+1] << 8) | vals[2*i];
     }
-    
+
 }
 
 float m3pi::get_trimpot_value()
@@ -123,7 +129,7 @@
     lcd_goto_xy(0,1);
     lcd_print("  line  ",8);
 
-    wait(1.0);
+    wait(0.5);
 
     lcd_clear();
     lcd_goto_xy(0,0);
@@ -135,7 +141,7 @@
         // loop while waiting for button to be press
     }
 
-    wait(1.0);
+    wait(0.5);
 
     lcd_clear();
     lcd_goto_xy(0,0);
@@ -149,7 +155,8 @@
     Timer timer;
     timer.start();
 
-    while (timer.read() < 10.0) {
+    while (timer.read() < 5.0) {
+
         write_leds(led_val++);
 
         if (led_val > 255) {
@@ -175,7 +182,7 @@
 
     lcd_clear();
     write_leds(0);
-    wait(1.0);
+    wait(0.5);
 }
 
 void m3pi::reset_calibration()
@@ -184,22 +191,15 @@
     wait_ms(50);
 }
 
-int m3pi::get_line_position()
+float m3pi::get_line_position()
 {
     _serial->putc(0xB6);
 
-    // order is different from claimed in User Guide?
     char lsb = _serial->getc();
     char msb = _serial->getc();
     int position = (msb<<8 | lsb);
 
-    return position - 2000;
-}
-
-float m3pi::get_normalised_line_position()
-{
-    int position = get_line_position();
-    return float(position)/2000.0;
+    return float(position - 2000)/2000.0;
 }
 
 void m3pi::lcd_clear()
@@ -242,6 +242,9 @@
     write_leds(0);    // LEDs off
 }
 
+/////////////////////////////// motor methods ////////////////////////////////
+
+
 void m3pi::left_motor(float speed)
 {
     // check within bounds
@@ -314,7 +317,6 @@
 
 void m3pi::spin_right(float speed)
 {
-
     speed = speed > 1.0 ? 1.0 : speed;
     speed = speed < 0.0 ? 0.0 : speed;
 
@@ -324,7 +326,6 @@
 
 void m3pi::spin_left(float speed)
 {
-
     speed = speed > 1.0 ? 1.0 : speed;
     speed = speed < 0.0 ? 0.0 : speed;
 
@@ -332,6 +333,9 @@
     right_motor(speed);
 }
 
+////////////////////////////////////////////////////////////////////////////////
+
+
 int m3pi::read_button()
 {
     return _button->read();
@@ -358,6 +362,74 @@
     lcd_print(buffer,6);
 }
 
+void m3pi::display_sensor_values(unsigned int values[],int y)
+{
+    // initialise array to ASCII '0'
+    char bin[5]= {'0','0','0','0','0'};
+
+    // loop through and if above threshold then sent to ASCII '1'
+    for (int i=0; i<5; i++) {
+        if (values[i] > 500) {
+            bin[i] = '1';
+        }
+    }
+
+    lcd_goto_xy(2,y);
+    lcd_print(bin,5);
+
+}
+
+unsigned int m3pi::get_sensor_array_value(unsigned int values[])
+{
+
+    unsigned int value = 0;
+
+    // loop through each bit, starting from PC4
+    for (int i = 4; i >= 0; i--) {
+
+        unsigned int weight = pow(2.0,4-i);
+
+        // check if over threshold
+        if (values[i] > 500) {
+            // add equivalent binary weight to value
+            value += weight;
+        }
+
+    }
+
+    return value;
+}
+
+float m3pi::calc_line_position(unsigned int values[])
+{
+    // calculate weighted average
+    unsigned int value =
+        (0*values[0]+1e3*values[1]+2e3*values[2]+3e3*values[3]+4e3*values[4])/
+        (values[0]+values[1]+values[2]+values[3]+values[4]);
+
+    // scale to between -1.0 and 1.0
+    float position = (int(value) - 2000)/2000.0;
+
+    float is_on_line = false;
+    write_leds(0x0);
+
+    // loop through and check if any sensor reading is above the threshold
+    for (int i = 0; i<5; i++) {
+        if (values[i] > 500) {
+            is_on_line = true;
+            write_leds(0xFF);
+        }
+    }
+
+    // update last line position if over line
+    if (is_on_line) {
+        _last_line_position = position;
+    }
+
+    // if not on line then the last line position will have the last value when over line
+    return _last_line_position;
+}
+
 /////////////////////////////// private methods ////////////////////////////////
 
 void m3pi::reset()