Diff: main.cpp

Revision:

41:e9fd0670f70c

Parent:

40:8b25c0531340

Child:

42:386fc2fcfb22

--- a/main.cpp	Thu Oct 29 14:52:07 2015 +0000
+++ b/main.cpp	Thu Oct 29 19:28:58 2015 +0000
@@ -8,7 +8,7 @@
 ///////////////////////////////////////////////info out
 HIDScope scope(6);// number of hidscope channels
 Ticker scope_ticker;//ticker for the scope
-const double scope_frequency=200; //HIDscope frequency 
+const double scope_frequency=200; //HIDscope frequency
 
 Timer checktimer;// timer to check how much time it takes to run the control loop, to see if the frequencies are not too high
 volatile double checktimervalue=0; // make a variable to store the time the loop has taken
@@ -21,8 +21,8 @@
 
 
 /////////////////////////////////////////////ENCODERS
-const float cpr_sensor=32; //counts per rotation of the sensor
-const float cpr_shaft=cpr_sensor*131;//counts per rotation of the outputshaft
+const double cpr_sensor=32; //counts per rotation of the sensor
+const double cpr_shaft=cpr_sensor*131;//counts per rotation of the outputshaft
 
 QEI encoder1(D13,D12,NC,cpr_sensor);/// encoders on motors X2 encoding
 QEI encoder2(D10,D11,NC,cpr_sensor);// first is pin a, pin b and the second is pin b, pin a so the encoders give positive rotation when the pod is moved forward
@@ -68,9 +68,9 @@
 const double Ts_control=1.0/control_frequency;//sample time of the controller
 
 double error1=0,//controller error storage variables
-      error2=0;
+       error2=0;
 
-InterruptIn valuechangebutton(PTC6);//button to change filter gains per arm via Serial 
+InterruptIn valuechangebutton(PTC6);//button to change filter gains per arm via Serial
 
 //safetyandthreshold
 AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm
@@ -98,7 +98,7 @@
 double filter_extragain1=1,filter_extragain2=1; // this is a factor to increase the gain per arm, adustable via serial
 
 
-//////////////////////////////// POSITION AND ANGLE 
+//////////////////////////////// POSITION AND ANGLE
 const double safetymarginfield=0.05; //adjustable, tweak for maximum but safe range
 const double mm_per_sec_emg=0.1;// move the pod 100 mm per sec if muscle is flexed
 const double y_start=0.225;//starting y position of the pod
@@ -110,13 +110,37 @@
 double desired_angle1=0; //desired angle of arm 1 (calculated with anglepos)
 double desired_angle2=0; // desired anvle of arm 2 (calculated with anglepos)
 
-const float fieldwidth=0.473; // width of the field
-const float maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield
+const double fieldwidth=0.473; // width of the field
+const double maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield
 
 angleandposition anglepos;// initiate the angle and position calculation library
 
 const double radtodeg=(180/PIE); // to convert radians to degrees
 
+
+/////////////////////////////////////////////////////////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_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);
+
+
+int demo_step=(0.125*demo_length_vector);
+
+double x_demo[demo_length_vector_int];
+double y_demo[demo_length_vector_int];
+
+Ticker demo_next_step_ticker;
+const double demo_next_step_frequency=(demo_length_vector/demo_total_time);
+
+
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
 volatile bool scopedata_go=false,
               control_go=false,
@@ -125,7 +149,8 @@
               readsignal_go=false,
               switchedmode=true,
               readbuttoncalibrate_go=false,
-              ledblink_go=false;
+              ledblink_go=false,
+              demo_next_step_go=false;
 
 void scopedata_activate()
 {
@@ -155,6 +180,10 @@
 {
     ledblink_go=true;
 }
+void demo_next_step_activate()
+{
+    demo_next_step_go=true;
+}
 
 ////////////////////////FUNCTIONS
 //gather data and send to scope
@@ -225,9 +254,9 @@
 void valuechange()
 {
     mycontroller.STOP(); // stop motors
-       pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1); // print extra gain for rigth arm
+    pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1); // print extra gain for rigth arm
     pc.scanf("%f", &filter_extragain1); //read the input from serial and write to filter_extragain1
-    
+
     pc.printf("Extra gain for left arm is now %f, enter new value\n",filter_extragain2); // print extra gain for left arm
     pc.scanf("%f", &filter_extragain2);//read the input from serial and write to filter_extragain2
 }
@@ -235,7 +264,7 @@
 // shoot the pod forward
 void shoot()
 {
-    ledgreen=1; 
+    ledgreen=1;
     double time=0;
     double stepsize=(0.5*PIE)/(timetoshoot*control_frequency); //calculate stepsize
     double profile_angle=0;// used for the profilie
@@ -269,10 +298,10 @@
 
         time+=(Ts_control);// add time it should take to calculated time
         filtereverything(true);//set al filter variables to 0
-        
+
         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
-                                    // this is because the control loop takes far less time than Ts_control
+        // if this is not done the loop wil go to fast and the motors can't keep up
+        // this is because the control loop takes far less time than Ts_control
     }
     //back
     time=0; // reset time
@@ -293,7 +322,7 @@
 
         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());
@@ -382,7 +411,7 @@
     mycontroller.STOP(); // stop the controller
     switchedmode=true; // set switchedmode to true so in the main loop some statements are executed
     modecounter++; // increase counter
-    if (modecounter==5) { // reset counter if counter=5
+    if (modecounter==6) { // reset counter if counter=6
         modecounter=0;
     } else {
         modecounter=modecounter;
@@ -391,6 +420,16 @@
     // tried it with interruptin but didn't work, bt this works good
 }
 
+void demo_next_step()
+{
+    if (demo_step>(demo_length_vector-2)) {
+        demo_step=0;
+    } else {
+        demo_step++;
+    }
+
+}
+
 ///////////////////////////////////////////////////MAIN
 
 int main()
@@ -403,9 +442,31 @@
     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
-    
+
+    // 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
@@ -433,7 +494,7 @@
             }
             if (filter_go==true) {// filter the emg signal
                 // TIME THIS LOOP TAKES: 0.000173 SEC
-                filtereverything(false); 
+                filtereverything(false);
                 filter_go=false;
             }
             if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position
@@ -540,10 +601,38 @@
                 control_go=false;
             }
         }
+        ///////////////////////////////////DEMO MODE
+        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
+                ledred=ledblue=ledgreen=1;
+                led1=led2=0;
+                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;
+            }
+        }
         //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE
-        if(modecounter==4) { 
+        if(modecounter==5) {
             if (switchedmode==true) {
-                pc.printf("Calibrate the EMG signals and threshold");
+                pc.printf("Calibrate the EMG signals and threshold\n");
                 ledgreen=1;
                 ledred=0;
                 switchedmode=false;
@@ -553,15 +642,15 @@
                 ledred=!ledred;
                 ledblink_go=false;
             }
-             if (filter_go==true) {// filter the emg signal
+            if (filter_go==true) {// filter the emg signal
                 // TIME THIS LOOP TAKES: 0.000173 SEC
                 filtereverything(false);
                 filter_go=false;
             }
         }
         checktimervalue=checktimer.read(); // write the time it has taken to a variable, so this variable can be displayed using hidschope.
-                                            // if chectimer.read() is used in scopedata, the value is probably ~0 
-                                            //because scopedata is called as one of the first functoins
+        // if chectimer.read() is used in scopedata, the value is probably ~0
+        //because scopedata is called as one of the first functoins
         checktimer.stop();
     }
 }