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