Diff: main.cpp

Revision:

19:6f22b5687587

Parent:

18:1c3254a32fd1

Child:

20:c5fb2ff5d457

--- a/main.cpp	Mon Oct 19 15:06:03 2015 +0000
+++ b/main.cpp	Tue Oct 20 12:47:08 2015 +0000
@@ -92,11 +92,13 @@
 float fieldwidth=0.473;
 float safetymarginfield=0.075; //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
+
 
 angleandposition anglepos;
 float y_start=0.255;
 float y_punch=0.473;
+float timetoshoot=0.5;
+
 
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
 volatile bool scopedata_go=false,
@@ -155,8 +157,6 @@
 /////filter
 void filtereverything()
 {
-    //filter_timer.reset();
-    // filter_timer.start();
     //pass1 so f1
     double pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
     double pass1_emg2 = myfilter2.filter(emg2_input.read(), v1_f1_emg2 , v2_f1_emg2 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
@@ -192,12 +192,8 @@
 
     filteredsignal1=(pass7_emg1*9e11*filter_extragain);
     filteredsignal2=(pass7_emg2*9e11*filter_extragain);
-
-    //filter_timer.stop();
 }
 
-
-
 //adjust controller values when sw2 is pressed
 void valuechange()
 {
@@ -215,34 +211,46 @@
     pc.scanf("%f", &filter_extragain);
 }
 
-const float schiethoek=0.35*PIE;
-const float schiettijd=0.5;
-void shoot() // THIS NEEDS ADJUSTMEND
+
+void shoot()
 {
-    pc.printf("SHOOT\n");
-    //hoeken groter maken
-    desired_angle[0]-=schiethoek;
-    desired_angle[1]+=schiethoek;
+    ledgreen=1;
+    float time=0;
+    float stepsize=(y_punch-y_start)/(timetoshoot*control_frequency);
+    float y_during_punch=y_start;
 
-    Timer schiettimer;
-    schiettimer.reset();
-    schiettimer.start();
-    float pass=0;
-    while(schiettimer.read()<=schiettijd) {
-// errors berekenen en naar de controller passen
+    Timer shoottimer;
+    shoottimer.reset();
+    shoottimer.start();
+    int count=0;
+    while (time<=timetoshoot) {
+        ledblue=!ledblue;
+        y_during_punch+=stepsize;
+        if (y_during_punch>=y_punch) {
+            y_during_punch=y_punch;
+        } else {
+            y_during_punch=y_during_punch;
+        }
+
+        desired_angle[0]=anglepos.positiontoangle1(desired_position,y_during_punch);
+        desired_angle[1]=anglepos.positiontoangle2(desired_position,y_during_punch);
+
         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
-        ledblue=!ledblue;
-        pass++;
+
+        mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
+scopedata();
+
+        time+=(Ts_control);
+        wait(time-shoottimer.read());
+        count++;
+        pc.printf("angle 1 =%f angle 2=%f\n",desired_angle[0],desired_angle[1]);
+
     }
-    schiettimer.stop();
+    shoottimer.stop();
     ledblue=1;
-    //terug na schieten
-    desired_angle[0]+=schiethoek;
-    desired_angle[1]-=schiethoek;
+    ledgreen=0;
+
 }
 
 ////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION
@@ -279,6 +287,7 @@
 ///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION
 void readsignalbutton()
 {
+    //write value of button to variable
     int buttonr=buttonR.read();
     int buttonl=buttonL.read();
     //check if pod has to shoot
@@ -336,6 +345,7 @@
     readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
     ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
 
+    pc.baud(115200);
     while(1) {
         if (changemodebutton==0) {
             changemode();
@@ -440,6 +450,7 @@
         ///////////////////////////////BUTTONCONTROLMODE
         if (modecounter==3) {
             if (switchedmode==true) {
+                pc.printf("Buttonmode, you can use the buttons to control the robot\n");
                 led1=led2=0;
                 ledred=ledblue=1;
                 switchedmode=false;