Diff: main.cpp

Revision:

11:c10651055871

Parent:

10:9e96d14d7034

Child:

12:b9f0b92bd659

--- a/main.cpp	Wed Oct 14 15:14:59 2015 +0000
+++ b/main.cpp	Mon Oct 19 08:23:58 2015 +0000
@@ -74,12 +74,17 @@
 Ticker readsignal_ticker;
 const double readsignal_frequency=25;
 
-DigitalOut led1(D8);
+DigitalOut led1(PTC12);
 DigitalOut led2(D9);
 
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
 float desired_position=0;
 float desired_angle[]= {0,0};
+float mm_per_sec_emg=50;// move the pod 50 mm per sec if muscle is flexed
+float fieldwidth=473;
+float safetymarginfield=75; //adjustable, tweak for maximum but safe range
+float maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield
+float rad_per_sec_emg=0.25*PIE;// THIS ONE IS NOT NESSECARY FOR ACTUAL PROGRAM
 
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
 volatile bool scopedata_go=false,
@@ -195,19 +200,69 @@
     pc.printf("Extra gain is now %f, enter new value\n",filter_extragain);
     pc.scanf("%f", &filter_extragain);
 }
+
+
+
+const float schiethoek=0.35*PIE;
+const float schiettijd=0.5;
+void shoot() // THIS NEEDS ADJUSTMEND
+{    
+    pc.printf("SHOOT\n");
+    //hoeken groter maken
+    desired_angle[0]-=schiethoek;
+    desired_angle[1]+=schiethoek;
+
+    Timer schiettimer;
+    schiettimer.reset();
+    schiettimer.start();
+    float pass=0;
+    while(schiettimer.read()<=schiettijd) {
+// errors berekenen en naar de controller passen
+       float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
+       float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
+        mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int, error2_int);
+        scopedata();
+        wait (Ts_control-(schiettimer.read()-Ts_control*pass)); // even wachten anders wordt de while loop te snel doorlopen en gaan de motoren wak
+        pass++;
+    }
+    schiettimer.stop();
+
+    //terug na schieten
+    desired_angle[0]+=schiethoek;
+    desired_angle[1]-=schiethoek;
+}
+
 void readsignal()
 {
+    //check if pod has to shoot
     if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
         led1=led2=1;
+        shoot();
+    // check if pod has to move to the right
     } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
         led1=1;
         led2=0;
+        desired_position += (rad_per_sec_emg/readsignal_frequency);// move desiredposition right ADJUS TO MM IN LAST VERSEION
+        if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
+            desired_position=maxdisplacement;
+        } else {
+            desired_position=desired_position;
+        }
+    // check if pod has to move to the left
     } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
         led1=0;
         led2=1;
+        desired_position -= (rad_per_sec_emg/readsignal_frequency);//move desiredposition left ADJUST TO MM IN FINAL VERSION
+        if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
+        desired_position=(-1*maxdisplacement);
+        } else {
+            desired_position=desired_position;
+        }
     } else {
         led1=led2=0;
     }
+    desired_angle[0]=(-1*desired_position);// REMOVE IN FINAL VERSION
+    desired_angle[1]=desired_position;//REMOVE IN FINAL VERSION
 }
 
 void changemode()
@@ -261,8 +316,8 @@
                 filter_go=false;
             }
             if (control_go==true) {
-                float error1=0;
-                float error2=0;
+                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
+                float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
             }
@@ -279,7 +334,7 @@
                 switchedmode=false;
             }
             if (control_go==true) {
-                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());;
+                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
                 float error2=0;// this is the error you want to use
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;