Diff: main.cpp

Revision:

42:386fc2fcfb22

Parent:

41:e9fd0670f70c

Child:

43:a5f47a1f11d2

--- a/main.cpp	Thu Oct 29 19:28:58 2015 +0000
+++ b/main.cpp	Fri Oct 30 10:42:36 2015 +0000
@@ -119,27 +119,28 @@
 
 
 /////////////////////////////////////////////////////////DEMO MODE
-const double demo_safety=0.1;
-const double demo_x_edge_rigth=(fieldwidth/2)-demo_safety;
-const double demo_x_edge_left=-((fieldwidth/2)-demo_safety);
-const double demo_y_back=0.1+demo_safety;
+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_y_front=y_punch;
 
-const double demo_length_vector=1000;
-const int demo_length_vector_int=demo_length_vector;
-const double demo_total_time=10;
-const double demo_x_step=(demo_x_edge_rigth-demo_x_edge_left)/(0.25*demo_length_vector);
-const double demo_y_step=(demo_y_front-demo_y_back)/(0.25*demo_length_vector);
+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);
+int demo_step=(0.125*demo_length_vector);//start on x=0 and y=ypunch
 
-double x_demo[demo_length_vector_int];
+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);
+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,
@@ -420,6 +421,63 @@
     // tried it with interruptin but didn't work, bt this works good
 }
 
+void demo_make_vector()
+{
+    // make position vectors for demo mode
+    x_demo[0]=demo_x_edge_left;
+    y_demo[0]=demo_y_front;
+
+    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;
+    }
+}
+
+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);
+
+
+        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
+
+        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
+
+    }
+}
 void demo_next_step()
 {
     if (demo_step>(demo_length_vector-2)) {
@@ -446,27 +504,6 @@
 
     pc.baud(115200);//set baudrate to 115200
 
-    // make position vectors for demo mode
-    x_demo[0]=demo_x_edge_left;
-    y_demo[0]=demo_y_front;
-
-    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;
-    }
-
     while(1) {// while (1) so continious loop
         valuechangebutton.fall(&valuechange);// used to change the filter gains
         checktimer.reset();// reset the timer to check the time it takes to run the entire control loop
@@ -490,6 +527,8 @@
                 pc.printf("Program running\n");// print to serial
                 ledgreen=0;// green led on
                 led1=led2=ledred=ledblue=1;//rest of
+                gotogoal(y_start);
+                desired_position=0;
                 switchedmode=false;
             }
             if (filter_go==true) {// filter the emg signal
@@ -605,9 +644,11 @@
         if (modecounter==4) {
             if (switchedmode==true) {
                 pc.printf("Demo mode, the pod moves around the field\n");
-                demo_step=(0.125*demo_length_vector);// start in the middle of the playfield
+                demo_make_vector();
+                demo_step=(0.625*demo_length_vector);// start in the middle for the goal
                 ledred=ledblue=ledgreen=1;
                 led1=led2=0;
+                gotogoal(demo_y_back);
                 switchedmode=false;
             }
             if (ledblink_go==true) {
@@ -634,7 +675,7 @@
             if (switchedmode==true) {
                 pc.printf("Calibrate the EMG signals and threshold\n");
                 ledgreen=1;
-                ledred=0;
+                ledred=ledblue=0;
                 switchedmode=false;
             }
             if(ledblink_go==true) {