Diff: main.cpp

Revision:

43:a5f47a1f11d2

Parent:

42:386fc2fcfb22

Child:

44:fd7b3ace6c19

--- a/main.cpp	Fri Oct 30 10:42:36 2015 +0000
+++ b/main.cpp	Mon Nov 02 11:48:52 2015 +0000
@@ -122,25 +122,12 @@
 const double demo_safety=0.05;// safety factor to stay off the edges
 const double demo_x_edge_rigth=(fieldwidth/2)-demo_safety;// rigth edge of the field
 const double demo_x_edge_left=-((fieldwidth/2)-demo_safety);// left edge of te field
-const double demo_y_back=0.1+demo_safety;// edge of the field near goal
+const double demo_goal_edge_rigth=0.1+demo_safety;// x edges of goal
+const double demo_goal_edge_left=-(0.1+demo_safety);
+const double demo_y_goal=0.1+demo_safety;// y position in front of goal
+const double demo_y_back=0.05+demo_safety;//back edge of the field 
 const double demo_y_front=y_punch;
 
-const double demo_length_vector=1000;// number of steps in the demo mode
-const int demo_length_vector_int=demo_length_vector;//int value to make vectors
-const double demo_total_time=10;// time the pod takes to travel the entire path
-const double demo_x_step=(demo_x_edge_rigth-demo_x_edge_left)/(0.25*demo_length_vector);// x discance to travel per step
-const double demo_y_step=(demo_y_front-demo_y_back)/(0.25*demo_length_vector);//y distan e to travel per step
-
-
-int demo_step=(0.125*demo_length_vector);//start on x=0 and y=ypunch
-
-double x_demo[demo_length_vector_int];// empty vector for storing x and y values of the path
-double y_demo[demo_length_vector_int];
-
-Ticker demo_next_step_ticker;
-const double demo_next_step_frequency=(demo_length_vector/demo_total_time);//frequency at which the next step in the path is used
-
-const double time_to_goal=1;
 
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
 volatile bool scopedata_go=false,
@@ -421,72 +408,37 @@
     // tried it with interruptin but didn't work, bt this works good
 }
 
-void demo_make_vector()
+void gotopos(double desired_x_pos, double desired_y_pos, double time_to_pos)
 {
-    // make position vectors for demo mode
-    x_demo[0]=demo_x_edge_left;
-    y_demo[0]=demo_y_front;
+    double timepos=0;
+    Timer postimer;
+    double pos_x_start=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
+    double pos_y_start=anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
+
+    double pos_stepsize_x=(pos_x_start-desired_x_pos)/(time_to_pos*control_frequency);
+    double pos_stepsize_y=(pos_y_start-desired_y_pos)/(time_to_pos*control_frequency);
 
-    for (int i=0; i<(0.25*demo_length_vector_int); i++) {
-        x_demo[i+1]=x_demo[i]+demo_x_step;
-        y_demo[i+1]=demo_y_front;
-    }
-    for (int i=(0.25*demo_length_vector_int); i<(0.5*demo_length_vector_int); i++) {
-        x_demo[i+1]=demo_x_edge_rigth;
-        y_demo[i+1]=y_demo[i]-demo_y_step;
-    }
-    for (int i=(0.5*demo_length_vector_int); i<(0.75*demo_length_vector_int); i++) {
-        x_demo[i+1]=x_demo[i]-demo_x_step;
-        y_demo[i+1]=demo_y_back;
-    }
-    for (int i=(0.75*demo_length_vector_int); i<(1*(demo_length_vector_int-1)); i++) {
-        x_demo[i+1]=demo_x_edge_left;
-        y_demo[i+1]=y_demo[i]+demo_y_step;
-    }
-}
+    double pos_x_moving=pos_x_start;
+    double pos_y_moving=pos_y_start;
+    postimer.start();
+    while(timepos<=time_to_pos) {
+        pos_x_moving-=pos_stepsize_x;
+        pos_y_moving-=pos_stepsize_y;
 
-void gotogoal(double desired_y_position_for_goal)
-{
-
-    double timegoal=0;
-    Timer goaltimer;
-    double goal_x_start=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
-    double goal_y_start=anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
-    double goal_stepsize_x=(goal_x_start-0)/(time_to_goal*control_frequency);
-    double goal_stepsize_y=(goal_y_start-desired_y_position_for_goal)/(time_to_goal*control_frequency);
-
-    double goal_x_going_to_goal=goal_x_start;
-    double goal_y_going_to_goal=goal_y_start;
-    goaltimer.start();
-    while(timegoal<=time_to_goal) {
-        goal_x_going_to_goal-=goal_stepsize_x;
-        goal_y_going_to_goal-=goal_stepsize_y;
-
-
-        desired_angle1=anglepos.positiontoangle1(goal_x_going_to_goal,goal_y_going_to_goal);// calculate desired angles
-        desired_angle2=anglepos.positiontoangle2(goal_x_going_to_goal,goal_y_going_to_goal);
-
+        desired_angle1=anglepos.positiontoangle1(pos_x_moving,pos_y_moving);// calculate desired angles
+        desired_angle2=anglepos.positiontoangle2( pos_x_moving,pos_y_moving);
 
         error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
         error2=(desired_angle2-counttorad*encoder2.getPulses());
 
         mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
 
-        timegoal+=(Ts_control);// add time it should take to calculated time
+        timepos+=(Ts_control);// add time it should take to calculated time
 
-        wait(timegoal-goaltimer.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
+        wait(timepos-postimer.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
 
     }
 }
-void demo_next_step()
-{
-    if (demo_step>(demo_length_vector-2)) {
-        demo_step=0;
-    } else {
-        demo_step++;
-    }
-
-}
 
 ///////////////////////////////////////////////////MAIN
 
@@ -500,7 +452,6 @@
     readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
     readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
     ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
-    demo_next_step_ticker.attach(&demo_next_step_activate,1.0/demo_next_step_frequency);
 
     pc.baud(115200);//set baudrate to 115200
 
@@ -527,7 +478,7 @@
                 pc.printf("Program running\n");// print to serial
                 ledgreen=0;// green led on
                 led1=led2=ledred=ledblue=1;//rest of
-                gotogoal(y_start);
+                gotopos(0,demo_y_goal,1);
                 desired_position=0;
                 switchedmode=false;
             }
@@ -644,31 +595,20 @@
         if (modecounter==4) {
             if (switchedmode==true) {
                 pc.printf("Demo mode, the pod moves around the field\n");
-                demo_make_vector();
-                demo_step=(0.625*demo_length_vector);// start in the middle for the goal
-                ledred=ledblue=ledgreen=1;
+                ledred=ledblue=ledgreen=0;
+                gotopos(0,demo_y_goal,1); // go to goal @x=0
                 led1=led2=0;
-                gotogoal(demo_y_back);
                 switchedmode=false;
             }
-            if (ledblink_go==true) {
-                ledred=ledblue=ledgreen=!ledred;//blink all leds
-                ledblink_go=false;
-            }
-            if (demo_next_step_go==true) {
-                demo_next_step();// next step of the path
-                pc.printf("step %d x %f y %f \n",demo_step,x_demo[demo_step],y_demo[demo_step] );
-                demo_next_step_go=false;
-            }
-            if (control_go==true) {// calculate wanted angles from position, errors and send to controller
-                desired_angle1=anglepos.positiontoangle1(x_demo[demo_step],y_demo[demo_step]);
-                desired_angle2=anglepos.positiontoangle2(x_demo[demo_step],y_demo[demo_step]);
-
-                error1=(desired_angle1-counttorad*encoder1.getPulses());
-                error2=(desired_angle2-counttorad*encoder2.getPulses());
-                mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
-                control_go=false;
-            }
+            gotopos(demo_goal_edge_left,demo_y_goal,0.25);
+            gotopos(demo_goal_edge_left,demo_y_back,0.25);
+            gotopos(demo_x_edge_left,demo_y_back,0.5);
+            gotopos(demo_x_edge_left,demo_y_front,2);
+            gotopos(demo_x_edge_rigth,demo_y_front,2);
+            gotopos(demo_x_edge_rigth,demo_y_back,2);
+            gotopos(demo_goal_edge_rigth,demo_y_back,1.25);
+            gotopos(demo_goal_edge_rigth,demo_y_goal,0.25);
+            gotopos(0,demo_y_goal,0.5);
         }
         //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE
         if(modecounter==5) {