Diff: main.cpp

Revision:

28:71a90073e482

Parent:

27:31cb8fbd976d

Child:

29:cd47a5a772db

--- a/main.cpp	Mon Oct 26 14:17:38 2015 +0000
+++ b/main.cpp	Tue Oct 27 13:05:36 2015 +0000
@@ -43,7 +43,7 @@
 DigitalIn buttonL(D3);//left button on biorobotics shield
 
 /////////////////READSIGNAL
-const double readsignal_frequency=25;//frequency at wich the filtered emg signal is sampled to be 0 or 1
+const double readsignal_frequency=100;//frequency at wich the filtered emg signal is sampled to be 0 or 1
 Ticker readsignal_ticker;
 
 
@@ -101,7 +101,7 @@
 const float y_start=0.145;//starting y position of the pod
 const float y_punch=0.473;// position to where there is punched
 const float timetoshoot=0.25;// time it can take to shoot
-const float timetogoback=0.25;// time it can take to go back after shooting
+const float timetogoback=0.5;// time it can take to go back after shooting
 
 float desired_position=0;
 float desired_angle1=0;
@@ -160,21 +160,21 @@
 void scopedata()
 {
     scope.set(0,desired_position);
-    scope.set(1,desired_angle1*radtodeg);
-    scope.set(2,desired_angle2*radtodeg);
-    scope.set(3,error1*radtodeg);
-    scope.set(4,error2*radtodeg);
-    scope.set(5,mycontroller.motor1pwm());
+    scope.set(1,emg1_input.read());
+    scope.set(2,emg2_input.read());
+    scope.set(3,filteredsignal1);
+    scope.set(4,filteredsignal2);
+    scope.set(5,threshold_value);
     scope.send();
 }
 //read potmeters and adjust the safetyfactor and threshold
 void safetyandthreshold()
 {
     mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal
-    threshold_value=((ceil (10*threshold_pot.read()) )/10); // adjust the threshold value between 0 and 1 rounded to 1 decimal
+    threshold_value=((ceil (100*threshold_pot.read()) )/100); // adjust the threshold value between 0 and 1 rounded to 2 decimals
 }
 /////filter
-void filtereverything()
+void filtereverything(bool makeempty)
 {
     //pass1 so f1
     double pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
@@ -211,8 +211,15 @@
 
     filteredsignal1=(pass7_emg1*9e11*filter_extragain);
     filteredsignal2=(pass7_emg2*9e11*filter_extragain);
+    
+    if (makeempty==true) {//this is needed so the filtered value is nog high after shooting
+        pass1_emg1 = pass1_emg2 =pass2_emg1 =pass2_emg2 =pass3_emg1 = pass3_emg2 =pass4_emg1 =pass4_emg2 =pass5_emg1 =pass5_emg2 =0;
+        pass5_emg1_abs=pass5_emg2_abs=pass6_emg1 =pass6_emg2 =pass7_emg1=pass7_emg2 =filteredsignal1=filteredsignal2=0;
+        v1_f1_emg1=v1_f1_emg2=v1_f2_emg1=v1_f2_emg2=v1_f3_emg1=v1_f3_emg2=v1_f4_emg1=v1_f4_emg2=v1_f5_emg1=0;
+        v1_f5_emg2=v1_f6_emg1=v1_f6_emg2=v1_f7_emg1=v1_f7_emg2=0;
+    }
+
 }
-
 //adjust controller values when sw2 is pressed
 void valuechange()
 {
@@ -225,8 +232,8 @@
 
     pc.printf("KD is now %f, enter new value\n",Kd);
     pc.scanf("%f", &Kd);
-    
-     pc.printf("Factor of tau_p=1.0/(factor*controlfrequency) is now %f, enter new value\n",factor_taup);
+
+    pc.printf("Factor of tau_p=1.0/(factor*controlfrequency) is now %f, enter new value\n",factor_taup);
     pc.scanf("%f", &factor_taup);
 
     pc.printf("Extra gain is now %f, enter new value\n",filter_extragain);
@@ -265,6 +272,8 @@
         scopedata();//send data to hidscope   WARING lower freqyency than normal
 
         time+=(Ts_control);// add time it should take to calculated time
+        //pc.printf("Time = %f\n",time);
+        filtereverything(true);//set al filter variables to 0
         wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need, if this is not done the loop wil go to fast and the motors can't keep up
     }
     //back
@@ -291,6 +300,8 @@
         scopedata();//send data to hidscope   WARING lower freqyency than normal
 
         time+=(Ts_control);// add time it should take to calculated time
+        //pc.printf("Time = %f\n",time);
+        filtereverything(false);
         wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need, if this is not done the loop wil go to fast and the motors can't keep up
     }
     shoottimer.stop();
@@ -329,6 +340,7 @@
         led1=led2=0;
     }
 }
+
 ///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION
 void readsignalbutton()
 {
@@ -364,7 +376,7 @@
     }
 }
 
-void changemode()//this makes the counter higher to switch between modes
+void changemode()   //this makes the counter higher to switch between modes
 {
     mycontroller.STOP();
     switchedmode=true;
@@ -421,7 +433,7 @@
             }
             if (filter_go==true) {// filter the emg signal
                 // TIME THIS LOOP TAKES: 0.000173 SEC
-                filtereverything();
+                filtereverything(false);
                 filter_go=false;
             }
             if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position
@@ -524,7 +536,7 @@
 
                 error1=(desired_angle1-counttorad*encoder1.getPulses());
                 error2=(desired_angle2-counttorad*encoder2.getPulses());
-                mycontroller.PID(error1,error2,Kp,Ki,Kd);
+                mycontroller.PIDLowPass(error1,error2,Kp,Ki,Kd,tau_p);
                 control_go=false;
             }
         }