Aansturing
Dependencies: Encoder MODSERIAL mbed
main.cpp
- Committer:
- bouvdberg
- Date:
- 2013-10-29
- Revision:
- 1:adc1d0589d54
- Parent:
- 0:ba5ff341b020
File content as of revision 1:adc1d0589d54:
#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);//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Encoder motor2(; PwmOut pwm_motor1(PTA12); DigitalOut motordir1(PTD3); PwmOut pwm_motor2(PTA5); DigitalOut motordir2(PTD1); void motor1.setPosition(1050) //UITZOEKEN HOE DIT WEL MOET: NU FOUTMELDING!!!!!!!!!!!!!!!!!!!!!!!!!! void motor2.setPosition(190) //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, snelheid; float M1position, M2position, M2phi; float Px, Py; //Variabelen motoraansturing float setpointM1, setpointM2; float setpointmin1M1=0.0, setpointmin1M2=0.0; float pwm_to_motor1, pwm_to_motor2; 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 //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); if((EMGlp1-EMGmin1)<0.0) v1=0.0; else v1=v1; v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2); if((EMGlp2-EMGmin2)<0.0) v2=0.0; else v2=v2; v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3) if((EMGlp3-EMGmin3)<0.0) v3=0.0; else v3=v3; v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4); if((EMGlp4-EMGmin4)<0.0) v4=0.0; else v4=v4; if(v1=0.0 && v2=0.0 && v3=0.0 && v4=0.0) { P_solenoid=0; //Uitzoeken welke klasse dit moet zijn!!!!!!!!!!!!!!!! input=0.0; snelheid=0.0; } else { P_solenoid=1; snelheid=SNELHEID; input=tan((v1-v2)/(v3-v4)); } //snelheidsvector met beperking positie M1position = motor1.getPosition(); M2position = motor2.getPosition(); M2phi=M1position-M2position+1600.0; Px=cos((M1position/3200.0)*2.0*PI)*ARM1+cos((M2position/3200.0)*2.0*PI)*ARM2; Py=sin((M1position/3200.0)*2.0*PI)*ARM1+sin((M2position/3200.0)*2.0*PI)*ARM2; if(Px > 0.29 || Px < 0.125) vx=0; else vx=cos(input)*snelheid; if(Py < -0.425 || Py > -0.125) vy=0; else vy=sin(input)*snelheid; //input positie phi1=motor1.getPosition(); theta=motor2.getPosition(); phi2=theta+phi1-PI; //Jacobiaan // [a b] // [c d] a=-sin(phi1)*ARM1; b=cos(phi1)*ARM1; c=-sin(phi2)*ARM2; d=cos(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 //Beveiliging hoeksnelheden keep_in_range(&w1, 200,1400); //Motoraansturing setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1; setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2; 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); //Beperking hoeken keep_in_range(&setpointM1, 200,1400);//Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen keep_in_range(&setpointM2, 178,1550);// Begrensd op 20 graden minimaal, werkelijke minimale waarde is 15 graden 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; }