Diff: main.cpp

Revision:

0:b6369e728067

diff -r 000000000000 -r b6369e728067 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 03 15:50:59 2014 +0000
@@ -0,0 +1,130 @@
+#include "mbed.h"
+
+//------------------------------------
+// Hyperterminal configuration
+// 9600 bauds, 8-bit data, no parity
+//------------------------------------
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+InterruptIn sig(D4);
+Timer sigTimer;
+PwmOut ctrl(D9);
+DigitalIn A(D7);
+DigitalIn B(D2);
+
+int sigPeriod = 0, refPeriod = 3000;
+
+double dutyCycle = 0.5, stepSize = 0.01;
+
+Timer displayTimer, controlTimer;
+
+void onRisingEdge();
+void displayRPM(int period);
+void controlLoop(int period);
+
+void readEncoder();
+void encoderIncrement();
+void encoderDecrement();
+
+
+int main() {
+  pc.printf("Hello world\n\r");
+  displayTimer.start();
+  controlTimer.start();
+  sig.mode(PullNone);
+  sig.rise(onRisingEdge);
+  sigTimer.start();
+  ctrl.period_us(1000);
+  ctrl.write(0.5);
+  while(1) { 
+    displayRPM(sigPeriod);
+    controlLoop(sigPeriod);
+    readEncoder();
+  }
+}
+
+void onRisingEdge()
+{
+    static const int queueLength = 1;
+    static int queue[queueLength], queueStart = 0, queueEnd = 0;
+    static uint64_t sum = 0;
+    int time;
+    
+    time = sigTimer.read_us();
+    if (time < 100)
+      return;
+    sigTimer.reset();
+    if (queueStart == queueEnd && sum != 0) {
+        sum -= queue[queueStart];
+        queueStart = (queueStart + 1) % queueLength;
+    }
+    sum += time;
+    queue[queueEnd] = time;
+    queueEnd = (queueEnd + 1) % queueLength;
+    
+    if (queueStart == queueEnd)
+        sigPeriod = sum / queueLength;
+    else
+        sigPeriod = sum / ((queueEnd + queueLength - queueStart) % queueLength);
+}
+
+void displayRPM(int period)
+{        
+    if (displayTimer.read_ms() < 100)
+        return;
+    displayTimer.reset();
+    pc.printf("period is %d, refPeriod is %d, dutyCycle is %f\n\r", period, refPeriod, dutyCycle);
+}
+
+void controlLoop(int period)
+{     
+    if (controlTimer.read_ms() < 10)
+        return;
+    controlTimer.reset();
+
+    double input = period - refPeriod;
+    static const double intGain = 1.0e-7;
+    static const double propGain = 1.0e-6;
+    static double intState = 0.0;
+    
+    intState += intGain * input;
+    if (intState > 0.9)
+        intState = 0.9;
+    else if (intState < 0.1)
+        intState = 0.1;
+    dutyCycle = propGain * input + intState;
+    if (dutyCycle < 0.05)
+        dutyCycle = 0.05;
+    else if (dutyCycle > 0.95)
+        dutyCycle = 0.95;
+    ctrl.write(1.0 - dutyCycle);
+}
+
+void readEncoder()
+{
+    static int state = 0;
+    int currentState;
+    const int stateOrder[] = {0, 2, 3, 1};
+    const int stateIndex[] = {0, 3, 1, 2};
+    int ccwState, cwState;
+    
+    currentState = (A << 1) + B;
+    ccwState = stateOrder[(stateIndex[state] + 3) & 3];
+    cwState = stateOrder[(stateIndex[state] + 1) & 3];
+    if (currentState == cwState)
+        encoderIncrement();
+    else if (currentState == ccwState)
+        encoderDecrement();
+    state = currentState;
+}
+
+void encoderIncrement()
+{
+    refPeriod -= 1000;
+}
+
+void encoderDecrement()
+{
+    refPeriod += 1000;
+}
\ No newline at end of file