Control PID

Dependencies:   MODSERIAL QEI mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "QEI.h"
00004 #include "Butterworth.h"
00005 
00006 PwmOut pwmpin(D5);
00007 DigitalOut directionpin(D4);
00008 AnalogIn potmeter(A2);
00009 
00010 AnalogIn    emg0( A0 );
00011 AnalogIn    emg1( A1 );
00012 
00013 MODSERIAL pc(USBTX, USBRX);
00014 QEI wheel (D13, D12, NC, 32);
00015 
00016 float angle_resolution = (360.0/32.0)*(1.0/131.0);  //degrees/counts 
00017 
00018 float mean(float *samples, int n) {
00019     float sum = 0.0;
00020     for (int i=0; i<n; i++) {
00021         sum += samples[i];
00022     }
00023     return sum / (float)n;
00024 }
00025 
00026 int main()
00027 {   
00028     pc.baud(115200);
00029     
00030     pwmpin.period_us(60);
00031     int pulses = 0;
00032     float angle;
00033     int n = 1;
00034     int i = 0;
00035     float meanmuscle;
00036     float u[500];
00037     float dxmax = 0.1;
00038     
00039     while (true) {
00040         u[i] = emg0.read();
00041         
00042         pulses = wheel.getPulses();
00043         angle = pulses*angle_resolution;      
00044         if(n%500 == 0){
00045             pc.printf("Angle is: %f degrees \r\n", meanmuscle);
00046             
00047             // center signal around 0
00048             u = u - mean(u);
00049             // here we have to filter the signal and take absolute value
00050             //
00051             // now take mean of filtered signal and convert to a dx
00052             y = mean(u);
00053             if(y < 0.02){
00054                 dx = 0;
00055             }
00056             else if(y > 0.2){
00057                 dx = 1
00058             }
00059             else{
00060                 dx=(y - 0.02)/0.18*dxmax;
00061             }
00062             // 0.02 and 0.2 obtained from analysing filtered output
00063             // use dx for inverse kinematics combined with current x to obtain
00064             // desired positiion. this will be thetaref     
00065             meanmuscle = (mean(u,500)-0.1)*4;
00066             directionpin = meanmuscle > 0.0f;
00067             pwmpin = fabs(meanmuscle);
00068             i=0;
00069         }
00070         n++;    
00071         i++;
00072         wait(0.001);     
00073     }
00074 }