Jasper Gerth / Mbed 2 deprecated includeair

The final program for the #include AIR robot

Dependencies:   Biquad HIDScope QEI angleandposition controlandadjust mbed

Fork of includeair by BioRobotics

main.cpp@43:a5f47a1f11d2, 2015-11-02 (annotated)

Committer:
Gerth
Date:
Mon Nov 02 11:48:52 2015 +0000
Revision:
43:a5f47a1f11d2
Parent:
42:386fc2fcfb22
Child:
44:fd7b3ace6c19
changed gotogoal to gotopos and used it for the demo;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:dd66fff537d7 1 #include "mbed.h"
Gerth 1:917c07a4f3ec 2 #include "QEI.h"
Gerth 1:917c07a4f3ec 3 #include "HIDScope.h"
Gerth 1:917c07a4f3ec 4 #include "Biquad.h"
Gerth 1:917c07a4f3ec 5 #include "controlandadjust.h"
Gerth 16:63320b8f79c2 6 #include "angleandposition.h"
Gerth 1:917c07a4f3ec 7
Gerth 20:c5fb2ff5d457 8 ///////////////////////////////////////////////info out
Gerth 20:c5fb2ff5d457 9 HIDScope scope(6);// number of hidscope channels
Gerth 35:0141a1fe8138 10 Ticker scope_ticker;//ticker for the scope
Gerth 41:e9fd0670f70c 11 const double scope_frequency=200; //HIDscope frequency
Gerth 35:0141a1fe8138 12
Gerth 35:0141a1fe8138 13 Timer checktimer;// timer to check how much time it takes to run the control loop, to see if the frequencies are not too high
Gerth 35:0141a1fe8138 14 volatile double checktimervalue=0; // make a variable to store the time the loop has taken
Gerth 20:c5fb2ff5d457 15
Gerth 20:c5fb2ff5d457 16 Serial pc(USBTX,USBRX);// serial connection to pc
Gerth 1:917c07a4f3ec 17
Gerth 35:0141a1fe8138 18 DigitalOut ledred(LED_RED);// leds on the k64f board
Gerth 16:63320b8f79c2 19 DigitalOut ledgreen(LED_GREEN);
Gerth 16:63320b8f79c2 20 DigitalOut ledblue(LED_BLUE);
Gerth 14:4c4f45a1dd23 21
Gerth 35:0141a1fe8138 22
Gerth 20:c5fb2ff5d457 23 /////////////////////////////////////////////ENCODERS
Gerth 41:e9fd0670f70c 24 const double cpr_sensor=32; //counts per rotation of the sensor
Gerth 41:e9fd0670f70c 25 const double cpr_shaft=cpr_sensor*131;//counts per rotation of the outputshaft
Gerth 20:c5fb2ff5d457 26
Gerth 20:c5fb2ff5d457 27 QEI encoder1(D13,D12,NC,cpr_sensor);/// encoders on motors X2 encoding
Gerth 35:0141a1fe8138 28 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
Gerth 20:c5fb2ff5d457 29
Gerth 35:0141a1fe8138 30 const double PIE=3.14159265359; // pi, for calculations
Gerth 39:ebcf0a60f58b 31 const double counttorad=((2*PIE)/cpr_shaft);// to convert counts to rotation in rad
Gerth 1:917c07a4f3ec 32
Gerth 24:dd75c961ae88 33
Gerth 6:37c94a5e205f 34 /////////////////////////////////CALIBRATION (MODE)
Gerth 20:c5fb2ff5d457 35 const double radpersec_calibrate=0.1*PIE;// speed of arms when in calibration mode
Gerth 20:c5fb2ff5d457 36 int modecounter=1;//counter in which mode the robot is
Gerth 35:0141a1fe8138 37 const double readbuttoncalibrate_frequency=50;//frequency at which the buttons are read when in calibration mode
Gerth 20:c5fb2ff5d457 38 const double ledblink_frequency=4;//frequency at which the green led and leds on top blink when in resp button or calibration mode
Gerth 6:37c94a5e205f 39
Gerth 20:c5fb2ff5d457 40 DigitalIn changemodebutton(PTA4);// button to change mode (sw3)
Gerth 35:0141a1fe8138 41 Ticker readbuttoncalibrate_ticker;//ticker for reading out the buttons when calibrating
Gerth 35:0141a1fe8138 42 Ticker ledblink_ticker;// ticker for blinking the leds
Gerth 14:4c4f45a1dd23 43
Gerth 20:c5fb2ff5d457 44 DigitalIn buttonR(D2);//rigth button on biorobotics shield
Gerth 20:c5fb2ff5d457 45 DigitalIn buttonL(D3);//left button on biorobotics shield
Gerth 8:54a7da09ccad 46
Gerth 27:31cb8fbd976d 47 /////////////////READSIGNAL
Gerth 28:71a90073e482 48 const double readsignal_frequency=100;//frequency at wich the filtered emg signal is sampled to be 0 or 1
Gerth 35:0141a1fe8138 49 Ticker readsignal_ticker; // ticker for reading the signal of the emg
Gerth 27:31cb8fbd976d 50
Gerth 27:31cb8fbd976d 51
Gerth 27:31cb8fbd976d 52 DigitalOut led1(PTC12);// rigth led on biorobotics shield
Gerth 27:31cb8fbd976d 53 DigitalOut led2(D9);//left led on biorobotics shield
Gerth 27:31cb8fbd976d 54
Gerth 1:917c07a4f3ec 55 //////////////////////////////////CONTROLLER
Gerth 25:21dcd3f9eac2 56 const double control_frequency=250;// frequency at which the controller is called
Gerth 1:917c07a4f3ec 57 //controller constants
Gerth 31:8646e853979f 58 float Kp_shoot=6;
Gerth 31:8646e853979f 59 float Ki_shoot=2;
Gerth 31:8646e853979f 60 float Kd_shoot=0.5;
Gerth 31:8646e853979f 61 float Kp_move=2;
Gerth 31:8646e853979f 62 float Ki_move=1.25;
Gerth 31:8646e853979f 63 float Kd_move=0.05;
Gerth 31:8646e853979f 64
Gerth 27:31cb8fbd976d 65 controlandadjust mycontroller(2,control_frequency); // make a controller, value in brackets is errorband in degrees and controller frequency
Gerth 20:c5fb2ff5d457 66
Gerth 35:0141a1fe8138 67 Ticker control_ticker;//ticker for the controller
Gerth 35:0141a1fe8138 68 const double Ts_control=1.0/control_frequency;//sample time of the controller
Gerth 27:31cb8fbd976d 69
Gerth 39:ebcf0a60f58b 70 double error1=0,//controller error storage variables
Gerth 41:e9fd0670f70c 71 error2=0;
Gerth 1:917c07a4f3ec 72
Gerth 41:e9fd0670f70c 73 InterruptIn valuechangebutton(PTC6);//button to change filter gains per arm via Serial
Gerth 1:917c07a4f3ec 74
Gerth 1:917c07a4f3ec 75 //safetyandthreshold
Gerth 3:48438eea184e 76 AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm
Gerth 3:48438eea184e 77 AnalogIn threshold_pot(A2);//pot1, used to adjust threshold if signal differs per person
Gerth 1:917c07a4f3ec 78
Gerth 1:917c07a4f3ec 79 Ticker safetyandthreshold_ticker; // ticker to read potmeters
Gerth 3:48438eea184e 80 const double safetyandthreshold_frequency=1; // frequency for the ticker
Gerth 3:48438eea184e 81
Gerth 39:ebcf0a60f58b 82 double threshold_value=1;//initial threshold value
Gerth 1:917c07a4f3ec 83
Gerth 1:917c07a4f3ec 84 ////////////////////////////////FILTER
Gerth 35:0141a1fe8138 85 const double filter_frequency=500; // frequency at which the emg signal is filtered
Gerth 35:0141a1fe8138 86 #include "filtervalues.h"// call the values for the biquads these are in a separate file to keep this code readable
Gerth 20:c5fb2ff5d457 87
Gerth 35:0141a1fe8138 88 Ticker filter_ticker;//Ticker for the filter
Gerth 1:917c07a4f3ec 89
Gerth 20:c5fb2ff5d457 90 Biquad myfilter1;// make filter for signal 1
Gerth 20:c5fb2ff5d457 91 Biquad myfilter2;//make filter for signal 2
Gerth 1:917c07a4f3ec 92
Gerth 20:c5fb2ff5d457 93 AnalogIn emg1_input(A0);//input for first emg signal
Gerth 20:c5fb2ff5d457 94 AnalogIn emg2_input(A1);//input for second emg signal
Gerth 20:c5fb2ff5d457 95
Gerth 20:c5fb2ff5d457 96 volatile double filteredsignal1=0;//the first filtered emg signal
Gerth 20:c5fb2ff5d457 97 volatile double filteredsignal2=0;//the second filtered emg signal
Gerth 39:ebcf0a60f58b 98 double filter_extragain1=1,filter_extragain2=1; // this is a factor to increase the gain per arm, adustable via serial
Gerth 1:917c07a4f3ec 99
Gerth 4:bf7765b0f612 100
Gerth 41:e9fd0670f70c 101 //////////////////////////////// POSITION AND ANGLE
Gerth 40:8b25c0531340 102 const double safetymarginfield=0.05; //adjustable, tweak for maximum but safe range
Gerth 34:bc2d64e86cb9 103 const double mm_per_sec_emg=0.1;// move the pod 100 mm per sec if muscle is flexed
Gerth 40:8b25c0531340 104 const double y_start=0.225;//starting y position of the pod
Gerth 34:bc2d64e86cb9 105 const double y_punch=0.473;// position to where there is punched
Gerth 34:bc2d64e86cb9 106 const double timetoshoot=0.35;// time it can take to shoot
Gerth 34:bc2d64e86cb9 107 const double timetogoback=1;// time it can take to go back after shooting
Gerth 20:c5fb2ff5d457 108
Gerth 35:0141a1fe8138 109 volatile double desired_position=0;//desired x position
Gerth 35:0141a1fe8138 110 double desired_angle1=0; //desired angle of arm 1 (calculated with anglepos)
Gerth 35:0141a1fe8138 111 double desired_angle2=0; // desired anvle of arm 2 (calculated with anglepos)
Gerth 19:6f22b5687587 112
Gerth 41:e9fd0670f70c 113 const double fieldwidth=0.473; // width of the field
Gerth 41:e9fd0670f70c 114 const double maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield
Gerth 3:48438eea184e 115
Gerth 20:c5fb2ff5d457 116 angleandposition anglepos;// initiate the angle and position calculation library
Gerth 20:c5fb2ff5d457 117
Gerth 39:ebcf0a60f58b 118 const double radtodeg=(180/PIE); // to convert radians to degrees
Gerth 38:df0b0abea598 119
Gerth 41:e9fd0670f70c 120
Gerth 41:e9fd0670f70c 121 /////////////////////////////////////////////////////////DEMO MODE
Gerth 42:386fc2fcfb22 122 const double demo_safety=0.05;// safety factor to stay off the edges
Gerth 42:386fc2fcfb22 123 const double demo_x_edge_rigth=(fieldwidth/2)-demo_safety;// rigth edge of the field
Gerth 42:386fc2fcfb22 124 const double demo_x_edge_left=-((fieldwidth/2)-demo_safety);// left edge of te field
Gerth 43:a5f47a1f11d2 125 const double demo_goal_edge_rigth=0.1+demo_safety;// x edges of goal
Gerth 43:a5f47a1f11d2 126 const double demo_goal_edge_left=-(0.1+demo_safety);
Gerth 43:a5f47a1f11d2 127 const double demo_y_goal=0.1+demo_safety;// y position in front of goal
Gerth 43:a5f47a1f11d2 128 const double demo_y_back=0.05+demo_safety;//back edge of the field
Gerth 41:e9fd0670f70c 129 const double demo_y_front=y_punch;
Gerth 41:e9fd0670f70c 130
Gerth 41:e9fd0670f70c 131
Gerth 1:917c07a4f3ec 132 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
Gerth 1:917c07a4f3ec 133 volatile bool scopedata_go=false,
Gerth 1:917c07a4f3ec 134 control_go=false,
Gerth 1:917c07a4f3ec 135 filter_go=false,
Gerth 3:48438eea184e 136 safetyandthreshold_go=false,
Gerth 6:37c94a5e205f 137 readsignal_go=false,
Gerth 8:54a7da09ccad 138 switchedmode=true,
Gerth 14:4c4f45a1dd23 139 readbuttoncalibrate_go=false,
Gerth 41:e9fd0670f70c 140 ledblink_go=false,
Gerth 41:e9fd0670f70c 141 demo_next_step_go=false;
Gerth 1:917c07a4f3ec 142
Gerth 1:917c07a4f3ec 143 void scopedata_activate()
Gerth 1:917c07a4f3ec 144 {
Gerth 1:917c07a4f3ec 145 scopedata_go=true;
Gerth 1:917c07a4f3ec 146 }
Gerth 1:917c07a4f3ec 147 void control_activate()
Gerth 1:917c07a4f3ec 148 {
Gerth 1:917c07a4f3ec 149 control_go=true;
Gerth 1:917c07a4f3ec 150 }
Gerth 1:917c07a4f3ec 151 void filter_activate()
Gerth 1:917c07a4f3ec 152 {
Gerth 1:917c07a4f3ec 153 filter_go=true;
Gerth 1:917c07a4f3ec 154 }
Gerth 1:917c07a4f3ec 155 void safetyandthreshold_activate()
Gerth 1:917c07a4f3ec 156 {
Gerth 1:917c07a4f3ec 157 safetyandthreshold_go=true;
Gerth 1:917c07a4f3ec 158 }
Gerth 3:48438eea184e 159 void readsignal_activate()
Gerth 3:48438eea184e 160 {
Gerth 3:48438eea184e 161 readsignal_go=true;
Gerth 3:48438eea184e 162 }
Gerth 7:7fbb2c028778 163 void readbuttoncalibrate_activate()
Gerth 7:7fbb2c028778 164 {
Gerth 7:7fbb2c028778 165 readbuttoncalibrate_go=true;
Gerth 7:7fbb2c028778 166 }
Gerth 14:4c4f45a1dd23 167 void ledblink_activate()
Gerth 14:4c4f45a1dd23 168 {
Gerth 14:4c4f45a1dd23 169 ledblink_go=true;
Gerth 14:4c4f45a1dd23 170 }
Gerth 41:e9fd0670f70c 171 void demo_next_step_activate()
Gerth 41:e9fd0670f70c 172 {
Gerth 41:e9fd0670f70c 173 demo_next_step_go=true;
Gerth 41:e9fd0670f70c 174 }
Gerth 1:917c07a4f3ec 175
Gerth 1:917c07a4f3ec 176 ////////////////////////FUNCTIONS
Gerth 1:917c07a4f3ec 177 //gather data and send to scope
Gerth 32:c940f6b6a6a0 178 void scopedata(double wanted_y)
Gerth 1:917c07a4f3ec 179 {
Gerth 35:0141a1fe8138 180 scope.set(0,desired_position); // desired x position
Gerth 35:0141a1fe8138 181 scope.set(1,wanted_y); // desired y position
Gerth 35:0141a1fe8138 182 scope.set(2,filteredsignal1); // filterded emg signal rigth arm
Gerth 35:0141a1fe8138 183 scope.set(3,filteredsignal2); // filtered emg signal left arm
Gerth 35:0141a1fe8138 184 scope.set(4,threshold_value); // threshold value
Gerth 35:0141a1fe8138 185 scope.set(5,mycontroller.motor1pwm()); //pwm signal send to motor 1
Gerth 35:0141a1fe8138 186 scope.send(); // send info to HIDScope server
Gerth 4:bf7765b0f612 187 }
Gerth 1:917c07a4f3ec 188 //read potmeters and adjust the safetyfactor and threshold
Gerth 1:917c07a4f3ec 189 void safetyandthreshold()
Gerth 1:917c07a4f3ec 190 {
Gerth 3:48438eea184e 191 mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal
Gerth 28:71a90073e482 192 threshold_value=((ceil (100*threshold_pot.read()) )/100); // adjust the threshold value between 0 and 1 rounded to 2 decimals
Gerth 1:917c07a4f3ec 193 }
Gerth 1:917c07a4f3ec 194 /////filter
Gerth 28:71a90073e482 195 void filtereverything(bool makeempty)
Gerth 1:917c07a4f3ec 196 {
Gerth 1:917c07a4f3ec 197 //pass1 so f1
Gerth 2:c7707856d137 198 double pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
Gerth 2:c7707856d137 199 double pass1_emg2 = myfilter2.filter(emg2_input.read(), v1_f1_emg2 , v2_f1_emg2 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
Gerth 1:917c07a4f3ec 200
Gerth 1:917c07a4f3ec 201 //pass2 so f2
Gerth 2:c7707856d137 202 double pass2_emg1 = myfilter1.filter(pass1_emg1, v1_f2_emg1 , v2_f2_emg1 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);
Gerth 2:c7707856d137 203 double pass2_emg2 = myfilter2.filter(pass1_emg2, v1_f2_emg2 , v2_f2_emg2 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);
Gerth 1:917c07a4f3ec 204
Gerth 1:917c07a4f3ec 205 //pass3 so f3
Gerth 2:c7707856d137 206 double pass3_emg1 = myfilter1.filter(pass2_emg1, v1_f3_emg1 , v2_f3_emg1 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);
Gerth 2:c7707856d137 207 double pass3_emg2 = myfilter2.filter(pass2_emg2, v1_f3_emg2 , v2_f3_emg2 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);
Gerth 1:917c07a4f3ec 208
Gerth 1:917c07a4f3ec 209 //pass4 so f4
Gerth 2:c7707856d137 210 double pass4_emg1 = myfilter1.filter(pass3_emg1, v1_f4_emg1 , v2_f4_emg1 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);
Gerth 2:c7707856d137 211 double pass4_emg2 = myfilter2.filter(pass3_emg2, v1_f4_emg2 , v2_f4_emg2 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);
Gerth 1:917c07a4f3ec 212
Gerth 1:917c07a4f3ec 213 //pass5 so f5
Gerth 2:c7707856d137 214 double pass5_emg1 = myfilter1.filter(pass4_emg1, v1_f5_emg1 , v2_f5_emg1 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);
Gerth 2:c7707856d137 215 double pass5_emg2 = myfilter2.filter(pass4_emg2, v1_f5_emg2 , v2_f5_emg2 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);
Gerth 1:917c07a4f3ec 216
Gerth 1:917c07a4f3ec 217 ///// take absolute value
Gerth 2:c7707856d137 218 double pass5_emg1_abs=(fabs(pass5_emg1));
Gerth 2:c7707856d137 219 double pass5_emg2_abs=(fabs(pass5_emg2));
Gerth 1:917c07a4f3ec 220
Gerth 1:917c07a4f3ec 221 //pass6 so f6
Gerth 2:c7707856d137 222 double pass6_emg1 = myfilter1.filter(pass5_emg1_abs, v1_f6_emg1 , v2_f6_emg1 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);
Gerth 2:c7707856d137 223 double pass6_emg2 = myfilter2.filter(pass5_emg2_abs, v1_f6_emg2 , v2_f6_emg2 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);
Gerth 1:917c07a4f3ec 224
Gerth 1:917c07a4f3ec 225
Gerth 1:917c07a4f3ec 226 //pass7 so f7
Gerth 2:c7707856d137 227 double pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
Gerth 2:c7707856d137 228 double pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
Gerth 1:917c07a4f3ec 229
Gerth 35:0141a1fe8138 230 filteredsignal1=(pass7_emg1*9e11*filter_extragain1);//this is needed to make the signal ~1 when flexed
Gerth 35:0141a1fe8138 231 filteredsignal2=(pass7_emg2*9e11*filter_extragain2);// filter_extragain is adjusted via serial
Gerth 30:cd4d9754a357 232
Gerth 35:0141a1fe8138 233 if (makeempty==true) {//this is needed so the filtered value is not high after shooting basically it resets the filter
Gerth 28:71a90073e482 234 pass1_emg1 = pass1_emg2 =pass2_emg1 =pass2_emg2 =pass3_emg1 = pass3_emg2 =pass4_emg1 =pass4_emg2 =pass5_emg1 =pass5_emg2 =0;
Gerth 28:71a90073e482 235 pass5_emg1_abs=pass5_emg2_abs=pass6_emg1 =pass6_emg2 =pass7_emg1=pass7_emg2 =filteredsignal1=filteredsignal2=0;
Gerth 28:71a90073e482 236 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;
Gerth 28:71a90073e482 237 v1_f5_emg2=v1_f6_emg1=v1_f6_emg2=v1_f7_emg1=v1_f7_emg2=0;
Gerth 28:71a90073e482 238 }
Gerth 28:71a90073e482 239
Gerth 1:917c07a4f3ec 240 }
Gerth 1:917c07a4f3ec 241 //adjust controller values when sw2 is pressed
Gerth 1:917c07a4f3ec 242 void valuechange()
Gerth 1:917c07a4f3ec 243 {
Gerth 35:0141a1fe8138 244 mycontroller.STOP(); // stop motors
Gerth 41:e9fd0670f70c 245 pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1); // print extra gain for rigth arm
Gerth 35:0141a1fe8138 246 pc.scanf("%f", &filter_extragain1); //read the input from serial and write to filter_extragain1
Gerth 41:e9fd0670f70c 247
Gerth 35:0141a1fe8138 248 pc.printf("Extra gain for left arm is now %f, enter new value\n",filter_extragain2); // print extra gain for left arm
Gerth 35:0141a1fe8138 249 pc.scanf("%f", &filter_extragain2);//read the input from serial and write to filter_extragain2
Gerth 1:917c07a4f3ec 250 }
Gerth 11:c10651055871 251
Gerth 20:c5fb2ff5d457 252 // shoot the pod forward
Gerth 19:6f22b5687587 253 void shoot()
Gerth 14:4c4f45a1dd23 254 {
Gerth 41:e9fd0670f70c 255 ledgreen=1;
Gerth 32:c940f6b6a6a0 256 double time=0;
Gerth 35:0141a1fe8138 257 double stepsize=(0.5*PIE)/(timetoshoot*control_frequency); //calculate stepsize
Gerth 35:0141a1fe8138 258 double profile_angle=0;// used for the profilie
Gerth 35:0141a1fe8138 259 double x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());// calculate actual x positon from the encoder angles
Gerth 35:0141a1fe8138 260 double y_during_punch=y_start;//set initial y value to baseline
Gerth 35:0141a1fe8138 261 Timer shoottimer; // make a timer
Gerth 19:6f22b5687587 262 shoottimer.reset();
Gerth 19:6f22b5687587 263 shoottimer.start();
Gerth 23:1c4a18799464 264 //forward
Gerth 19:6f22b5687587 265 while (time<=timetoshoot) {
Gerth 35:0141a1fe8138 266 ledblue=!ledblue; // blink blue led (very fast, so not noticable blinking)
Gerth 25:21dcd3f9eac2 267
Gerth 32:c940f6b6a6a0 268 profile_angle+=stepsize; // add stepsize to angle
Gerth 33:1c7b498ded25 269
Gerth 32:c940f6b6a6a0 270 y_during_punch=(y_punch-y_start)*(1-cos(profile_angle))+y_start; // calculate y position for shooting
Gerth 33:1c7b498ded25 271
Gerth 20:c5fb2ff5d457 272 if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety
Gerth 19:6f22b5687587 273 y_during_punch=y_punch;
Gerth 19:6f22b5687587 274 } else {
Gerth 19:6f22b5687587 275 y_during_punch=y_during_punch;
Gerth 33:1c7b498ded25 276 }
Gerth 19:6f22b5687587 277
Gerth 29:cd47a5a772db 278 desired_angle1=anglepos.positiontoangle1(x_punch,y_during_punch);// calculate desired angles
Gerth 29:cd47a5a772db 279 desired_angle2=anglepos.positiontoangle2(x_punch,y_during_punch);
Gerth 19:6f22b5687587 280
Gerth 25:21dcd3f9eac2 281 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
Gerth 25:21dcd3f9eac2 282 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 19:6f22b5687587 283
Gerth 31:8646e853979f 284 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
Gerth 31:8646e853979f 285 scopedata(y_during_punch);//send data to hidscope WARING lower freqyency than normal
Gerth 19:6f22b5687587 286
Gerth 20:c5fb2ff5d457 287 time+=(Ts_control);// add time it should take to calculated time
Gerth 28:71a90073e482 288 filtereverything(true);//set al filter variables to 0
Gerth 41:e9fd0670f70c 289
Gerth 35:0141a1fe8138 290 wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need,
Gerth 41:e9fd0670f70c 291 // if this is not done the loop wil go to fast and the motors can't keep up
Gerth 41:e9fd0670f70c 292 // this is because the control loop takes far less time than Ts_control
Gerth 11:c10651055871 293 }
Gerth 23:1c4a18799464 294 //back
Gerth 35:0141a1fe8138 295 time=0; // reset time
Gerth 35:0141a1fe8138 296 shoottimer.reset();//reset timer
Gerth 35:0141a1fe8138 297 stepsize=(PIE)/(timetogoback*control_frequency);//calculate stepsize
Gerth 35:0141a1fe8138 298 profile_angle=0;//reset angle for the displacement profile
Gerth 35:0141a1fe8138 299 desired_position=0;// set desired x position to 0, so the robot is in the middle in front of the goal after shooting
Gerth 26:c935e39cce8a 300 while (time<=timetogoback) {
Gerth 35:0141a1fe8138 301 ledblue=!ledblue;// blink blue led
Gerth 25:21dcd3f9eac2 302
Gerth 32:c940f6b6a6a0 303 profile_angle+=stepsize; // add stepsize to y position
Gerth 33:1c7b498ded25 304 y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting
Gerth 23:1c4a18799464 305 if (y_during_punch<=y_start) {//to check if y position is not smaller than y_start for safety
Gerth 23:1c4a18799464 306 y_during_punch=y_start;
Gerth 23:1c4a18799464 307 } else {
Gerth 23:1c4a18799464 308 y_during_punch=y_during_punch;
Gerth 23:1c4a18799464 309 }
Gerth 23:1c4a18799464 310
Gerth 34:bc2d64e86cb9 311 desired_angle1=anglepos.positiontoangle1(desired_position,y_during_punch);// calculate desired angles
Gerth 34:bc2d64e86cb9 312 desired_angle2=anglepos.positiontoangle2(desired_position,y_during_punch);
Gerth 41:e9fd0670f70c 313
Gerth 23:1c4a18799464 314
Gerth 25:21dcd3f9eac2 315 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
Gerth 25:21dcd3f9eac2 316 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 23:1c4a18799464 317
Gerth 31:8646e853979f 318 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
Gerth 31:8646e853979f 319 scopedata(y_during_punch);//send data to hidscope WARING lower freqyency than normal
Gerth 23:1c4a18799464 320
Gerth 23:1c4a18799464 321 time+=(Ts_control);// add time it should take to calculated time
Gerth 35:0141a1fe8138 322 filtereverything(false);//start filtering the signal (lower frewquency than normal)
Gerth 23:1c4a18799464 323 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
Gerth 25:21dcd3f9eac2 324 }
Gerth 19:6f22b5687587 325 shoottimer.stop();
Gerth 35:0141a1fe8138 326 ledblue=1; // blue led off
Gerth 35:0141a1fe8138 327 ledgreen=0; // green led on
Gerth 11:c10651055871 328 }
Gerth 11:c10651055871 329
Gerth 16:63320b8f79c2 330 ////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION
Gerth 3:48438eea184e 331 void readsignal()
Gerth 3:48438eea184e 332 {
Gerth 11:c10651055871 333 //check if pod has to shoot
Gerth 4:bf7765b0f612 334 if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
Gerth 35:0141a1fe8138 335 led1=led2=1; // ligth up both leds
Gerth 11:c10651055871 336 shoot();
Gerth 14:4c4f45a1dd23 337 // check if pod has to move to the right
Gerth 4:bf7765b0f612 338 } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
Gerth 35:0141a1fe8138 339 led1=1;// ligth up rigth led
Gerth 4:bf7765b0f612 340 led2=0;
Gerth 16:63320b8f79c2 341 desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
Gerth 11:c10651055871 342 if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
Gerth 11:c10651055871 343 desired_position=maxdisplacement;
Gerth 11:c10651055871 344 } else {
Gerth 11:c10651055871 345 desired_position=desired_position;
Gerth 11:c10651055871 346 }
Gerth 14:4c4f45a1dd23 347 // check if pod has to move to the left
Gerth 4:bf7765b0f612 348 } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
Gerth 35:0141a1fe8138 349 led1=0; // ligth up left led
Gerth 4:bf7765b0f612 350 led2=1;
Gerth 16:63320b8f79c2 351 desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
Gerth 11:c10651055871 352 if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
Gerth 14:4c4f45a1dd23 353 desired_position=(-1*maxdisplacement);
Gerth 11:c10651055871 354 } else {
Gerth 11:c10651055871 355 desired_position=desired_position;
Gerth 11:c10651055871 356 }
Gerth 3:48438eea184e 357 } else {
Gerth 4:bf7765b0f612 358 led1=led2=0;
Gerth 3:48438eea184e 359 }
Gerth 16:63320b8f79c2 360 }
Gerth 28:71a90073e482 361
Gerth 16:63320b8f79c2 362 ///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION
Gerth 35:0141a1fe8138 363 void readsignalbutton() // same as above, only with buttons as input
Gerth 16:63320b8f79c2 364 {
Gerth 19:6f22b5687587 365 //write value of button to variable
Gerth 17:72d3522165ac 366 int buttonr=buttonR.read();
Gerth 17:72d3522165ac 367 int buttonl=buttonL.read();
Gerth 16:63320b8f79c2 368 //check if pod has to shoot
Gerth 17:72d3522165ac 369 if (buttonr==0 && buttonl==0) {
Gerth 16:63320b8f79c2 370 led1=led2=1;
Gerth 16:63320b8f79c2 371 shoot();
Gerth 16:63320b8f79c2 372 // check if pod has to move to the right
Gerth 17:72d3522165ac 373 } else if (buttonr==0 && buttonl==1) {
Gerth 16:63320b8f79c2 374 led1=1;
Gerth 16:63320b8f79c2 375 led2=0;
Gerth 16:63320b8f79c2 376 desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
Gerth 17:72d3522165ac 377 if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
Gerth 16:63320b8f79c2 378 desired_position=maxdisplacement;
Gerth 16:63320b8f79c2 379 } else {
Gerth 16:63320b8f79c2 380 desired_position=desired_position;
Gerth 16:63320b8f79c2 381 }
Gerth 17:72d3522165ac 382 // check if pod has to move to the left
Gerth 17:72d3522165ac 383 } else if (buttonr==1 && buttonl==0) {
Gerth 16:63320b8f79c2 384 led1=0;
Gerth 16:63320b8f79c2 385 led2=1;
Gerth 16:63320b8f79c2 386 desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
Gerth 17:72d3522165ac 387 if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
Gerth 16:63320b8f79c2 388 desired_position=(-1*maxdisplacement);
Gerth 16:63320b8f79c2 389 } else {
Gerth 16:63320b8f79c2 390 desired_position=desired_position;
Gerth 16:63320b8f79c2 391 }
Gerth 16:63320b8f79c2 392 } else {
Gerth 16:63320b8f79c2 393 led1=led2=0;
Gerth 16:63320b8f79c2 394 }
Gerth 3:48438eea184e 395 }
Gerth 3:48438eea184e 396
Gerth 28:71a90073e482 397 void changemode() //this makes the counter higher to switch between modes
Gerth 6:37c94a5e205f 398 {
Gerth 35:0141a1fe8138 399 mycontroller.STOP(); // stop the controller
Gerth 35:0141a1fe8138 400 switchedmode=true; // set switchedmode to true so in the main loop some statements are executed
Gerth 35:0141a1fe8138 401 modecounter++; // increase counter
Gerth 41:e9fd0670f70c 402 if (modecounter==6) { // reset counter if counter=6
Gerth 6:37c94a5e205f 403 modecounter=0;
Gerth 6:37c94a5e205f 404 } else {
Gerth 6:37c94a5e205f 405 modecounter=modecounter;
Gerth 6:37c94a5e205f 406 }
Gerth 20:c5fb2ff5d457 407 wait(1);// needed because else it is checked too fast if the button is pressed and modes change too fast
Gerth 35:0141a1fe8138 408 // tried it with interruptin but didn't work, bt this works good
Gerth 6:37c94a5e205f 409 }
Gerth 0:dd66fff537d7 410
Gerth 43:a5f47a1f11d2 411 void gotopos(double desired_x_pos, double desired_y_pos, double time_to_pos)
Gerth 42:386fc2fcfb22 412 {
Gerth 43:a5f47a1f11d2 413 double timepos=0;
Gerth 43:a5f47a1f11d2 414 Timer postimer;
Gerth 43:a5f47a1f11d2 415 double pos_x_start=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
Gerth 43:a5f47a1f11d2 416 double pos_y_start=anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
Gerth 43:a5f47a1f11d2 417
Gerth 43:a5f47a1f11d2 418 double pos_stepsize_x=(pos_x_start-desired_x_pos)/(time_to_pos*control_frequency);
Gerth 43:a5f47a1f11d2 419 double pos_stepsize_y=(pos_y_start-desired_y_pos)/(time_to_pos*control_frequency);
Gerth 42:386fc2fcfb22 420
Gerth 43:a5f47a1f11d2 421 double pos_x_moving=pos_x_start;
Gerth 43:a5f47a1f11d2 422 double pos_y_moving=pos_y_start;
Gerth 43:a5f47a1f11d2 423 postimer.start();
Gerth 43:a5f47a1f11d2 424 while(timepos<=time_to_pos) {
Gerth 43:a5f47a1f11d2 425 pos_x_moving-=pos_stepsize_x;
Gerth 43:a5f47a1f11d2 426 pos_y_moving-=pos_stepsize_y;
Gerth 42:386fc2fcfb22 427
Gerth 43:a5f47a1f11d2 428 desired_angle1=anglepos.positiontoangle1(pos_x_moving,pos_y_moving);// calculate desired angles
Gerth 43:a5f47a1f11d2 429 desired_angle2=anglepos.positiontoangle2( pos_x_moving,pos_y_moving);
Gerth 42:386fc2fcfb22 430
Gerth 42:386fc2fcfb22 431 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
Gerth 42:386fc2fcfb22 432 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 42:386fc2fcfb22 433
Gerth 42:386fc2fcfb22 434 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
Gerth 42:386fc2fcfb22 435
Gerth 43:a5f47a1f11d2 436 timepos+=(Ts_control);// add time it should take to calculated time
Gerth 42:386fc2fcfb22 437
Gerth 43:a5f47a1f11d2 438 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
Gerth 42:386fc2fcfb22 439
Gerth 42:386fc2fcfb22 440 }
Gerth 42:386fc2fcfb22 441 }
Gerth 41:e9fd0670f70c 442
Gerth 16:63320b8f79c2 443 ///////////////////////////////////////////////////MAIN
Gerth 7:7fbb2c028778 444
Gerth 0:dd66fff537d7 445 int main()
Gerth 0:dd66fff537d7 446 {
Gerth 35:0141a1fe8138 447 //initiate tickers
Gerth 1:917c07a4f3ec 448 safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
Gerth 1:917c07a4f3ec 449 filter_ticker.attach(&filter_activate,1.0/filter_frequency);
Gerth 1:917c07a4f3ec 450 control_ticker.attach(&control_activate,1.0/control_frequency);
Gerth 1:917c07a4f3ec 451 scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
Gerth 3:48438eea184e 452 readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
Gerth 7:7fbb2c028778 453 readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
Gerth 14:4c4f45a1dd23 454 ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
Gerth 14:4c4f45a1dd23 455
Gerth 20:c5fb2ff5d457 456 pc.baud(115200);//set baudrate to 115200
Gerth 41:e9fd0670f70c 457
Gerth 35:0141a1fe8138 458 while(1) {// while (1) so continious loop
Gerth 35:0141a1fe8138 459 valuechangebutton.fall(&valuechange);// used to change the filter gains
Gerth 35:0141a1fe8138 460 checktimer.reset();// reset the timer to check the time it takes to run the entire control loop
Gerth 26:c935e39cce8a 461 checktimer.start();
Gerth 20:c5fb2ff5d457 462 if (changemodebutton==0) {// check if the change mode button is pressed
Gerth 35:0141a1fe8138 463 changemode();//call changemode
Gerth 1:917c07a4f3ec 464 }
Gerth 20:c5fb2ff5d457 465 if (scopedata_go==true) {//send scopedata
Gerth 38:df0b0abea598 466 //TIME THIS LOOP TAKES: 0.000008 SEC (PEAKS AT 0.000015)
Gerth 35:0141a1fe8138 467 scopedata(y_start);// call scopedata, use baseline as desired y position
Gerth 26:c935e39cce8a 468 scopedata_go=false;
Gerth 9:4ee354663560 469 }
Gerth 20:c5fb2ff5d457 470 if (safetyandthreshold_go==true) {// check the potmeters
Gerth 26:c935e39cce8a 471 //TIME THIS LOOP TAKES: 0.000032 SEC
Gerth 35:0141a1fe8138 472 safetyandthreshold(); // read out the potmeters on top of the biorobotics board
Gerth 9:4ee354663560 473 safetyandthreshold_go=false;
Gerth 26:c935e39cce8a 474 }
Gerth 7:7fbb2c028778 475 ///////////////////////////////////////////NORMAL RUNNING MODE
Gerth 7:7fbb2c028778 476 if(modecounter==0) {
Gerth 6:37c94a5e205f 477 if (switchedmode==true) {
Gerth 35:0141a1fe8138 478 pc.printf("Program running\n");// print to serial
Gerth 35:0141a1fe8138 479 ledgreen=0;// green led on
Gerth 35:0141a1fe8138 480 led1=led2=ledred=ledblue=1;//rest of
Gerth 43:a5f47a1f11d2 481 gotopos(0,demo_y_goal,1);
Gerth 42:386fc2fcfb22 482 desired_position=0;
Gerth 6:37c94a5e205f 483 switchedmode=false;
Gerth 6:37c94a5e205f 484 }
Gerth 20:c5fb2ff5d457 485 if (filter_go==true) {// filter the emg signal
Gerth 26:c935e39cce8a 486 // TIME THIS LOOP TAKES: 0.000173 SEC
Gerth 41:e9fd0670f70c 487 filtereverything(false);
Gerth 26:c935e39cce8a 488 filter_go=false;
Gerth 6:37c94a5e205f 489 }
Gerth 20:c5fb2ff5d457 490 if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position
Gerth 26:c935e39cce8a 491 // TIME THIS LOOP TAKES: 0.000005 SEC
Gerth 17:72d3522165ac 492 readsignal();
Gerth 17:72d3522165ac 493 readsignal_go=false;
Gerth 26:c935e39cce8a 494 }
Gerth 20:c5fb2ff5d457 495 if (control_go==true) {// calculate angles from positions and send error to controller
Gerth 26:c935e39cce8a 496 //TIME THIS LOOP TAKES: 0.000223 SEC
Gerth 26:c935e39cce8a 497
Gerth 20:c5fb2ff5d457 498 desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
Gerth 20:c5fb2ff5d457 499 desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
Gerth 16:63320b8f79c2 500
Gerth 39:ebcf0a60f58b 501 double error1=(desired_angle1-counttorad*encoder1.getPulses());
Gerth 39:ebcf0a60f58b 502 double error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 31:8646e853979f 503 mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
Gerth 6:37c94a5e205f 504 control_go=false;
Gerth 26:c935e39cce8a 505 }
Gerth 1:917c07a4f3ec 506 }
Gerth 7:7fbb2c028778 507 ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
Gerth 6:37c94a5e205f 508 if (modecounter==1) {
Gerth 6:37c94a5e205f 509 if(switchedmode==true) {
Gerth 6:37c94a5e205f 510 pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
Gerth 16:63320b8f79c2 511 led1=led2=ledred=0;
Gerth 16:63320b8f79c2 512 ledgreen=ledblue=1;
Gerth 6:37c94a5e205f 513 switchedmode=false;
Gerth 14:4c4f45a1dd23 514 }
Gerth 14:4c4f45a1dd23 515 if (ledblink_go==true) {
Gerth 20:c5fb2ff5d457 516 led1=!led1;// blink rigth led on biorobotics shield (because rigth arm is being calibrated)
Gerth 16:63320b8f79c2 517 ledblink_go=false;
Gerth 6:37c94a5e205f 518 }
Gerth 20:c5fb2ff5d457 519 if (readbuttoncalibrate_go==true) {//check wich button is pressed and adjust wanted angle of rigth arm
Gerth 9:4ee354663560 520 if (buttonR.read()==0 && buttonL.read()==1) {
Gerth 20:c5fb2ff5d457 521 desired_angle1 += (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 9:4ee354663560 522 readbuttoncalibrate_go=false;
Gerth 9:4ee354663560 523 }
Gerth 9:4ee354663560 524 if (buttonR.read()==1 && buttonL.read()==0) {
Gerth 20:c5fb2ff5d457 525 desired_angle1 -= (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 9:4ee354663560 526 readbuttoncalibrate_go=false;
Gerth 9:4ee354663560 527 }
Gerth 9:4ee354663560 528 }
Gerth 25:21dcd3f9eac2 529 if (control_go==true) {// calculate errors and send them to controllers
Gerth 25:21dcd3f9eac2 530 error1=(desired_angle1-counttorad*encoder1.getPulses());
Gerth 35:0141a1fe8138 531 error2=0;// only adjust rigth arm
Gerth 31:8646e853979f 532 mycontroller.PI(error1,error2,Kp_move,Ki_move);
Gerth 20:c5fb2ff5d457 533 control_go=false;
Gerth 20:c5fb2ff5d457 534 }
Gerth 8:54a7da09ccad 535 }
Gerth 8:54a7da09ccad 536 ////////////////////////////////////////////CALIBRATE LEFT ARM
Gerth 8:54a7da09ccad 537 if (modecounter==2) {
Gerth 8:54a7da09ccad 538 if(switchedmode==true) {
Gerth 8:54a7da09ccad 539 pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
Gerth 16:63320b8f79c2 540 led1=led2=ledred=0;
Gerth 16:63320b8f79c2 541 ledgreen=ledblue=1;
Gerth 8:54a7da09ccad 542 switchedmode=false;
Gerth 8:54a7da09ccad 543 }
Gerth 14:4c4f45a1dd23 544 if (ledblink_go==true) {
Gerth 20:c5fb2ff5d457 545 led2=!led2;// blink left led on biorobotics shield (because left arm is being calibrated)
Gerth 16:63320b8f79c2 546 ledblink_go=false;
Gerth 14:4c4f45a1dd23 547 }
Gerth 25:21dcd3f9eac2 548 if (readbuttoncalibrate_go==true) {//
Gerth 20:c5fb2ff5d457 549 if (buttonR.read()==0 && buttonL.read()==1) {//check wich button is pressed and adjust wanted angle of left arm
Gerth 20:c5fb2ff5d457 550 desired_angle2 += (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 8:54a7da09ccad 551 readbuttoncalibrate_go=false;
Gerth 7:7fbb2c028778 552 }
Gerth 8:54a7da09ccad 553 if (buttonR.read()==1 && buttonL.read()==0) {
Gerth 20:c5fb2ff5d457 554 desired_angle2 -= (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 8:54a7da09ccad 555 readbuttoncalibrate_go=false;
Gerth 7:7fbb2c028778 556 }
Gerth 6:37c94a5e205f 557 }
Gerth 25:21dcd3f9eac2 558 if (control_go==true) {// calculate errors and send to controller
Gerth 35:0141a1fe8138 559 error1=0; // only adjus left arm
Gerth 25:21dcd3f9eac2 560 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 31:8646e853979f 561 mycontroller.PI(error1,error2,Kp_move,Ki_move);
Gerth 20:c5fb2ff5d457 562 control_go=false;
Gerth 20:c5fb2ff5d457 563 }
Gerth 3:48438eea184e 564 }
Gerth 16:63320b8f79c2 565 ///////////////////////////////BUTTONCONTROLMODE
Gerth 15:17de575b7385 566 if (modecounter==3) {
Gerth 16:63320b8f79c2 567 if (switchedmode==true) {
Gerth 19:6f22b5687587 568 pc.printf("Buttonmode, you can use the buttons to control the robot\n");
Gerth 16:63320b8f79c2 569 led1=led2=0;
Gerth 25:21dcd3f9eac2 570 ledred=ledblue=1;
Gerth 21:6954cc25f2a7 571 encoder1.reset();// reset encoders so they are at 0 degrees
Gerth 21:6954cc25f2a7 572 encoder2.reset();
Gerth 36:3856509519cf 573 desired_position=0;//set desired position to the middle of the field, where the pod actually is.
Gerth 16:63320b8f79c2 574 switchedmode=false;
Gerth 16:63320b8f79c2 575 }
Gerth 16:63320b8f79c2 576 if (ledblink_go==true) {
Gerth 35:0141a1fe8138 577 ledgreen=!ledgreen; // blink green led
Gerth 16:63320b8f79c2 578 ledblink_go=false;
Gerth 15:17de575b7385 579 }
Gerth 35:0141a1fe8138 580 if (readsignal_go==true) {// read buttons and adjust wanted position
Gerth 17:72d3522165ac 581 readsignalbutton();
Gerth 17:72d3522165ac 582 readsignal_go=false;
Gerth 17:72d3522165ac 583 }
Gerth 20:c5fb2ff5d457 584 if (control_go==true) {// calculate wanted angles from position, errors and send to controller
Gerth 20:c5fb2ff5d457 585 desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
Gerth 20:c5fb2ff5d457 586 desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
Gerth 17:72d3522165ac 587
Gerth 25:21dcd3f9eac2 588 error1=(desired_angle1-counttorad*encoder1.getPulses());
Gerth 25:21dcd3f9eac2 589 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 31:8646e853979f 590 mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
Gerth 17:72d3522165ac 591 control_go=false;
Gerth 16:63320b8f79c2 592 }
Gerth 16:63320b8f79c2 593 }
Gerth 41:e9fd0670f70c 594 ///////////////////////////////////DEMO MODE
Gerth 41:e9fd0670f70c 595 if (modecounter==4) {
Gerth 41:e9fd0670f70c 596 if (switchedmode==true) {
Gerth 41:e9fd0670f70c 597 pc.printf("Demo mode, the pod moves around the field\n");
Gerth 43:a5f47a1f11d2 598 ledred=ledblue=ledgreen=0;
Gerth 43:a5f47a1f11d2 599 gotopos(0,demo_y_goal,1); // go to goal @x=0
Gerth 41:e9fd0670f70c 600 led1=led2=0;
Gerth 41:e9fd0670f70c 601 switchedmode=false;
Gerth 41:e9fd0670f70c 602 }
Gerth 43:a5f47a1f11d2 603 gotopos(demo_goal_edge_left,demo_y_goal,0.25);
Gerth 43:a5f47a1f11d2 604 gotopos(demo_goal_edge_left,demo_y_back,0.25);
Gerth 43:a5f47a1f11d2 605 gotopos(demo_x_edge_left,demo_y_back,0.5);
Gerth 43:a5f47a1f11d2 606 gotopos(demo_x_edge_left,demo_y_front,2);
Gerth 43:a5f47a1f11d2 607 gotopos(demo_x_edge_rigth,demo_y_front,2);
Gerth 43:a5f47a1f11d2 608 gotopos(demo_x_edge_rigth,demo_y_back,2);
Gerth 43:a5f47a1f11d2 609 gotopos(demo_goal_edge_rigth,demo_y_back,1.25);
Gerth 43:a5f47a1f11d2 610 gotopos(demo_goal_edge_rigth,demo_y_goal,0.25);
Gerth 43:a5f47a1f11d2 611 gotopos(0,demo_y_goal,0.5);
Gerth 41:e9fd0670f70c 612 }
Gerth 35:0141a1fe8138 613 //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE
Gerth 41:e9fd0670f70c 614 if(modecounter==5) {
Gerth 33:1c7b498ded25 615 if (switchedmode==true) {
Gerth 41:e9fd0670f70c 616 pc.printf("Calibrate the EMG signals and threshold\n");
Gerth 33:1c7b498ded25 617 ledgreen=1;
Gerth 42:386fc2fcfb22 618 ledred=ledblue=0;
Gerth 33:1c7b498ded25 619 switchedmode=false;
Gerth 33:1c7b498ded25 620 }
Gerth 33:1c7b498ded25 621 if(ledblink_go==true) {
Gerth 33:1c7b498ded25 622 ledgreen=!ledgreen;
Gerth 33:1c7b498ded25 623 ledred=!ledred;
Gerth 33:1c7b498ded25 624 ledblink_go=false;
Gerth 33:1c7b498ded25 625 }
Gerth 41:e9fd0670f70c 626 if (filter_go==true) {// filter the emg signal
Gerth 33:1c7b498ded25 627 // TIME THIS LOOP TAKES: 0.000173 SEC
Gerth 33:1c7b498ded25 628 filtereverything(false);
Gerth 33:1c7b498ded25 629 filter_go=false;
Gerth 33:1c7b498ded25 630 }
Gerth 33:1c7b498ded25 631 }
Gerth 35:0141a1fe8138 632 checktimervalue=checktimer.read(); // write the time it has taken to a variable, so this variable can be displayed using hidschope.
Gerth 41:e9fd0670f70c 633 // if chectimer.read() is used in scopedata, the value is probably ~0
Gerth 41:e9fd0670f70c 634 //because scopedata is called as one of the first functoins
Gerth 25:21dcd3f9eac2 635 checktimer.stop();
Gerth 0:dd66fff537d7 636 }
Gerth 8:54a7da09ccad 637 }