Control PID
Dependencies: MODSERIAL QEI mbed
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 }
Generated on Sat Jul 23 2022 08:02:01 by
1.7.2