Diff: main.cpp

Revision:

0:13d8a77fb1d7

Child:

1:98efd8dd9077

diff -r 000000000000 -r 13d8a77fb1d7 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Nov 21 03:20:07 2015 +0000
@@ -0,0 +1,157 @@
+#include "mbed.h"
+#include "AVEncoder.h"
+
+// set things
+Serial pc(SERIAL_TX, SERIAL_RX);
+Ticker Systicker;
+Timer timer;
+
+PwmOut right_forward(PB_10);
+PwmOut right_reverse(PA_6);
+PwmOut left_forward(PA_7);
+PwmOut left_reverse(PB_6);
+
+// 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
+AnalogIn _gyro(PA_0);
+// 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);
+
+//Right Front IR
+DigitalOut eRF(PC_12);
+AnalogIn rRF(PA_4);
+
+//Right Side IR
+DigitalOut eRS(PC_15);
+AnalogIn rRS(PB_0);
+
+DigitalOut myled(LED1);
+
+volatile float gyro_offset = 0;
+
+volatile float line_prevError = 0;
+volatile float enco_prevError = 0;
+volatile float gyro_prevError = 0;
+
+volatile float line_accumulator = 0;
+volatile float line_decayFactor = 1;
+volatile float enco_accumulator = 0;
+volatile float enco_decayFactor = 1;
+volatile float gyro_accumulator = 0;
+volatile float gyro_decayFactor = 1;
+
+volatile float left_speed = 0.5;
+volatile float right_speed = 0.5;
+
+const float left_max_speed = 6; // max speed is 6 encoder pulses per ms.
+const float right_max_speed = 6.2; 
+
+const float gyro_propo = 0.75;
+const float gyro_integ = 0;
+const float gyro_deriv = 1;
+
+const float enco_propo = 0.05;
+const float enco_integ = 0;
+const float enco_deriv = 5;
+
+const float spin_enco_weight = 0.5;
+const float spin_gyro_weight = 1 - spin_enco_weight;
+
+volatile float enco_error;
+volatile float enco_pid;
+volatile float gyro_error;
+volatile float gyro_pid;
+volatile float w_error;
+
+void reset()
+{
+    l_enco.reset();
+    r_enco.reset();
+}
+
+void systick()
+{
+    enco_error = l_enco.getPulses() - r_enco.getPulses();
+    gyro_error = _gyro.read() - gyro_offset;
+    
+    enco_pid = 0;
+    //enco_pid += enco_propo * enco_error;
+    //enco_pid += enco_deriv * (enco_error - enco_prevError);
+    
+    gyro_pid = 0;
+    gyro_pid += gyro_propo * gyro_error;
+    gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
+    
+    w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
+    left_speed += w_error;
+    right_speed -= w_error;
+    
+    left_forward = left_speed / left_max_speed;
+    left_reverse = 0;
+    right_forward = right_speed / right_max_speed;
+    right_reverse = 0;
+    
+    reset();
+}
+
+// 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 setup()
+{
+    pc.printf("Hello World\r\n");
+    
+    eRS = 0;
+    eRF = 0;
+    eLS = 0;
+    eLF = 0;
+    
+    myled = 1;
+    
+    for ( int i = 0; i < 1000; i++ )
+    {
+        offsetCalc();
+    }
+    
+    //left_forward = left_speed / left_max_speed;
+//    left_reverse = 0;
+//    right_forward = right_speed / right_max_speed;
+//    right_reverse = 0;
+    
+    wait(2);
+    
+    // repeat this for some time frame. 
+    Systicker.attach_us(&systick, 1000);
+}
+    
+
+int main() {
+    setup();
+    while(1) {
+        
+        pc.printf("enco_error: %f, gyro_error: %f, w_error: %f\r\n", enco_error, gyro_error, w_error);
+        //wait(1);
+    }
+}