Arcadie Cracan / Mbed 2 deprecated dcMotorControl

PI DC motor control with encoder sensor

Dependencies:   mbed

main.cpp@0:b6369e728067, 2014-11-03 (annotated)

Committer:
acracan
Date:
Mon Nov 03 15:50:59 2014 +0000
Revision:
0:b6369e728067
First version of PI control...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acracan 0:b6369e728067 1 #include "mbed.h"
acracan 0:b6369e728067 2
acracan 0:b6369e728067 3 //------------------------------------
acracan 0:b6369e728067 4 // Hyperterminal configuration
acracan 0:b6369e728067 5 // 9600 bauds, 8-bit data, no parity
acracan 0:b6369e728067 6 //------------------------------------
acracan 0:b6369e728067 7
acracan 0:b6369e728067 8 Serial pc(SERIAL_TX, SERIAL_RX);
acracan 0:b6369e728067 9
acracan 0:b6369e728067 10 InterruptIn sig(D4);
acracan 0:b6369e728067 11 Timer sigTimer;
acracan 0:b6369e728067 12 PwmOut ctrl(D9);
acracan 0:b6369e728067 13 DigitalIn A(D7);
acracan 0:b6369e728067 14 DigitalIn B(D2);
acracan 0:b6369e728067 15
acracan 0:b6369e728067 16 int sigPeriod = 0, refPeriod = 3000;
acracan 0:b6369e728067 17
acracan 0:b6369e728067 18 double dutyCycle = 0.5, stepSize = 0.01;
acracan 0:b6369e728067 19
acracan 0:b6369e728067 20 Timer displayTimer, controlTimer;
acracan 0:b6369e728067 21
acracan 0:b6369e728067 22 void onRisingEdge();
acracan 0:b6369e728067 23 void displayRPM(int period);
acracan 0:b6369e728067 24 void controlLoop(int period);
acracan 0:b6369e728067 25
acracan 0:b6369e728067 26 void readEncoder();
acracan 0:b6369e728067 27 void encoderIncrement();
acracan 0:b6369e728067 28 void encoderDecrement();
acracan 0:b6369e728067 29
acracan 0:b6369e728067 30
acracan 0:b6369e728067 31 int main() {
acracan 0:b6369e728067 32 pc.printf("Hello world\n\r");
acracan 0:b6369e728067 33 displayTimer.start();
acracan 0:b6369e728067 34 controlTimer.start();
acracan 0:b6369e728067 35 sig.mode(PullNone);
acracan 0:b6369e728067 36 sig.rise(onRisingEdge);
acracan 0:b6369e728067 37 sigTimer.start();
acracan 0:b6369e728067 38 ctrl.period_us(1000);
acracan 0:b6369e728067 39 ctrl.write(0.5);
acracan 0:b6369e728067 40 while(1) {
acracan 0:b6369e728067 41 displayRPM(sigPeriod);
acracan 0:b6369e728067 42 controlLoop(sigPeriod);
acracan 0:b6369e728067 43 readEncoder();
acracan 0:b6369e728067 44 }
acracan 0:b6369e728067 45 }
acracan 0:b6369e728067 46
acracan 0:b6369e728067 47 void onRisingEdge()
acracan 0:b6369e728067 48 {
acracan 0:b6369e728067 49 static const int queueLength = 1;
acracan 0:b6369e728067 50 static int queue[queueLength], queueStart = 0, queueEnd = 0;
acracan 0:b6369e728067 51 static uint64_t sum = 0;
acracan 0:b6369e728067 52 int time;
acracan 0:b6369e728067 53
acracan 0:b6369e728067 54 time = sigTimer.read_us();
acracan 0:b6369e728067 55 if (time < 100)
acracan 0:b6369e728067 56 return;
acracan 0:b6369e728067 57 sigTimer.reset();
acracan 0:b6369e728067 58 if (queueStart == queueEnd && sum != 0) {
acracan 0:b6369e728067 59 sum -= queue[queueStart];
acracan 0:b6369e728067 60 queueStart = (queueStart + 1) % queueLength;
acracan 0:b6369e728067 61 }
acracan 0:b6369e728067 62 sum += time;
acracan 0:b6369e728067 63 queue[queueEnd] = time;
acracan 0:b6369e728067 64 queueEnd = (queueEnd + 1) % queueLength;
acracan 0:b6369e728067 65
acracan 0:b6369e728067 66 if (queueStart == queueEnd)
acracan 0:b6369e728067 67 sigPeriod = sum / queueLength;
acracan 0:b6369e728067 68 else
acracan 0:b6369e728067 69 sigPeriod = sum / ((queueEnd + queueLength - queueStart) % queueLength);
acracan 0:b6369e728067 70 }
acracan 0:b6369e728067 71
acracan 0:b6369e728067 72 void displayRPM(int period)
acracan 0:b6369e728067 73 {
acracan 0:b6369e728067 74 if (displayTimer.read_ms() < 100)
acracan 0:b6369e728067 75 return;
acracan 0:b6369e728067 76 displayTimer.reset();
acracan 0:b6369e728067 77 pc.printf("period is %d, refPeriod is %d, dutyCycle is %f\n\r", period, refPeriod, dutyCycle);
acracan 0:b6369e728067 78 }
acracan 0:b6369e728067 79
acracan 0:b6369e728067 80 void controlLoop(int period)
acracan 0:b6369e728067 81 {
acracan 0:b6369e728067 82 if (controlTimer.read_ms() < 10)
acracan 0:b6369e728067 83 return;
acracan 0:b6369e728067 84 controlTimer.reset();
acracan 0:b6369e728067 85
acracan 0:b6369e728067 86 double input = period - refPeriod;
acracan 0:b6369e728067 87 static const double intGain = 1.0e-7;
acracan 0:b6369e728067 88 static const double propGain = 1.0e-6;
acracan 0:b6369e728067 89 static double intState = 0.0;
acracan 0:b6369e728067 90
acracan 0:b6369e728067 91 intState += intGain * input;
acracan 0:b6369e728067 92 if (intState > 0.9)
acracan 0:b6369e728067 93 intState = 0.9;
acracan 0:b6369e728067 94 else if (intState < 0.1)
acracan 0:b6369e728067 95 intState = 0.1;
acracan 0:b6369e728067 96 dutyCycle = propGain * input + intState;
acracan 0:b6369e728067 97 if (dutyCycle < 0.05)
acracan 0:b6369e728067 98 dutyCycle = 0.05;
acracan 0:b6369e728067 99 else if (dutyCycle > 0.95)
acracan 0:b6369e728067 100 dutyCycle = 0.95;
acracan 0:b6369e728067 101 ctrl.write(1.0 - dutyCycle);
acracan 0:b6369e728067 102 }
acracan 0:b6369e728067 103
acracan 0:b6369e728067 104 void readEncoder()
acracan 0:b6369e728067 105 {
acracan 0:b6369e728067 106 static int state = 0;
acracan 0:b6369e728067 107 int currentState;
acracan 0:b6369e728067 108 const int stateOrder[] = {0, 2, 3, 1};
acracan 0:b6369e728067 109 const int stateIndex[] = {0, 3, 1, 2};
acracan 0:b6369e728067 110 int ccwState, cwState;
acracan 0:b6369e728067 111
acracan 0:b6369e728067 112 currentState = (A << 1) + B;
acracan 0:b6369e728067 113 ccwState = stateOrder[(stateIndex[state] + 3) & 3];
acracan 0:b6369e728067 114 cwState = stateOrder[(stateIndex[state] + 1) & 3];
acracan 0:b6369e728067 115 if (currentState == cwState)
acracan 0:b6369e728067 116 encoderIncrement();
acracan 0:b6369e728067 117 else if (currentState == ccwState)
acracan 0:b6369e728067 118 encoderDecrement();
acracan 0:b6369e728067 119 state = currentState;
acracan 0:b6369e728067 120 }
acracan 0:b6369e728067 121
acracan 0:b6369e728067 122 void encoderIncrement()
acracan 0:b6369e728067 123 {
acracan 0:b6369e728067 124 refPeriod -= 1000;
acracan 0:b6369e728067 125 }
acracan 0:b6369e728067 126
acracan 0:b6369e728067 127 void encoderDecrement()
acracan 0:b6369e728067 128 {
acracan 0:b6369e728067 129 refPeriod += 1000;
acracan 0:b6369e728067 130 }