Diff: main.cpp

Revision:

35:0141a1fe8138

Parent:

34:bc2d64e86cb9

Child:

36:3856509519cf

diff -r bc2d64e86cb9 -r 0141a1fe8138 main.cpp
--- a/main.cpp	Thu Oct 29 10:35:42 2015 +0000
+++ b/main.cpp	Thu Oct 29 12:44:46 2015 +0000
@@ -7,44 +7,46 @@
 
 ///////////////////////////////////////////////info out
 HIDScope scope(6);// number of hidscope channels
-const double scope_frequency=500; //HIDscope frequency
-Timer checktimer;
-volatile double checktimervalue=0;
+Ticker scope_ticker;//ticker for the scope
+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
 
 Serial pc(USBTX,USBRX);// serial connection to pc
 
-DigitalOut ledred(LED_RED);
+DigitalOut ledred(LED_RED);// leds on the k64f board
 DigitalOut ledgreen(LED_GREEN);
 DigitalOut ledblue(LED_BLUE);
 
-Ticker scope_ticker;
+
 /////////////////////////////////////////////ENCODERS
-const float cpr_sensor=32;
-const float cpr_shaft=cpr_sensor*131;//counts per rotation of the sensor
+const float cpr_sensor=32; //counts per rotation of the sensor
+const float 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);
+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
 
-const double PIE=3.14159265359;
-const float counttorad=((2*PIE)/cpr_shaft);// counts per rotation of the shaft
+const double PIE=3.14159265359; // pi, for calculations
+const float counttorad=((2*PIE)/cpr_shaft);// to convert counts to rotation in rad
 
 
 /////////////////////////////////CALIBRATION (MODE)
 const double radpersec_calibrate=0.1*PIE;// speed of arms when in calibration mode
 int modecounter=1;//counter in which mode the robot is
-const double readbuttoncalibrate_frequency=10;//frequency at which the buttons are read when in calibration mode
+const double readbuttoncalibrate_frequency=50;//frequency at which the buttons are read when in calibration mode
 const double ledblink_frequency=4;//frequency at which the green led and leds on top blink when in resp button or calibration mode
 
 DigitalIn changemodebutton(PTA4);// button to change mode (sw3)
-Ticker readbuttoncalibrate_ticker;
-Ticker ledblink_ticker;
+Ticker readbuttoncalibrate_ticker;//ticker for reading out the buttons when calibrating
+Ticker ledblink_ticker;// ticker for blinking the leds
 
 DigitalIn buttonR(D2);//rigth button on biorobotics shield
 DigitalIn buttonL(D3);//left button on biorobotics shield
 
 /////////////////READSIGNAL
 const double readsignal_frequency=100;//frequency at wich the filtered emg signal is sampled to be 0 or 1
-Ticker readsignal_ticker;
+Ticker readsignal_ticker; // ticker for reading the signal of the emg
 
 
 DigitalOut led1(PTC12);// rigth led on biorobotics shield
@@ -60,18 +62,15 @@
 float Ki_move=1.25;
 float Kd_move=0.05;
 
-float factor_taup=1.5;
-float tau_p=1.0/(factor_taup*control_frequency);
 controlandadjust mycontroller(2,control_frequency); // make a controller, value in brackets is errorband in degrees and controller frequency
 
-Ticker control_ticker;
-
-const double Ts_control=1.0/control_frequency;
+Ticker control_ticker;//ticker for the controller
+const double Ts_control=1.0/control_frequency;//sample time of the controller
 
 float error1=0,//controller error storage variables
       error2=0;
 
-InterruptIn valuechangebutton(PTC6);//button to change controller constants
+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
@@ -83,10 +82,10 @@
 float threshold_value=1;//initial threshold value
 
 ////////////////////////////////FILTER
-const double filter_frequency=500;
-#include "filtervalues.h"// call the values for the biquads
+const double filter_frequency=500; // frequency at which the emg signal is filtered
+#include "filtervalues.h"// call the values for the biquads these are in a separate file to keep this code readable
 
-Ticker filter_ticker;
+Ticker filter_ticker;//Ticker for the filter
 
 Biquad myfilter1;// make filter for signal 1
 Biquad myfilter2;//make filter for signal 2
@@ -96,7 +95,7 @@
 
 volatile double filteredsignal1=0;//the first filtered emg signal
 volatile double filteredsignal2=0;//the second filtered emg signal
-float filter_extragain1=1,filter_extragain2=1;
+float filter_extragain1=1,filter_extragain2=1; // this is a factor to increase the gain per arm, adustable via serial
 
 
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
@@ -107,18 +106,16 @@
 const double timetoshoot=0.35;// time it can take to shoot
 const double timetogoback=1;// time it can take to go back after shooting
 
-double desired_position=0;
-double desired_angle1=0;
-double desired_angle2=0;
+volatile double desired_position=0;//desired x position
+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;
+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
 
 angleandposition anglepos;// initiate the angle and position calculation library
 
-const float radtodeg=(180/PIE);
-
-
+const float radtodeg=(180/PIE); // to convert radians to degrees
 
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
 volatile bool scopedata_go=false,
@@ -163,13 +160,13 @@
 //gather data and send to scope
 void scopedata(double wanted_y)
 {
-    scope.set(0,desired_position);
-    scope.set(1,wanted_y);
-    scope.set(2,filteredsignal1);
-    scope.set(3,filteredsignal2);
-    scope.set(4,threshold_value);
-    scope.set(5,mycontroller.motor2pwm());
-    scope.send();
+    scope.set(0,desired_position); // desired x position
+    scope.set(1,wanted_y); // desired y position
+    scope.set(2,filteredsignal1); // filterded emg signal rigth arm
+    scope.set(3,filteredsignal2); // filtered emg signal left arm
+    scope.set(4,threshold_value); // threshold value
+    scope.set(5,mycontroller.motor1pwm()); //pwm signal send to motor 1
+    scope.send(); // send info to HIDScope server
 }
 //read potmeters and adjust the safetyfactor and threshold
 void safetyandthreshold()
@@ -213,10 +210,10 @@
     double pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
     double pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
 
-    filteredsignal1=(pass7_emg1*9e11*filter_extragain1);
-    filteredsignal2=(pass7_emg2*9e11*filter_extragain2);
+    filteredsignal1=(pass7_emg1*9e11*filter_extragain1);//this is needed to make the signal ~1 when flexed
+    filteredsignal2=(pass7_emg2*9e11*filter_extragain2);// filter_extragain is adjusted via serial
 
-    if (makeempty==true) {//this is needed so the filtered value is nog high after shooting
+    if (makeempty==true) {//this is needed so the filtered value is not high after shooting basically it resets the filter
         pass1_emg1 = pass1_emg2 =pass2_emg1 =pass2_emg2 =pass3_emg1 = pass3_emg2 =pass4_emg1 =pass4_emg2 =pass5_emg1 =pass5_emg2 =0;
         pass5_emg1_abs=pass5_emg2_abs=pass6_emg1 =pass6_emg2 =pass7_emg1=pass7_emg2 =filteredsignal1=filteredsignal2=0;
         v1_f1_emg1=v1_f1_emg2=v1_f2_emg1=v1_f2_emg2=v1_f3_emg1=v1_f3_emg2=v1_f4_emg1=v1_f4_emg2=v1_f5_emg1=0;
@@ -227,29 +224,29 @@
 //adjust controller values when sw2 is pressed
 void valuechange()
 {
-    mycontroller.STOP();
-       pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1);
-    pc.scanf("%f", &filter_extragain1);
+    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.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);
-    pc.scanf("%f", &filter_extragain2);
+    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
 }
 
 // shoot the pod forward
 void shoot()
 {
-    ledgreen=1;
+    ledgreen=1; 
     double time=0;
-    double stepsize=(0.5*PIE)/(timetoshoot*control_frequency);
-    double profile_angle=0;
-    double x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
-    double y_during_punch=y_start;
-    Timer shoottimer;
+    double stepsize=(0.5*PIE)/(timetoshoot*control_frequency); //calculate stepsize
+    double profile_angle=0;// used for the profilie
+    double x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());// calculate actual x positon from the encoder angles
+    double y_during_punch=y_start;//set initial y value to baseline
+    Timer shoottimer; // make a timer
     shoottimer.reset();
     shoottimer.start();
     //forward
     while (time<=timetoshoot) {
-        ledblue=!ledblue;
+        ledblue=!ledblue; // blink blue led (very fast, so not noticable blinking)
 
         profile_angle+=stepsize; // add stepsize to angle
 
@@ -271,18 +268,20 @@
         scopedata(y_during_punch);//send data to hidscope   WARING lower freqyency than normal
 
         time+=(Ts_control);// add time it should take to calculated time
-        //pc.printf("Time = %f\n",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
+        
+        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
     }
     //back
-    time=0;
-    shoottimer.reset();
-    stepsize=(PIE)/(timetogoback*control_frequency);
-    profile_angle=0;
-    desired_position=0;
+    time=0; // reset time
+    shoottimer.reset();//reset timer
+    stepsize=(PIE)/(timetogoback*control_frequency);//calculate stepsize
+    profile_angle=0;//reset angle for the displacement profile
+    desired_position=0;// set desired x position to 0, so the robot is in the middle in front of the goal after shooting
     while (time<=timetogoback) {
-        ledblue=!ledblue;
+        ledblue=!ledblue;// blink blue led
 
         profile_angle+=stepsize; // add stepsize to y position
         y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting
@@ -303,13 +302,12 @@
         scopedata(y_during_punch);//send data to hidscope   WARING lower freqyency than normal
 
         time+=(Ts_control);// add time it should take to calculated time
-        //pc.printf("Time = %f\n",time);
-        filtereverything(false);
+        filtereverything(false);//start filtering the signal (lower frewquency than normal)
         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
     }
     shoottimer.stop();
-    ledblue=1;
-    ledgreen=0;
+    ledblue=1; // blue led off
+    ledgreen=0; // green led on
 }
 
 ////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION
@@ -317,11 +315,11 @@
 {
     //check if pod has to shoot
     if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
-        led1=led2=1;
+        led1=led2=1; // ligth up  both leds
         shoot();
         // check if pod has to move to the right
     } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
-        led1=1;
+        led1=1;// ligth up rigth led
         led2=0;
         desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
         if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
@@ -331,7 +329,7 @@
         }
         // check if pod has to move to the left
     } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
-        led1=0;
+        led1=0; // ligth up left led
         led2=1;
         desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
         if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
@@ -345,7 +343,7 @@
 }
 
 ///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION
-void readsignalbutton()
+void readsignalbutton() // same as above, only with buttons as input
 {
     //write value of button to variable
     int buttonr=buttonR.read();
@@ -381,23 +379,23 @@
 
 void changemode()   //this makes the counter higher to switch between modes
 {
-    mycontroller.STOP();
-    switchedmode=true;
-    modecounter++;
-    if (modecounter==5) {
+    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
         modecounter=0;
     } else {
         modecounter=modecounter;
     }
     wait(1);// needed because else it is checked too fast if the button is pressed and modes change too fast
-    // tried it with interruptin but dinn't work
+    // tried it with interruptin but didn't work, bt this works good
 }
 
 ///////////////////////////////////////////////////MAIN
 
 int main()
 {
-    //tickers
+    //initiate tickers
     safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
     filter_ticker.attach(&filter_activate,1.0/filter_frequency);
     control_ticker.attach(&control_activate,1.0/control_frequency);
@@ -407,36 +405,35 @@
     ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
 
     pc.baud(115200);//set baudrate to 115200
-    while(1) {
-        valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
-        checktimer.reset();
+    
+    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
         checktimer.start();
         if (changemodebutton==0) {// check if the change mode button is pressed
-            changemode();
+            changemode();//call changemode
         }
         if (scopedata_go==true) {//send scopedata
             //TIME THIS LOOP TAKES 0.000008 SEC (PEAKS AT 0.000015)
-            scopedata(y_start);
+            scopedata(y_start);// call scopedata, use baseline as desired y position
             scopedata_go=false;
         }
         if (safetyandthreshold_go==true) {// check the potmeters
             //TIME THIS LOOP TAKES: 0.000032 SEC
-            safetyandthreshold();
+            safetyandthreshold(); // read out the potmeters on top of the biorobotics board
             safetyandthreshold_go=false;
         }
         ///////////////////////////////////////////NORMAL RUNNING MODE
         if(modecounter==0) {
             if (switchedmode==true) {
-                encoder1.reset();// reset encoders so they are at 0 degrees
-                encoder2.reset();
-                pc.printf("Program running\n");//
-                ledgreen=0;
-                led1=led2=ledred=ledblue=1;
+                pc.printf("Program running\n");// print to serial
+                ledgreen=0;// green led on
+                led1=led2=ledred=ledblue=1;//rest of
                 switchedmode=false;
             }
             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
@@ -455,7 +452,6 @@
                 mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
                 control_go=false;
             }
-           // valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
         }
         ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
         if (modecounter==1) {
@@ -481,7 +477,7 @@
             }
             if (control_go==true) {// calculate errors and send them to controllers
                 error1=(desired_angle1-counttorad*encoder1.getPulses());
-                error2=0;
+                error2=0;// only adjust rigth arm
                 mycontroller.PI(error1,error2,Kp_move,Ki_move);
                 control_go=false;
             }
@@ -509,7 +505,7 @@
                 }
             }
             if (control_go==true) {// calculate errors and send to controller
-                error1=0;
+                error1=0; // only adjus left arm
                 error2=(desired_angle2-counttorad*encoder2.getPulses());
                 mycontroller.PI(error1,error2,Kp_move,Ki_move);
                 control_go=false;
@@ -526,10 +522,10 @@
                 switchedmode=false;
             }
             if (ledblink_go==true) {
-                ledgreen=!ledgreen;
+                ledgreen=!ledgreen; // blink green led
                 ledblink_go=false;
             }
-            if (readsignal_go==true) {// read buttons and adjus wanted position
+            if (readsignal_go==true) {// read buttons and adjust wanted position
                 readsignalbutton();
                 readsignal_go=false;
             }
@@ -543,7 +539,8 @@
                 control_go=false;
             }
         }
-        if(modecounter==4) {
+        //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE
+        if(modecounter==4) { 
             if (switchedmode==true) {
                 pc.printf("Calibrate the EMG signals and threshold");
                 ledgreen=1;
@@ -561,7 +558,9 @@
                 filter_go=false;
             }
         }
-        checktimervalue=checktimer.read();
+        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
         checktimer.stop();
     }
 }