Diff: main.cpp

Revision:

25:21dcd3f9eac2

Parent:

24:dd75c961ae88

Child:

26:c935e39cce8a

--- a/main.cpp	Tue Oct 20 15:14:40 2015 +0000
+++ b/main.cpp	Fri Oct 23 10:18:21 2015 +0000
@@ -7,7 +7,9 @@
 
 ///////////////////////////////////////////////info out
 HIDScope scope(6);// number of hidscope channels
-const double scope_frequency=50; //HIDscope frequency
+const double scope_frequency=500; //HIDscope frequency
+Timer checktimer;
+volatile double checktimervalue=0;
 
 Serial pc(USBTX,USBRX);// serial connection to pc
 
@@ -41,7 +43,7 @@
 DigitalIn buttonL(D3);//left button on biorobotics shield
 
 //////////////////////////////////CONTROLLER
-const double control_frequency=50;// frequency at which the controller is called
+const double control_frequency=250;// frequency at which the controller is called
 //controller constants
 float Kp=1;
 float Ki=0.1;
@@ -162,6 +164,7 @@
     scope.set(2,desired_angle2*radtodeg);
     scope.set(3,error1*radtodeg);
     scope.set(4,error2*radtodeg);
+    scope.set(5,checktimervalue);
     scope.send();
 }
 //read potmeters and adjust the safetyfactor and threshold
@@ -241,7 +244,7 @@
     //forward
     while (time<=timetoshoot) {
         ledblue=!ledblue;
-        
+
         y_during_punch+=stepsize; // add stepsize to y position
         if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety
             y_during_punch=y_punch;
@@ -252,8 +255,8 @@
         desired_angle1=anglepos.positiontoangle1(desired_position,y_during_punch);// calculate desired angles
         desired_angle2=anglepos.positiontoangle2(desired_position,y_during_punch);
 
-         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
-         error2=(desired_angle2-counttorad*encoder2.getPulses());
+        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
         scopedata();//send data to hidscope   WARING lower freqyency than normal
@@ -266,7 +269,7 @@
     shoottimer.reset();
     while (time<=timetoshoot) {
         ledblue=!ledblue;
-        
+
         y_during_punch-=stepsize; // add stepsize to y position
         if (y_during_punch<=y_start) {//to check if y position is not smaller than y_start for safety
             y_during_punch=y_start;
@@ -277,15 +280,15 @@
         desired_angle1=anglepos.positiontoangle1(desired_position,y_during_punch);// calculate desired angles
         desired_angle2=anglepos.positiontoangle2(desired_position,y_during_punch);
 
-         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
-         error2=(desired_angle2-counttorad*encoder2.getPulses());
+        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
         scopedata();//send data to hidscope   WARING lower freqyency than normal
 
         time+=(Ts_control);// add time it should take to calculated time
         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();
     ledblue=1;
     ledgreen=0;
@@ -357,7 +360,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;
@@ -368,7 +371,7 @@
         modecounter=modecounter;
     }
     wait(1);// needed because else it is checked too fast if the button is pressed and modes change too fast
-            // tried it with interruptin but dinn't work
+    // tried it with interruptin but dinn't work
 }
 
 ///////////////////////////////////////////////////MAIN
@@ -386,17 +389,21 @@
 
     pc.baud(115200);//set baudrate to 115200
     while(1) {
+        checktimer.reset();
+       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)
             scopedata();
-            scopedata_go=false;
+            scopedata_go=false;             
         }
         if (safetyandthreshold_go==true) {// check the potmeters
+        //TIME THIS LOOP TAKES: 0.000032 SEC          
             safetyandthreshold();
             safetyandthreshold_go=false;
-        }
+                    }
         ///////////////////////////////////////////NORMAL RUNNING MODE
         if(modecounter==0) {
             if (switchedmode==true) {
@@ -408,14 +415,18 @@
                 switchedmode=false;
             }
             if (filter_go==true) {// filter the emg signal
+            // 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
                 readsignal();
                 readsignal_go=false;
-            }
+                           }
             if (control_go==true) {// calculate angles from positions and send error to controller
+            //TIME THIS LOOP TAKES: 0.000223 SEC
+            
                 desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
                 desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
 
@@ -423,7 +434,7 @@
                 float error2=(desired_angle2-counttorad*encoder2.getPulses());
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
-            }
+                            }
             valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
         }
         ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
@@ -448,9 +459,9 @@
                     readbuttoncalibrate_go=false;
                 }
             }
-             if (control_go==true) {// calculate errors and send them to controllers
-                 error1=(desired_angle1-counttorad*encoder1.getPulses());
-                 error2=0;
+            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);
                 control_go=false;
             }
@@ -467,7 +478,7 @@
                 led2=!led2;// blink left led on biorobotics shield (because left arm is being calibrated)
                 ledblink_go=false;
             }
-            if (readbuttoncalibrate_go==true) {// 
+            if (readbuttoncalibrate_go==true) {//
                 if (buttonR.read()==0 && buttonL.read()==1) {//check wich button is pressed and adjust wanted angle of left arm
                     desired_angle2 += (radpersec_calibrate/readbuttoncalibrate_frequency);
                     readbuttoncalibrate_go=false;
@@ -477,9 +488,9 @@
                     readbuttoncalibrate_go=false;
                 }
             }
-             if (control_go==true) {// calculate errors and send to controller
-                 error1=0;
-                 error2=(desired_angle2-counttorad*encoder2.getPulses());
+            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);
                 control_go=false;
             }
@@ -489,7 +500,7 @@
             if (switchedmode==true) {
                 pc.printf("Buttonmode, you can use the buttons to control the robot\n");
                 led1=led2=0;
-                ledred=ledblue=1;                
+                ledred=ledblue=1;
                 encoder1.reset();// reset encoders so they are at 0 degrees
                 encoder2.reset();
                 switchedmode=false;
@@ -506,13 +517,14 @@
                 desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
                 desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
 
-                 error1=(desired_angle1-counttorad*encoder1.getPulses());
-                 error2=(desired_angle2-counttorad*encoder2.getPulses());
+                error1=(desired_angle1-counttorad*encoder1.getPulses());
+                error2=(desired_angle2-counttorad*encoder2.getPulses());
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
             }
             valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
         }
-
+        checktimervalue=checktimer.read();
+        checktimer.stop();
     }
 }