Diff: main.cpp

Revision:

31:8646e853979f

Parent:

30:cd4d9754a357

Child:

32:c940f6b6a6a0

--- a/main.cpp	Wed Oct 28 10:48:23 2015 +0000
+++ b/main.cpp	Wed Oct 28 11:49:29 2015 +0000
@@ -53,9 +53,13 @@
 //////////////////////////////////CONTROLLER
 const double control_frequency=250;// frequency at which the controller is called
 //controller constants
-float Kp=2;
-float Ki=0.5;
-float Kd=0.5;
+float Kp_shoot=6;
+float Ki_shoot=2;
+float Kd_shoot=0.5;
+float Kp_move=2;
+float Ki_move=1.25;
+float Kd_move=0.05;
+
 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
@@ -100,8 +104,8 @@
 const float mm_per_sec_emg=0.1;// move the pod 100 mm per sec if muscle is flexed
 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.5;// time it can take to shoot
-const float timetogoback=0.5;// time it can take to go back after shooting
+const float timetoshoot=0.25;// time it can take to shoot
+const float timetogoback=1;// time it can take to go back after shooting
 
 float desired_position=0;
 float desired_angle1=0;
@@ -157,10 +161,10 @@
 
 ////////////////////////FUNCTIONS
 //gather data and send to scope
-void scopedata()
+void scopedata(float wanted_y)
 {
     scope.set(0,desired_position-anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()));
-    scope.set(1,anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()));
+    scope.set(1,wanted_y-anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()));
     scope.set(2,error1*radtodeg);
     scope.set(3,error2*radtodeg);
     scope.set(4,mycontroller.motor1pwm());
@@ -224,14 +228,14 @@
 void valuechange()
 {
     mycontroller.STOP();
-    pc.printf("KP is now %f, enter new value\n",Kp);
-    pc.scanf("%f", &Kp);
+    pc.printf("KP is now %f, enter new value\n",Kp_move);
+    pc.scanf("%f", &Kp_move);
 
-    pc.printf("KI is now %f, enter new value\n",Ki);
-    pc.scanf("%f", &Ki);
+    pc.printf("KI is now %f, enter new value\n",Ki_move);
+    pc.scanf("%f", &Ki_move);
 
-    pc.printf("KD is now %f, enter new value\n",Kd);
-    pc.scanf("%f", &Kd);
+    pc.printf("KD is now %f, enter new value\n",Kd_move);
+    pc.scanf("%f", &Kd_move);
 
     pc.printf("Factor of tau_p=1.0/(factor*controlfrequency) is now %f, enter new value\n",factor_taup);
     pc.scanf("%f", &factor_taup);
@@ -269,8 +273,8 @@
         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
         error2=(desired_angle2-counttorad*encoder2.getPulses());
 
-        mycontroller.PID(error1,error2,Kp,Ki,Kd);;// send errors to controller
-        scopedata();//send data to hidscope   WARING lower freqyency than normal
+        mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
+        scopedata(y_during_punch);//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);
@@ -297,8 +301,8 @@
         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
         error2=(desired_angle2-counttorad*encoder2.getPulses());
 
-        mycontroller.PID(error1,error2,Kp,Ki,Kd);;// send errors to controller
-        scopedata();//send data to hidscope   WARING lower freqyency than normal
+        mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
+        scopedata(y_during_punch);//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);
@@ -414,7 +418,7 @@
         }
         if (scopedata_go==true) {//send scopedata
             //TIME THIS LOOP TAKES 0.000008 SEC (PEAKS AT 0.000015)
-            scopedata();
+            scopedata(y_start);
             scopedata_go=false;
         }
         if (safetyandthreshold_go==true) {// check the potmeters
@@ -450,7 +454,7 @@
 
                 float error1=(desired_angle1-counttorad*encoder1.getPulses());
                 float error2=(desired_angle2-counttorad*encoder2.getPulses());
-                mycontroller.PI(error1,error2,Kp,Ki);
+                mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
                 control_go=false;
             }
             valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
@@ -480,7 +484,7 @@
             if (control_go==true) {// calculate errors and send them to controllers
                 error1=(desired_angle1-counttorad*encoder1.getPulses());
                 error2=0;
-                mycontroller.PI(error1,error2,Kp,Ki);
+                mycontroller.PI(error1,error2,Kp_move,Ki_move);
                 control_go=false;
             }
         }
@@ -509,7 +513,7 @@
             if (control_go==true) {// calculate errors and send to controller
                 error1=0;
                 error2=(desired_angle2-counttorad*encoder2.getPulses());
-                mycontroller.PI(error1,error2,Kp,Ki);
+                mycontroller.PI(error1,error2,Kp_move,Ki_move);
                 control_go=false;
             }
         }
@@ -537,7 +541,7 @@
 
                 error1=(desired_angle1-counttorad*encoder1.getPulses());
                 error2=(desired_angle2-counttorad*encoder2.getPulses());
-                mycontroller.PIDLowPass(error1,error2,Kp,Ki,Kd,tau_p);
+                mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
                 control_go=false;
             }
         }