Diff: main.cpp

Revision:

4:453d534b1c1d

Parent:

2:5f9b78950a17

Child:

5:f9837617817b

diff -r 247358a782f1 -r 453d534b1c1d main.cpp
--- a/main.cpp	Sat Nov 14 00:00:15 2015 +0000
+++ b/main.cpp	Sat Nov 14 00:15:41 2015 +0000
@@ -1,27 +1,30 @@
+//Micromouse code
 #include "mbed.h"
-#include "QEI.h"
+#include "AVEncoder.h"
 
+// set things
+Serial pc(SERIAL_TX, SERIAL_RX);
+Ticker Systicker;
+Timer timer;
+
+PwmOut motor1_forward(PB_10);
 PwmOut motor1_reverse(PA_6);
-PwmOut motor1_forward(PB_10);
 PwmOut motor2_forward(PA_7);
 PwmOut motor2_reverse(PB_6);
 
-//DigitalIn encoder1A(PA_15);
-//DigitalIn encoder1B(PB_3);
-//DigitalIn encoder2A(PA_1);
-//DigitalIn encoder2B(PA_10); 
+// TODO: change our encoder pins from AnalogIn into:
+// otherwise, we can also use the AVEncoder thing as well.  
+AVEncoder l_enco(PA_15, PB_3);
+AVEncoder r_enco(PA_1, PA_10);
 
-
-
-//Gyro
+// gyro
 AnalogIn _gyro(PA_0);
-AnalogIn gyro_cal(PC_1); //no
+// AnalogIn gyro_cal(PC_1) ?? currently this isn't connected. 
 
 //Left Front IR
 DigitalOut eLF(PC_3);
 AnalogIn rLF(PC_0);
                                                     //PC_4 is an ADC
-
 //Left Side IR
 DigitalOut eLS(PC_2);
 AnalogIn rLS(PC_1);
@@ -36,132 +39,230 @@
 
 DigitalOut myled(LED1);
 
-QEI LENC (PA_15, PB_3, NC, 100 );
-QEI RENC (PA_1, PA_10, NC, 100 );
+// global variables. 
+volatile float gyro_offset = 0;
+
+volatile float line_speed = 1; // currently is in terms of encoder pulses / ms. 
+volatile float angl_speed = 0; // should not turn while moving forward.
+
+volatile float line_prevError = 0;
+volatile int   enco_prevError = 0;
+volatile float gyro_prevError = 0;
 
-int distance = 0;
+volatile float line_accumulator = 0;
+volatile float line_decayFactor = 1.5;
+volatile float enco_accumulator = 0;
+volatile float enco_decayFactor = 1.5;
+volatile float gyro_accumulator = 0;
+volatile float gyro_decayFactor = 1.5;
+
+volatile float left_speed = 0;
+volatile float right_speed = 0;
 
-void rightForward(float speed);
-void leftForward(float speed);
+volatile unsigned long l_pulses = 0;
+volatile unsigned long r_pulses = 0;
 
-float read_gyro( void );
+// pid constants. these need to be tuned. but i set them as a default val for now.
+// line refers to the translational speed. 
+// enco and gyro will be used primarily for angular speed. 
+// (we can change the names later, 
+// i added line in after i realized that i already had the angular code)
+const float line_propo = 1;
+const float line_integ = 1;
+const float line_deriv = 1;
 
-Serial pc (SERIAL_TX, SERIAL_RX);
+const float gyro_propo = 1;
+const float gyro_integ = 1;
+const float gyro_deriv = 1;
+
+const float enco_propo = 1;
+const float enco_integ = 1;
+const float enco_deriv = 1;
+
+const float spin_enco_weight = 0.5;
+const float spin_gyro_weight = 1 - spin_enco_weight;
 
-int main() {
-   // LENC.reset();
-   // RENC.reset();
-   eRS = 0;
-   eRF = 0;
-   eLS = 0;
-   eLF = 0;
-        
-    myled = 1;
-    while(1) {
-        
-        motor1_reverse = 0;
-        motor1_forward = 0;
-    
-        motor2_forward = 0;
-        motor2_reverse = 0;
-        
-        eRS = 0;
-        eRF = 0;
-        eLS = 0;
-        eLF = 0;
-        
-        pc.printf("LEDS off\r\n");
-        wait(1);
-        pc.printf("Left Front: \t%f\r\n", rLF.read()); 
-        pc.printf("Left Side: \t%f\r\n", rLS.read()); 
-        pc.printf("Right Front: \t%f\r\n", rRS.read()); 
-        pc.printf("Right Side: \t%f\r\n\n", rRF.read()); 
-        wait(.5);    
-        
-        eRS = 1;
-        eRF = 1;
-        eLS = 1;
-        eLF = 1;
-        
-        pc.printf("LEDS on\r\n");
-        wait(1);
-        pc.printf("Left Front: \t%f\r\n", rLF.read()); 
-        pc.printf("Left Side: \t%f\r\n", rLS.read()); 
-        pc.printf("Right Front: \t%f\r\n", rRS.read()); 
-        pc.printf("Right Side: \t%f\r\n\n", rRF.read());     
-      //  pc.printf("%f\r\n", _gyro.read());  
-//         wait(0.5);
-    }
+// this is just so that we can maintain what state our mouse is in. 
+// currently this has no real use, but it may in the future. 
+// or we could just remove this entirely. 
+typedef enum 
+{
+    STOPPED, 
+    FORWARD, 
+    TURNING, 
+    UNKNOWN
+} STATE;
+STATE mouse_state;
+
+// helper functions
+void reset();
+void offsetCalc();
+void stop();
+void moveForward();
+void turn();
 
-    
-     //   int in = encoder1A.read();
-    //    if(in==1)
-   // myled=1;
-   // else
-   // myled=0;
+// interrupt stuff. 
+void incLENC()
+{
+    l_pulses++;
+}
+
+void incRENC()
+{
+    r_pulses++;
+}
 
-    //}
-    //
-//    motor1_reverse = 1.0;
-//    motor1_forward = 1.0;
-//    
-//    motor2_forward = 1.0;
-//    motor2_reverse = 1.0;
-    //rightForward(0.5);
-    //leftForward(-0.5);
-}
- 
- 
- float read_gyro ( void ){
-    // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset 
-    float r_gyro = _gyro.read();  // Get the Analog value from the STM32
-    r_gyro *= gyro_cal;     // NB 3.4 seemed to work better that 3.3 but better to run calibration routine
-    r_gyro = 1.5 - r_gyro;  // 1.5V off-set, invert sign
-    r_gyro *= 1492.5;       // Convert to DPS (+ve = clockwise, -ve = c.clockwise) 
-    return r_gyro;
+// PID Control
+// this contains the simplistic PID control for the most part. 
+// we do have to calibrate constants though. 
+void systick()
+{
+    if ( mouse_state == STOPPED || mouse_state == UNKNOWN )
+    {
+        // do nothing? 
+        // reset?
+        reset();
+        offsetCalc();
+        return;
     }
     
-void moveForward(float speed)
+    int dt = timer.read_us(); // should be around 1 ms.
+    timer.reset();
+    
+    float line_error = line_speed * dt - 0.5 * ( l_enco.getPulses() - r_enco.getPulses());
+    int enco_error = l_enco.getPulses() - r_enco.getPulses();
+    float gyro_error = _gyro.read() - gyro_offset;
+    
+    line_accumulator += line_error;
+    enco_accumulator += enco_error;
+    gyro_accumulator += gyro_error;
+    
+    float line_pid = 0;
+    line_pid += line_propo * line_error;
+    line_pid += line_integ * line_accumulator;
+    line_pid += line_deriv * (line_error - line_prevError)/dt;
+    
+    float enco_pid = 0;
+    enco_pid += enco_propo * enco_error;
+    enco_pid += enco_integ * enco_accumulator;
+    enco_pid += enco_deriv * (enco_error - enco_prevError)/dt;
+    
+    float gyro_pid = 0;
+    gyro_pid += gyro_propo * gyro_error;
+    gyro_pid += gyro_integ * gyro_accumulator;
+    gyro_pid += gyro_deriv * (gyro_error - gyro_prevError)/dt;
+        
+    // forward moving pid control. 
+    if ( mouse_state == FORWARD )
+    {
+        float x_error = line_pid;
+        float w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
+        left_speed += x_error + w_error;
+        right_speed += x_error - w_error;
+        // offsetCalc();
+    }
+    if ( mouse_state == TURNING )
+    {
+        // nothing for now. if we turn in place, we assume no pid control.
+        // this may have to change when we try curve turns.
+    }
+    
+    line_prevError = line_error;
+    enco_prevError = enco_error;
+    gyro_prevError = gyro_error;
+    
+    line_accumulator /= line_decayFactor;
+    enco_accumulator /= enco_decayFactor;
+    gyro_accumulator /= gyro_decayFactor;
+}
+
+// setup stuff. 
+void setup()
 {
-    motor1_forward = speed;
-    motor2_forward = speed;
+    mouse_state = STOPPED;
+    
+    eRS = 0;
+    eRF = 0;
+    eLS = 0;
+    eLF = 0;
+    
+    myled = 1;
     
-    motor1_reverse = 0;
-    motor2_reverse = 0;
+    // repeat this for some time frame. 
+    offsetCalc();
+    Systicker.attach_us(&systick, 1000);
+}
+
+void reset()
+{
+    l_pulses = 0;
+    r_pulses = 0;
 }
 
 
+// computes gyro_offset
+// uses a "weighted" average. 
+// idea is that the current gyro offset is weighted more than previous ones. 
+    // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n). 
+    // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
+    // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
+// currently this is only in the setup function. we can run this when the mouse is running in a line
+// when we figure out good line running pid.
+void offsetCalc()
+{
+    gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
+}
 
-void rightForward(float speed) {
-    if (speed == 0) {
-        motor1_forward = 1.0;
-        motor1_reverse = 1.0;
-    }
+
     
-    if (speed > 0) {
-        motor1_forward = speed;
-        motor1_reverse = 0;
-    }
-    
-    else {
-        motor1_forward = 0;
-        motor1_reverse = -speed;
+int main()
+{
+    setup();
+    while(1)
+    {
+        pc.printf("The left motor is going at speed: %d\r\n", left_speed);  
+        pc.printf("The left motor is going at speed: %d\r\n", right_speed);
+        wait(1);
     }
 }
 
-void leftForward(float speed) {
-    if (speed == 0) {
-        motor2_forward = 1.0;
-        motor2_reverse = 1.0;
+
+// movement functions.
+void moveForward()
+{
+    mouse_state = FORWARD;
+    
+    if ( left_speed > 0 ) // which should be always.
+    {
+        motor1_forward = left_speed;
+        motor1_reverse = 0;
+    }
+    else
+    {
+        motor1_forward = 0;
+        motor1_reverse = -left_speed;
     }
     
-    if (speed > 0) {
-        motor2_forward = speed;
+    if ( right_speed > 0 ) // which again should be always.
+    {
+        motor2_forward = right_speed;
         motor2_reverse = 0;
     }
-    
-    else {
+    else
+    {
         motor2_forward = 0;
-        motor2_reverse = -speed;
+        motor2_reverse = -right_speed;
     }
-}
\ No newline at end of file
+}
+
+void stop()
+{
+    mouse_state = STOPPED;
+    
+    motor1_forward = 1.0;
+    motor1_reverse = 1.0;
+    motor2_forward = 1.0;
+    motor2_reverse = 1.0;
+}
+
+void turn(int dir); // maybe split this into two functions?
\ No newline at end of file