Diff: Motor.cpp

Revision:

0:18e417fff669

Child:

1:041f0a1fc2e5

Child:

2:3bf51023ea23

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp	Fri Mar 13 08:18:24 2015 +0000
@@ -0,0 +1,114 @@
+        #include "Motor.h"
+
+/* Theory of operation of the tachometer reading system
+
+    The interrupt is Tach, an InterruptIn object.
+
+    To filter out the horrible transients, some debounce and stuff is being done. This function, tach_mark(), is called
+    on a rising edge. It checks whether the risen edge is still there 10us later. (Using a blocking wait_us() call,
+    which is bad practice, but not that bad considering how short 10 us is. I don't think it would be that difficult to instead have it \
+    set up a 10us delay and nonblockingly interrupt-call a function using a Timeout.
+
+    Anyway, if it is still there, it then records the read from TachTimer and resets the TachTimer, and adds the read value to
+    the ring buffer and to the delta_t global.
+
+    It *also* disables the Tach rising edge interrupt. (i.e disables itself) and enables a ~~falling~~ edge interrupt on Tach. This
+    falling edge interupt then does mostly the same thing w/r/t filtering, and enables the rising edge after a confirmed falling edge.
+    
+*/
+void addToRingBuf(int val, volatile int *ringBuf, volatile int *ringBufIdx)          //Is called in ISR. Adds stuff to ring buffer, obviously.
+{
+    ringBuf[*ringBufIdx] = val;
+    *ringBufIdx =  (*ringBufIdx + 1) % RING_BUF_SZ;
+}
+
+
+void tach_mark(volatile int *ringBuf,
+               volatile int *ringBufIdx,
+               InterruptIn *Tach, 
+               Timer *TachTimer, 
+               volatile bool *new_tach_flag, 
+               void (*tach_antimark_handler)(void))      //PRIMARY ISR called on rising edge of encoder.
+{
+    int dt = TachTimer->read_us();
+    
+    wait_us(10);
+    if(Tach->read()) {
+        TachTimer->reset();
+        *new_tach_flag = 1;
+        addToRingBuf(dt, ringBuf, ringBufIdx);
+        Tach->rise(NULL);
+        Tach->fall(tach_antimark_handler);      // Reset after a falling edge.  //set to handler for antimark
+    }
+}
+
+
+void tach_antimark(InterruptIn *Tach, void (*tach_mark_handler)(void))
+{
+    wait_us(10);                        // This can surely be done better with a Timeout. Medium priority. Highest is getting it in a
+                                        //library/object and to work with two encoders at once.
+    if(!Tach->read()) {
+        Tach->fall(NULL);
+        Tach->rise(tach_mark_handler);          //set to handler function for mark
+    }
+}
+
+
+float get_speed(volatile bool *new_tach_flag, 
+                Timer *TachTimer, 
+                volatile int *ringBuf, 
+                volatile int *ringBufIdx)
+{
+    if(new_tach_flag) {
+        new_tach_flag = 0;
+    }
+    
+    // if no pulse in 400ms, motor is probably stopped
+    // 3% duty cycle is lowest the motor will run with, has <400ms period
+    if(TachTimer->read_ms() > 400) {
+        addToRingBuf(INT_MAX, ringBuf, ringBufIdx);
+        TachTimer->reset();
+    }
+
+    int avg = 0;
+    for (int i = 0; i < RING_BUF_SZ; i++) {
+        if (ringBuf[i] == INT_MAX) {
+            avg = INT_MAX;
+            break;
+        }
+        avg += ringBuf[i];
+    }
+
+    if (avg == INT_MAX) {
+        return 0;
+    } else {
+        avg = avg / RING_BUF_SZ;
+        if (avg > 400000) { //400 ms
+            return 0;
+        } else {
+            return WHEEL_CIRCUM / ((2.0 * (float)avg) / 1000.0);
+        }
+    }
+}
+
+void speed_control(volatile float target_spd, 
+                   float actual_spd, 
+                   float *iState, 
+                   float iLimit, 
+                   volatile float ki, 
+                   volatile float kp, 
+                   PwmOut *motor)
+{
+    
+    float delta_spd = target_spd - actual_spd;
+    
+    (*iState) += delta_spd;
+    if ((*iState)*ki > iLimit) {
+        (*iState) = iLimit;
+    } else if ((*iState)*ki < -iLimit) {
+        (*iState) = -iLimit;
+    }
+    
+    // PI controller!
+    motor->write(kp * delta_spd + ki * (*iState));    
+}