Diff: main.cpp

Revision:

20:c5fb2ff5d457

Parent:

19:6f22b5687587

Child:

21:6954cc25f2a7

--- a/main.cpp	Tue Oct 20 12:47:08 2015 +0000
+++ b/main.cpp	Tue Oct 20 13:16:37 2015 +0000
@@ -5,47 +5,50 @@
 #include "controlandadjust.h"
 #include "angleandposition.h"
 
-//info out
-HIDScope scope(6);
-Ticker scope_ticker;
-const double scope_frequency=500;
-Serial pc(USBTX,USBRX);
+///////////////////////////////////////////////info out
+HIDScope scope(6);// number of hidscope channels
+const double scope_frequency=500; //HIDscope frequency
+
+Serial pc(USBTX,USBRX);// serial connection to pc
 
 DigitalOut ledred(LED_RED);
 DigitalOut ledgreen(LED_GREEN);
 DigitalOut ledblue(LED_BLUE);
 
-////////////////ENCODERS
+Ticker scope_ticker;
+/////////////////////////////////////////////ENCODERS
 const float cpr_sensor=32;
-const float cpr_shaft=cpr_sensor*131;
-QEI encoder1(D13,D12,NC,cpr_sensor);/// maybe use Encoder in stead of QEI, because Encoder had setposition
+const float cpr_shaft=cpr_sensor*131;//counts per rotation of the sensor
+
+QEI encoder1(D13,D12,NC,cpr_sensor);/// encoders on motors X2 encoding
 QEI encoder2(D10,D11,NC,cpr_sensor);
+
 const double PIE=3.14159265359;
-const float counttorad=((2*PIE)/cpr_shaft);
+const float counttorad=((2*PIE)/cpr_shaft);// counts per rotation of the shaft
 
 /////////////////////////////////CALIBRATION (MODE)
-int modecounter=1;
-DigitalIn changemodebutton(PTA4);
+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 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;
-const double readbuttoncalibrate_frequency=10;
-
 Ticker ledblink_ticker;
-const double ledblink_frequency=4;
 
-const double radpersec_calibrate=0.1*PIE;
-
-DigitalIn buttonR(D2);
-DigitalIn buttonL(D3);
+DigitalIn buttonR(D2);//rigth button on biorobotics shield
+DigitalIn buttonL(D3);//left button on biorobotics shield
 
 //////////////////////////////////CONTROLLER
-controlandadjust mycontroller; // make a controller
+const double control_frequency=25;// frequency at which the controller is called
 //controller constants
 float Kp=0.5;
 float Ki=0.01;
 float Kd=0.001;
+
+controlandadjust mycontroller; // make a controller
 Ticker control_ticker;
-const double control_frequency=25;
+
 const double Ts_control=1.0/control_frequency;
 
 float error1_int=0;// storage variables for the errors
@@ -65,39 +68,45 @@
 float threshold_value=1;//initial threshold value
 
 ////////////////////////////////FILTER
-#include "filtervalues.h"
+const double filter_frequency=500;
+#include "filtervalues.h"// call the values for the biquads
+
 Ticker filter_ticker;
-const double filter_frequency=500;
-Biquad myfilter1;
-Biquad myfilter2;
 
-AnalogIn emg1_input(A0);
-AnalogIn emg2_input(A1);
+Biquad myfilter1;// make filter for signal 1
+Biquad myfilter2;//make filter for signal 2
 
-double filteredsignal1=0;
-double filteredsignal2=0;
+AnalogIn emg1_input(A0);//input for first emg signal
+AnalogIn emg2_input(A1);//input for second emg signal
+
+volatile double filteredsignal1=0;//the first filtered emg signal
+volatile double filteredsignal2=0;//the second filtered emg signal
 float filter_extragain=1;
 
 /////////////////READSIGNAL
+const double readsignal_frequency=25;//frequency at wich the filtered emg signal is sampled to be 0 or 1
 Ticker readsignal_ticker;
-const double readsignal_frequency=25;
 
-DigitalOut led1(PTC12);
-DigitalOut led2(D9);
+
+DigitalOut led1(PTC12);// rigth led on biorobotics shield
+DigitalOut led2(D9);//left led on biorobotics shield
 
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
+const float safetymarginfield=0.075; //adjustable, tweak for maximum but safe range
+const float mm_per_sec_emg=0.050;// move the pod 50 mm per sec if muscle is flexed
+const float y_start=0.255;//starting y position of the pod
+const float y_punch=0.473;// position to where there is punched
+const float timetoshoot=0.5;// time it can take to shoot
+
 float desired_position=0;
-float desired_angle[]= {0,0};
-float mm_per_sec_emg=0.050;// move the pod 50 mm per sec if muscle is flexed
-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 desired_angle1=0;
+float desired_angle2=0;
 
+const float fieldwidth=0.473;
+const float maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield
 
-angleandposition anglepos;
-float y_start=0.255;
-float y_punch=0.473;
-float timetoshoot=0.5;
+angleandposition anglepos;// initiate the angle and position calculation library
+
 
 
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
@@ -144,8 +153,8 @@
 void scopedata()
 {
     scope.set(0,desired_position);
-    scope.set(1,desired_angle[0]);
-    scope.set(2,desired_angle[1]);
+    scope.set(1,desired_angle1);
+    scope.set(2,desired_angle2);
     scope.send();
 }
 //read potmeters and adjust the safetyfactor and threshold
@@ -211,46 +220,42 @@
     pc.scanf("%f", &filter_extragain);
 }
 
-
+// shoot the pod forward
 void shoot()
 {
     ledgreen=1;
     float time=0;
     float stepsize=(y_punch-y_start)/(timetoshoot*control_frequency);
-    float y_during_punch=y_start;
+    float y_during_punch=y_start;// set initial y position to start position
 
     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+=stepsize; // add stepsize to y position
+        if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety
             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);
+        desired_angle1=anglepos.positiontoangle1(desired_position,y_during_punch);// calculate desired angles
+        desired_angle2=anglepos.positiontoangle2(desired_position,y_during_punch);
 
-        float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
-        float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
+        float error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
+        float error2=(desired_angle2-counttorad*encoder2.getPulses());
 
-        mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
-scopedata();
+        mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);// send errors to controller
+        scopedata();//send data to hidscope   WARING lower freqyency than normal
 
-        time+=(Ts_control);
-        wait(time-shoottimer.read());
-        count++;
-        pc.printf("angle 1 =%f angle 2=%f\n",desired_angle[0],desired_angle[1]);
-
+        time+=(Ts_control);// add time it should take to calculated time
+        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;
-
 }
 
 ////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION
@@ -319,7 +324,7 @@
     }
 }
 
-void changemode()
+void changemode()//this makes the counter higher to switch between modes 
 {
     mycontroller.STOP();
     switchedmode=true;
@@ -329,7 +334,8 @@
     } else {
         modecounter=modecounter;
     }
-    wait(1);
+    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
 }
 
 ///////////////////////////////////////////////////MAIN
@@ -345,49 +351,47 @@
     readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
     ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
 
-    pc.baud(115200);
+    pc.baud(115200);//set baudrate to 115200
     while(1) {
-        if (changemodebutton==0) {
+        if (changemodebutton==0) {// check if the change mode button is pressed
             changemode();
         }
-        if (scopedata_go==true) {
+        if (scopedata_go==true) {//send scopedata
             scopedata();
             scopedata_go=false;
         }
-        if (safetyandthreshold_go==true) {
+        if (safetyandthreshold_go==true) {// check the potmeters
             safetyandthreshold();
             safetyandthreshold_go=false;
         }
         ///////////////////////////////////////////NORMAL RUNNING MODE
         if(modecounter==0) {
             if (switchedmode==true) {
-                encoder1.reset();
+                encoder1.reset();// reset encoders so they are at 0 degrees
                 encoder2.reset();
                 pc.printf("Program running\n");//
                 ledgreen=0;
                 led1=led2=ledred=ledblue=1;
                 switchedmode=false;
             }
-
-            if (filter_go==true) {
+            if (filter_go==true) {// filter the emg signal
                 filtereverything();
                 filter_go=false;
             }
-            if (readsignal_go==true) {
+            if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position
                 readsignal();
                 readsignal_go=false;
             }
-            if (control_go==true) {
-                desired_angle[0]=anglepos.positiontoangle1(desired_position,y_start);
-                desired_angle[1]=anglepos.positiontoangle2(desired_position,y_start);
+            if (control_go==true) {// calculate angles from positions and send error to controller
+                desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
+                desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
 
-                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
-                float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
+                float error1=(desired_angle1-counttorad*encoder1.getPulses());
+                float error2=(desired_angle2-counttorad*encoder2.getPulses());
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
             }
-
-            valuechangebutton.fall(&valuechange);
+            valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
         }
         ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
         if (modecounter==1) {
@@ -398,25 +402,25 @@
                 switchedmode=false;
             }
             if (ledblink_go==true) {
-                led1=!led1;
+                led1=!led1;// blink rigth led on biorobotics shield (because rigth arm is being calibrated)
                 ledblink_go=false;
             }
-            if (control_go==true) {
-                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
-                float error2=0;// this is the error you want to use
-                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
-                control_go=false;
-            }
-            if (readbuttoncalibrate_go==true) {
+            if (readbuttoncalibrate_go==true) {//check wich button is pressed and adjust wanted angle of rigth arm
                 if (buttonR.read()==0 && buttonL.read()==1) {
-                    desired_angle[0] += (radpersec_calibrate/readbuttoncalibrate_frequency);
+                    desired_angle1 += (radpersec_calibrate/readbuttoncalibrate_frequency);
                     readbuttoncalibrate_go=false;
                 }
                 if (buttonR.read()==1 && buttonL.read()==0) {
-                    desired_angle[0] -= (radpersec_calibrate/readbuttoncalibrate_frequency);
+                    desired_angle1 -= (radpersec_calibrate/readbuttoncalibrate_frequency);
                     readbuttoncalibrate_go=false;
                 }
             }
+             if (control_go==true) {// calculate errors and send them to controllers
+                float error1=(desired_angle1-counttorad*encoder1.getPulses());
+                float error2=0;
+                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
+                control_go=false;
+            }
         }
         ////////////////////////////////////////////CALIBRATE LEFT ARM
         if (modecounter==2) {
@@ -427,25 +431,25 @@
                 switchedmode=false;
             }
             if (ledblink_go==true) {
-                led2=!led2;
+                led2=!led2;// blink left led on biorobotics shield (because left arm is being calibrated)
                 ledblink_go=false;
             }
-            if (control_go==true) {
-                float error1=0;
-                float error2=(desired_angle[1]-counttorad*encoder2.getPulses());// this is the error you want to use
-                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
-                control_go=false;
-            }
-            if (readbuttoncalibrate_go==true) {
-                if (buttonR.read()==0 && buttonL.read()==1) {
-                    desired_angle[1] += (radpersec_calibrate/readbuttoncalibrate_frequency);
+            if (readbuttoncalibrate_go==true) {// 
+                if (buttonR.read()==0 && buttonL.read()==1) {//check wich button is pressed and adjust wanted angle of left arm
+                    desired_angle2 += (radpersec_calibrate/readbuttoncalibrate_frequency);
                     readbuttoncalibrate_go=false;
                 }
                 if (buttonR.read()==1 && buttonL.read()==0) {
-                    desired_angle[1] -= (radpersec_calibrate/readbuttoncalibrate_frequency);
+                    desired_angle2 -= (radpersec_calibrate/readbuttoncalibrate_frequency);
                     readbuttoncalibrate_go=false;
                 }
             }
+             if (control_go==true) {// calculate errors and send to controller
+                float error1=0;
+                float error2=(desired_angle2-counttorad*encoder2.getPulses());
+                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
+                control_go=false;
+            }
         }
         ///////////////////////////////BUTTONCONTROLMODE
         if (modecounter==3) {
@@ -459,16 +463,16 @@
                 ledgreen=!ledgreen;
                 ledblink_go=false;
             }
-            if (readsignal_go==true) {
+            if (readsignal_go==true) {// read buttons and adjus wanted position
                 readsignalbutton();
                 readsignal_go=false;
             }
-            if (control_go==true) {
-                desired_angle[0]=anglepos.positiontoangle1(desired_position,y_start);
-                desired_angle[1]=anglepos.positiontoangle2(desired_position,y_start);
+            if (control_go==true) {// calculate wanted angles from position, errors and send to controller
+                desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
+                desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
 
-                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
-                float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
+                float error1=(desired_angle1-counttorad*encoder1.getPulses());
+                float error2=(desired_angle2-counttorad*encoder2.getPulses());
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
             }