Diff: main.cpp

Revision:

2:5f175018d1ff

Parent:

1:8fb04c532736

Child:

3:4db5e8ed9657

--- a/main.cpp	Mon Nov 04 20:38:22 2013 +0000
+++ b/main.cpp	Tue Nov 05 11:01:57 2013 +0000
@@ -26,14 +26,14 @@
 
     //LOCAL VARIABLES
     /*Potmeter input*/
-    //AnalogIn potmeter(PTC2);
-    //AnalogIn potmeter2(PTB0);
+    AnalogIn potmeter(PTC2);
+    AnalogIn potmeter2(PTB0);
 
     //EMG input
-    AnalogIn emg0(PTB0);
+    /*AnalogIn emg0(PTB0);
     AnalogIn emg1(PTB1);
     AnalogIn emg2(PTB2);
-    AnalogIn emg3(PTB3);
+    AnalogIn emg3(PTB3);*/
     /* Encoder, using my encoder library */
     /* First pin should be PTDx or PTAx  */
     /* because those pins can be used as */
@@ -161,7 +161,7 @@
             /*while loop voor de emg*/
     
         /* EMG Filter 1*/
-        x = emg0.read();                    //Reading EMG value
+        //x = emg0.read();                    //Reading EMG value
         y = 0.6389*x+1.2779*x1+0.6389*x2-y1*1.143-y2*0.4128;    //Formula for highpass filter at 20Hz as given in slides
         z = y*0.3913+y1*-0.7827+y2*0.3913-z1*-0.3695-z2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
         z = abs(z); //Rectify the signal
@@ -184,7 +184,7 @@
          //pc.printf("%f \n \r",(s1*f));
         
         /* EMG Filter 2*/
-        xx = emg1.read();                    //Reading EMG value
+        //xx = emg1.read();                    //Reading EMG value
         yy = 0.6389*xx+1.2779*xx1+0.6389*xx2-yy1*1.143-yy2*0.4128;    //Formula for highpass filter at 20Hz as given in slides
         zz = yy*0.3913+yy1*-0.7827+yy2*0.3913-zz1*-0.3695-zz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
         zz = abs(zz); //Rectify the signal
@@ -201,7 +201,7 @@
         if (ff<0.1)
         ff=0;
         /* EMG Filter 3*/
-        xxx = emg2.read();                    //Reading EMG value
+        //xxx = emg2.read();                    //Reading EMG value
         yyy = 0.6389*xxx+1.2779*xxx1+0.6389*xxx2-yyy1*1.143-yyy2*0.4128;    //Formula for highpass filter at 20Hz as given in slides
         zzz = yyy*0.3913+yyy1*-0.7827+yyy2*0.3913-zzz1*-0.3695-zzz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
         zzz = abs(zzz); //Rectify the signal
@@ -218,7 +218,7 @@
         if (fff<0.1)
         fff=0;
         /* EMG Filter 4*/
-        xxxx = emg3.read();                    //Reading EMG value
+        //xxxx = emg3.read();                    //Reading EMG value
         yyyy = 0.6389*xxxx+1.2779*xxxx1+0.6389*xxxx2-yyyy1*1.143-yyyy2*0.4128;    //Formula for highpass filter at 20Hz as given in slides
         zzzz = yyyy*0.3913+yyyy1*-0.7827+yyyy2*0.3913-zzzz1*-0.3695-zzzz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
         zzzz = abs(zzzz); //Rectify the signal
@@ -273,8 +273,8 @@
         /* to get useful setpoint value         */
         /*setpoint = ((ff)-0.2255)*1226.55;*/ /* SHOUDER kan van -23 tot 79 graden draaien*/
         
-        setpoint = (emg_dir*1226.55); //emgPos moet wel tussen 0 en 1 zitte?
-        setpoint2 = (emg_dir2*1226.55);
+        setpoint = (potmeter.read()*-1226.55); //emgPos moet wel tussen 0 en 1 zitte?
+        setpoint2 = (potmeter2.read()*-1226.55);
         /*setpoint2 = (potmeter2.read())*1226,55; ELLEBOOG kan van 0 tot 102 graden draaien*/
         //pc.printf("%f, %f \n \r",emgPos1, setpoint);
         // Print setpoint and current position to serial terminal
@@ -290,7 +290,7 @@
         up = kp*e;
         ei = e*Ts + ei1;
         ui = ki*ei;
-        keep_in_range(&ui, -0.5*setpoint,0.5*setpoint);
+        keep_in_range(&ui, -0.3,0.3);
         ed = (e-e1)/Ts;
         ud = ed*kd;
         u = up + ui + ud;
@@ -306,7 +306,7 @@
         up2 = kp2*e2;
         ei2 = e2*Ts + ei12;
         ui2 = ki2*ei2;
-        keep_in_range(&ui, -0.5*setpoint2,0.5*setpoint2);
+        keep_in_range(&ui, -0.3,0.3);
         ed2 = (e2-e12)/Ts;
         ud2 = ed2*kd2;
         u2 = up2 + ui2 + ud2;