Diff: main.cpp

Revision:

26:c935e39cce8a

Parent:

25:21dcd3f9eac2

Child:

27:31cb8fbd976d

--- a/main.cpp	Fri Oct 23 10:18:21 2015 +0000
+++ b/main.cpp	Mon Oct 26 12:26:27 2015 +0000
@@ -48,18 +48,12 @@
 float Kp=1;
 float Ki=0.1;
 float Kd=0.001;
-controlandadjust mycontroller(6); // make a controller, value in brackets is errorband
+controlandadjust mycontroller(6,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;
-float error2=0;
-float error1_int=0;// storage variables for the errors
-float error2_int=0;
-float error1_prev=0;
-float error2_prev=0;
+float error1=0,//controller error storage variables
+      error2=0;
 
 InterruptIn valuechangebutton(PTC6);//button to change controller constants
 
@@ -99,9 +93,10 @@
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
 const float safetymarginfield=0.075; //adjustable, tweak for maximum but safe range
 const float mm_per_sec_emg=0.1;// move the pod 100 mm per sec if muscle is flexed
-const float y_start=0.155;//starting y position of the pod
+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
 
 float desired_position=0;
 float desired_angle1=0;
@@ -258,7 +253,7 @@
         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
         error2=(desired_angle2-counttorad*encoder2.getPulses());
 
-        mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);// send errors to controller
+        mycontroller.PI(error1,error2,Kp,Ki);// 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
@@ -267,7 +262,8 @@
     //back
     time=0;
     shoottimer.reset();
-    while (time<=timetoshoot) {
+    stepsize=(y_punch-y_start)/(timetogoback*control_frequency);
+    while (time<=timetogoback) {
         ledblue=!ledblue;
 
         y_during_punch-=stepsize; // add stepsize to y position
@@ -283,7 +279,7 @@
         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
         error2=(desired_angle2-counttorad*encoder2.getPulses());
 
-        mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);// send errors to controller
+        mycontroller.PI(error1,error2,Kp,Ki);// 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
@@ -390,20 +386,20 @@
     pc.baud(115200);//set baudrate to 115200
     while(1) {
         checktimer.reset();
-       checktimer.start();
+        checktimer.start();
         if (changemodebutton==0) {// check if the change mode button is pressed
             changemode();
         }
         if (scopedata_go==true) {//send scopedata
-        //TIME THIS LOOP TAKES 0.000008 SEC (PEAKS AT 0.000015)
+            //TIME THIS LOOP TAKES 0.000008 SEC (PEAKS AT 0.000015)
             scopedata();
-            scopedata_go=false;             
+            scopedata_go=false;
         }
         if (safetyandthreshold_go==true) {// check the potmeters
-        //TIME THIS LOOP TAKES: 0.000032 SEC          
+            //TIME THIS LOOP TAKES: 0.000032 SEC
             safetyandthreshold();
             safetyandthreshold_go=false;
-                    }
+        }
         ///////////////////////////////////////////NORMAL RUNNING MODE
         if(modecounter==0) {
             if (switchedmode==true) {
@@ -415,26 +411,26 @@
                 switchedmode=false;
             }
             if (filter_go==true) {// filter the emg signal
-            // TIME THIS LOOP TAKES: 0.000173 SEC           
+                // TIME THIS LOOP TAKES: 0.000173 SEC
                 filtereverything();
-                filter_go=false;                
+                filter_go=false;
             }
             if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position
-            // TIME THIS LOOP TAKES: 0.000005 SEC
+                // TIME THIS LOOP TAKES: 0.000005 SEC
                 readsignal();
                 readsignal_go=false;
-                           }
+            }
             if (control_go==true) {// calculate angles from positions and send error to controller
-            //TIME THIS LOOP TAKES: 0.000223 SEC
-            
+                //TIME THIS LOOP TAKES: 0.000223 SEC
+
                 desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
                 desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
 
                 float error1=(desired_angle1-counttorad*encoder1.getPulses());
                 float error2=(desired_angle2-counttorad*encoder2.getPulses());
-                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
+                mycontroller.PI(error1,error2,Kp,Ki);
                 control_go=false;
-                            }
+            }
             valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
         }
         ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
@@ -462,7 +458,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,Ts_control,error1_int,error2_int);
+                mycontroller.PI(error1,error2,Kp,Ki);
                 control_go=false;
             }
         }
@@ -491,7 +487,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,Ts_control,error1_int,error2_int);
+                mycontroller.PI(error1,error2,Kp,Ki);
                 control_go=false;
             }
         }
@@ -519,8 +515,9 @@
 
                 error1=(desired_angle1-counttorad*encoder1.getPulses());
                 error2=(desired_angle2-counttorad*encoder2.getPulses());
-                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
+                mycontroller.PI(error1,error2,Kp,Ki);
                 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
         }