Control PID
Dependencies: MODSERIAL QEI mbed
main.cpp@0:545f4726720c, 2018-10-15 (annotated)
- Committer:
- MaikOvermars
- Date:
- Mon Oct 15 08:23:49 2018 +0000
- Revision:
- 0:545f4726720c
stuff
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MaikOvermars | 0:545f4726720c | 1 | #include "mbed.h" |
MaikOvermars | 0:545f4726720c | 2 | #include "MODSERIAL.h" |
MaikOvermars | 0:545f4726720c | 3 | #include "QEI.h" |
MaikOvermars | 0:545f4726720c | 4 | #include "Butterworth.h" |
MaikOvermars | 0:545f4726720c | 5 | |
MaikOvermars | 0:545f4726720c | 6 | PwmOut pwmpin(D5); |
MaikOvermars | 0:545f4726720c | 7 | DigitalOut directionpin(D4); |
MaikOvermars | 0:545f4726720c | 8 | AnalogIn potmeter(A2); |
MaikOvermars | 0:545f4726720c | 9 | |
MaikOvermars | 0:545f4726720c | 10 | AnalogIn emg0( A0 ); |
MaikOvermars | 0:545f4726720c | 11 | AnalogIn emg1( A1 ); |
MaikOvermars | 0:545f4726720c | 12 | |
MaikOvermars | 0:545f4726720c | 13 | MODSERIAL pc(USBTX, USBRX); |
MaikOvermars | 0:545f4726720c | 14 | QEI wheel (D13, D12, NC, 32); |
MaikOvermars | 0:545f4726720c | 15 | |
MaikOvermars | 0:545f4726720c | 16 | float angle_resolution = (360.0/32.0)*(1.0/131.0); //degrees/counts |
MaikOvermars | 0:545f4726720c | 17 | |
MaikOvermars | 0:545f4726720c | 18 | float mean(float *samples, int n) { |
MaikOvermars | 0:545f4726720c | 19 | float sum = 0.0; |
MaikOvermars | 0:545f4726720c | 20 | for (int i=0; i<n; i++) { |
MaikOvermars | 0:545f4726720c | 21 | sum += samples[i]; |
MaikOvermars | 0:545f4726720c | 22 | } |
MaikOvermars | 0:545f4726720c | 23 | return sum / (float)n; |
MaikOvermars | 0:545f4726720c | 24 | } |
MaikOvermars | 0:545f4726720c | 25 | |
MaikOvermars | 0:545f4726720c | 26 | int main() |
MaikOvermars | 0:545f4726720c | 27 | { |
MaikOvermars | 0:545f4726720c | 28 | pc.baud(115200); |
MaikOvermars | 0:545f4726720c | 29 | |
MaikOvermars | 0:545f4726720c | 30 | pwmpin.period_us(60); |
MaikOvermars | 0:545f4726720c | 31 | int pulses = 0; |
MaikOvermars | 0:545f4726720c | 32 | float angle; |
MaikOvermars | 0:545f4726720c | 33 | int n = 1; |
MaikOvermars | 0:545f4726720c | 34 | int i = 0; |
MaikOvermars | 0:545f4726720c | 35 | float meanmuscle; |
MaikOvermars | 0:545f4726720c | 36 | float u[500]; |
MaikOvermars | 0:545f4726720c | 37 | float dxmax = 0.1; |
MaikOvermars | 0:545f4726720c | 38 | |
MaikOvermars | 0:545f4726720c | 39 | while (true) { |
MaikOvermars | 0:545f4726720c | 40 | u[i] = emg0.read(); |
MaikOvermars | 0:545f4726720c | 41 | |
MaikOvermars | 0:545f4726720c | 42 | pulses = wheel.getPulses(); |
MaikOvermars | 0:545f4726720c | 43 | angle = pulses*angle_resolution; |
MaikOvermars | 0:545f4726720c | 44 | if(n%500 == 0){ |
MaikOvermars | 0:545f4726720c | 45 | pc.printf("Angle is: %f degrees \r\n", meanmuscle); |
MaikOvermars | 0:545f4726720c | 46 | |
MaikOvermars | 0:545f4726720c | 47 | // center signal around 0 |
MaikOvermars | 0:545f4726720c | 48 | u = u - mean(u); |
MaikOvermars | 0:545f4726720c | 49 | // here we have to filter the signal and take absolute value |
MaikOvermars | 0:545f4726720c | 50 | // |
MaikOvermars | 0:545f4726720c | 51 | // now take mean of filtered signal and convert to a dx |
MaikOvermars | 0:545f4726720c | 52 | y = mean(u); |
MaikOvermars | 0:545f4726720c | 53 | if(y < 0.02){ |
MaikOvermars | 0:545f4726720c | 54 | dx = 0; |
MaikOvermars | 0:545f4726720c | 55 | } |
MaikOvermars | 0:545f4726720c | 56 | else if(y > 0.2){ |
MaikOvermars | 0:545f4726720c | 57 | dx = 1 |
MaikOvermars | 0:545f4726720c | 58 | } |
MaikOvermars | 0:545f4726720c | 59 | else{ |
MaikOvermars | 0:545f4726720c | 60 | dx=(y - 0.02)/0.18*dxmax; |
MaikOvermars | 0:545f4726720c | 61 | } |
MaikOvermars | 0:545f4726720c | 62 | // 0.02 and 0.2 obtained from analysing filtered output |
MaikOvermars | 0:545f4726720c | 63 | // use dx for inverse kinematics combined with current x to obtain |
MaikOvermars | 0:545f4726720c | 64 | // desired positiion. this will be thetaref |
MaikOvermars | 0:545f4726720c | 65 | meanmuscle = (mean(u,500)-0.1)*4; |
MaikOvermars | 0:545f4726720c | 66 | directionpin = meanmuscle > 0.0f; |
MaikOvermars | 0:545f4726720c | 67 | pwmpin = fabs(meanmuscle); |
MaikOvermars | 0:545f4726720c | 68 | i=0; |
MaikOvermars | 0:545f4726720c | 69 | } |
MaikOvermars | 0:545f4726720c | 70 | n++; |
MaikOvermars | 0:545f4726720c | 71 | i++; |
MaikOvermars | 0:545f4726720c | 72 | wait(0.001); |
MaikOvermars | 0:545f4726720c | 73 | } |
MaikOvermars | 0:545f4726720c | 74 | } |