Aansturing
Dependencies: Encoder MODSERIAL mbed
Revision 0:ba5ff341b020, committed 2013-10-29
- Comitter:
- bouvdberg
- Date:
- Tue Oct 29 12:33:42 2013 +0000
- Child:
- 1:adc1d0589d54
- Commit message:
- Robot arm aansturing
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Tue Oct 29 12:33:42 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/vsluiter/code/Encoder/#2dd7853c911a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Tue Oct 29 12:33:42 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#b04ce87dc424
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Oct 29 12:33:42 2013 +0000 @@ -0,0 +1,201 @@ +#include "mbed.h" +#include "MODSERIAL.h" +#include "encoder.h" + +// definieren constanten +#define PI 3.1415926 +//plant +#define ARM1 0.36 +#define ARM2 0.18 +//PD +#define CP 0.0002 +#define CD 0.002 +#define CLP1 0.9975 +#define CLP2 0.001 +//Snelheid +#define SNELHEID 0.05 +//LOOPTIME +#define LOOPTIME 0.01 +//Filtering EMG +#define HP1 0.8187 +#define HP2 20.0 +#define HP3 20.0 +#define LP1 0.9512 +#define LP2 0.04877 + +void keep_in_range(float * in, float min, float max); + +volatile bool looptimerflag; + +void setlooptimerflag(void) +{ + looptimerflag = true; +} + +int main() { + AnalogIn EMG0(PTB0); + AnalogIn EMG1(PTB1); + AnalogIn EMG2(PTB2); + AnalogIn EMG3(PTB3); + AnalogIn potmeter(PTC2); + DigitalIn ButtonUP(PTE20); //Knopjes voor kalibratie + DigitalIn ButtonDOWN(PTE21); + DigitalIn ButtonSELECT(PTE22); + DigitalIn ButtonSTOP(PTE23); + DigitalOut Solenoid(PTD4); //Solenoid + Encoder motor1(PTD0,PTD2);//Moet aangesloten zijn op twee pinnen, waarvan een met interrupt mogelijkheid + Encoder motor2(; + PwmOut pwm_motor1(PTA12); + DigitalOut motordir1(PTD3); + PwmOut pwm_motor2(PTA5); + DigitalOut motordir2(PTD1); + + //Variabelen verwerking EMG + float emg_value1, emg_value2, emg_value3, emg_value4; + float emg_value1min1=0.0, emg_value2min1=0.0, emg_value3min1=0.0, emg_value4min1=0.0; + float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4; + float EMGhp1min1=0.0, EMGhp2min1=0.0, EMGhp3min1=0.0, EMGhp4min1=0.0, EMGlp1min1=0.0, EMGlp2min1=0.0, EMGlp3min1=0.0, EMGlp4min1=0.0; + float EMGmax1, EMGmin1, EMGmax2, EMGmin2, EMGmax3, EMGmin3, EMGmax4, EMGmin4; + + //Variabelen bepaling input systeem + float input; + float w1, w2, wM2, phi1, phi2, theta; + float a, b, c, d, ai, bi, ci, di; + float v1, v2, v3, v4, vx, vy; + + //Variabelen motoraansturing + float setpointM1, setpointM2; + float setpointmin1M1=0.0, setpointmin1M2=0.0; + float pwm_to_motor1, pwm_to_motor2; + float M1position, M2position; + float foutM1, foutM2; + float foutmin1M1=0.0, foutmin1M2=0.0; + float foutverschilM1, foutverschilM2; + float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0; + float CDloop=CD/LOOPTIME; + + //Kalibratie + //1. Bepalen EMGmax1 EMGmin1 + //2. Bepalen EMGmax2 EMGmin2 + //3. Bepalen EMGmax3 EMGmin3 + //4. Bepalen EMGmax4 EMGmin4 + EMGmax1=+(potmeter.read()-0.5)*; + EMGmin1=+(potmeter.read()-0.5)*; + EMGmax2=+(potmeter.read()-0.5)*; + EMGmin2=+(potmeter.read()-0.5)*; + EMGmax3=+(potmeter.read()-0.5)*; + EMGmin3=+(potmeter.read()-0.5)*; + EMGmax4=+(potmeter.read()-0.5)*; + EMGmin4=+(potmeter.read()-0.5)*; + + //Aansturing + Ticker looptimer; + looptimer.attach(setlooptimerflag,LOOPTIME); + while(1) { + while(looptimerflag != true); + looptimerflag = false; + + //uitlezen + emg_value1 = EMG0.read(); + emg_value2 = EMG1.read(); + emg_value3 = EMG2.read(); + emg_value4 = EMG3.read(); + + //filtering + EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1; + EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1; + EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1; + EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1; + EMGhp1=abs(EMGhp1); + EMGhp2=abs(EMGhp2); + EMGhp3=abs(EMGhp3); + EMGhp4=abs(EMGhp4); + EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1; + EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1; + EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1; + EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1; + + //berekenen setpoint + //hoekinput + v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1); + v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2); + v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3); + v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4); + input=tan((v1-v2)/(v3-v4)); + + //snelheidsvector + vx=cos(input)*SNELHEID; + vy=sin(input)*SNELHEID; + + //input positie + phi1=motor1.getPosition();// Integraal van ideale positie maken!!!!!!!!!! + theta=motor2.getPosition(); + phi2=theta+phi1-PI; + + //Jacobiaan + // [a b] + // [c d] + a=cos(phi1)*ARM1; + b=sin(phi1)*ARM1; + c=cos(phi2)*ARM2; + d=sin(phi2)*ARM2; + + //inverse + // [ai bi] + // [ci di] + ai=d/(a*d-b*c); + bi=-b/(a*d-b*c); + ci=-c/(a*d-b*c); + di=a/(a*d-b*c); + + //vermenigvuldiging + // [ai bi] [vx] [w1] + // [ci di] * [vy] = [w2] + w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1 + w2=ci*vx+di*vy; + wM2=w2-w1;//hoeksnelheid motor 2 + + //Motoraansturing + setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1; + setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2; + M1position = motor1.getPosition(); + M2position = motor2.getPosition(); + foutM1 = setpointM1-M1position; + foutM2 = setpointM2-M2position; + foutverschilM1 = foutM1-foutmin1M1; + foutverschilM2 = foutM2-foutmin1M2; + foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1; + foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2; + pwm_to_motor1 = foutM1*CP+foutverschilM1*CDloop; + pwm_to_motor2 = foutM2*CP+foutverschilM2*CDloop; + keep_in_range(&pwm_to_motor1, -1,1); + keep_in_range(&pwm_to_motor2, -1,1); + keep_in_range(&setpointM1, -800,800);//Juiste hoeken invoeren + keep_in_range(&setpointM2, 0,1500); + if(pwm_to_motor1 > 0) + motordir1 = 0; + else + motordir1 = 1; + if(pwm_to_motor2 > 0) + motordir2 = 0; + else + motordir2 = 1; + + //WRITE VALUE TO MOTOR + pwm_motor1.write(abs(pwm_to_motor1)); + pwm_motor2.write(abs(pwm_to_motor2)); + + //Definieren waarden in de verleden tijd + foutmin1M1=foutM1; + foutmin1M2=foutM2; + foutverschilmin1M1=foutverschilM1; + foutverschilmin1M2=foutverschilM2; + setpointmin1M1=setpointM1; + setpointmin1M2=setpointM2; + } +} + +void keep_in_range(float * in, float min, float max) +{ + *in > min ? *in < max? : *in = max: *in = min; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 29 12:33:42 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file