Maik Overmars / Mbed 2 deprecated control

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?

UserRevisionLine numberNew 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 }