Revision 1:adc1d0589d54, committed 2013-10-29

Comitter:

bouvdberg

Date:

Tue Oct 29 16:09:09 2013 +0000

Parent:

0:ba5ff341b020

Commit message:

Nieuwe versie

Changed in this revision

--- a/main.cpp	Tue Oct 29 12:33:42 2013 +0000
+++ b/main.cpp	Tue Oct 29 16:09:09 2013 +0000
@@ -43,13 +43,16 @@
     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 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;
@@ -61,13 +64,14 @@
     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;
+    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 M1position, M2position;
     float foutM1, foutM2;
     float foutmin1M1=0.0, foutmin1M2=0.0;
     float foutverschilM1, foutverschilM2;
@@ -79,14 +83,9 @@
     //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;
@@ -118,27 +117,67 @@
         //berekenen setpoint
         //hoekinput
         v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);
+        if((EMGlp1-EMGmin1)<0.0)
+            v1=0.0;
+        else
+            v1=v1;
         v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);
-        v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3);
+        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);
-        input=tan((v1-v2)/(v3-v4));
+        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
-        vx=cos(input)*SNELHEID;
-        vy=sin(input)*SNELHEID;
+        //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();// Integraal van ideale positie maken!!!!!!!!!!
+        phi1=motor1.getPosition();
         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;
+        a=-sin(phi1)*ARM1;
+        b=cos(phi1)*ARM1;
+        c=-sin(phi2)*ARM2;
+        d=cos(phi2)*ARM2;
         
         //inverse
         // [ai  bi]
@@ -154,12 +193,13 @@
         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;
-        M1position = motor1.getPosition();
-        M2position = motor2.getPosition();
         foutM1 = setpointM1-M1position;
         foutM2 = setpointM2-M2position;
         foutverschilM1 = foutM1-foutmin1M1;
@@ -170,8 +210,11 @@
         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);
+        
+        //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
@@ -181,6 +224,7 @@
         else
             motordir2 = 1;
         
+        
         //WRITE VALUE TO MOTOR  
         pwm_motor1.write(abs(pwm_to_motor1));
         pwm_motor2.write(abs(pwm_to_motor2));