Diff: main.cpp

Revision:

27:31cb8fbd976d

Parent:

26:c935e39cce8a

Child:

28:71a90073e482

--- a/main.cpp	Mon Oct 26 12:26:27 2015 +0000
+++ b/main.cpp	Mon Oct 26 14:17:38 2015 +0000
@@ -42,16 +42,28 @@
 DigitalIn buttonR(D2);//rigth button on biorobotics shield
 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
+Ticker readsignal_ticker;
+
+
+DigitalOut led1(PTC12);// rigth led on biorobotics shield
+DigitalOut led2(D9);//left led on biorobotics shield
+
 //////////////////////////////////CONTROLLER
 const double control_frequency=250;// frequency at which the controller is called
 //controller constants
-float Kp=1;
-float Ki=0.1;
-float Kd=0.001;
-controlandadjust mycontroller(6,control_frequency); // make a controller, value in brackets is errorband in degrees and controller frequency
+float Kp=2;
+float Ki=0.5;
+float Kd=0.5;
+float factor_taup=1.5;
+float tau_p=1.0/(factor_taup*control_frequency);
+controlandadjust mycontroller(2,control_frequency); // make a controller, value in brackets is errorband in degrees and controller frequency
 
 Ticker control_ticker;
 
+const double Ts_control=1.0/control_frequency;
+
 float error1=0,//controller error storage variables
       error2=0;
 
@@ -82,13 +94,6 @@
 volatile double filteredsignal2=0;//the second filtered emg signal
 float filter_extragain=1;
 
-/////////////////READSIGNAL
-const double readsignal_frequency=25;//frequency at wich the filtered emg signal is sampled to be 0 or 1
-Ticker readsignal_ticker;
-
-
-DigitalOut led1(PTC12);// rigth led on biorobotics shield
-DigitalOut led2(D9);//left led on biorobotics shield
 
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
 const float safetymarginfield=0.075; //adjustable, tweak for maximum but safe range
@@ -96,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.5;// time it can take to go back after shooting
+const float timetogoback=0.25;// time it can take to go back after shooting
 
 float desired_position=0;
 float desired_angle1=0;
@@ -159,7 +164,7 @@
     scope.set(2,desired_angle2*radtodeg);
     scope.set(3,error1*radtodeg);
     scope.set(4,error2*radtodeg);
-    scope.set(5,checktimervalue);
+    scope.set(5,mycontroller.motor1pwm());
     scope.send();
 }
 //read potmeters and adjust the safetyfactor and threshold
@@ -220,6 +225,9 @@
 
     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.scanf("%f", &factor_taup);
 
     pc.printf("Extra gain is now %f, enter new value\n",filter_extragain);
     pc.scanf("%f", &filter_extragain);
@@ -253,7 +261,7 @@
         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
         error2=(desired_angle2-counttorad*encoder2.getPulses());
 
-        mycontroller.PI(error1,error2,Kp,Ki);// send errors to controller
+        mycontroller.PID(error1,error2,Kp,Ki,Kd);;// send errors to controller
         scopedata();//send data to hidscope   WARING lower freqyency than normal
 
         time+=(Ts_control);// add time it should take to calculated time
@@ -279,7 +287,7 @@
         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
         error2=(desired_angle2-counttorad*encoder2.getPulses());
 
-        mycontroller.PI(error1,error2,Kp,Ki);// send errors to controller
+        mycontroller.PID(error1,error2,Kp,Ki,Kd);;// send errors to controller
         scopedata();//send data to hidscope   WARING lower freqyency than normal
 
         time+=(Ts_control);// add time it should take to calculated time
@@ -385,6 +393,7 @@
 
     pc.baud(115200);//set baudrate to 115200
     while(1) {
+        valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
         checktimer.reset();
         checktimer.start();
         if (changemodebutton==0) {// check if the change mode button is pressed
@@ -515,11 +524,9 @@
 
                 error1=(desired_angle1-counttorad*encoder1.getPulses());
                 error2=(desired_angle2-counttorad*encoder2.getPulses());
-                mycontroller.PI(error1,error2,Kp,Ki);
+                mycontroller.PID(error1,error2,Kp,Ki,Kd);
                 control_go=false;
-                pc.printf("error is %f Ki*error=%f en dat is %f procent\n",error1*radtodeg,Ki*error2_int,((Ki*error2_int)/mycontroller.motor2pwm())*100);
             }
-            valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
         }
         checktimervalue=checktimer.read();
         checktimer.stop();