Diff: main.cpp

Revision:

23:1c4a18799464

Parent:

22:b29ba919d93e

Child:

24:dd75c961ae88

diff -r b29ba919d93e -r 1c4a18799464 main.cpp
--- a/main.cpp	Tue Oct 20 13:56:29 2015 +0000
+++ b/main.cpp	Tue Oct 20 14:45:02 2015 +0000
@@ -7,7 +7,7 @@
 
 ///////////////////////////////////////////////info out
 HIDScope scope(6);// number of hidscope channels
-const double scope_frequency=500; //HIDscope frequency
+const double scope_frequency=50; //HIDscope frequency
 
 Serial pc(USBTX,USBRX);// serial connection to pc
 
@@ -109,6 +109,8 @@
 
 angleandposition anglepos;// initiate the angle and position calculation library
 
+const float radtodeg=(180/PIE);
+
 
 
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
@@ -155,10 +157,10 @@
 void scopedata()
 {
     scope.set(0,desired_position);
-    scope.set(1,desired_angle1);
-    scope.set(2,desired_angle2);
-    scope.set(3,error1);
-    scope.set(4,error2);
+    scope.set(1,desired_angle1*radtodeg);
+    scope.set(2,desired_angle2*radtodeg);
+    scope.set(3,error1*radtodeg);
+    scope.set(4,error2*radtodeg);
     scope.send();
 }
 //read potmeters and adjust the safetyfactor and threshold
@@ -235,6 +237,7 @@
     Timer shoottimer;
     shoottimer.reset();
     shoottimer.start();
+    //forward
     while (time<=timetoshoot) {
         ledblue=!ledblue;
         
@@ -257,6 +260,31 @@
         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
     }
+    //back
+    time=0;
+    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;
+        } else {
+            y_during_punch=y_during_punch;
+        }
+
+        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());
+
+        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;