Peter Knoben / Mbed 2 deprecated Oudverslag

Oud verslag voor Biquad.

Dependencies:   Biquad HIDScope QEI angleandposition controlandadjust mbed

Fork of includeair by Jasper Gerth

main.cpp@47:29ea2915b811, 2015-11-02 (annotated)

Committer:
Gerth
Date:
Mon Nov 02 15:39:16 2015 +0000
Revision:
47:29ea2915b811
Parent:
46:c03f2c576630 
ingeleverd

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 44:fd7b3ace6c19 40 InterruptIn 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 44:fd7b3ace6c19 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 44:fd7b3ace6c19 131 int demo_go=0;
Gerth 44:fd7b3ace6c19 132
Gerth 44:fd7b3ace6c19 133
Gerth 41:e9fd0670f70c 134
Gerth 1:917c07a4f3ec 135 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
Gerth 1:917c07a4f3ec 136 volatile bool scopedata_go=false,
Gerth 1:917c07a4f3ec 137 control_go=false,
Gerth 1:917c07a4f3ec 138 filter_go=false,
Gerth 3:48438eea184e 139 safetyandthreshold_go=false,
Gerth 6:37c94a5e205f 140 readsignal_go=false,
Gerth 8:54a7da09ccad 141 switchedmode=true,
Gerth 14:4c4f45a1dd23 142 readbuttoncalibrate_go=false,
Gerth 41:e9fd0670f70c 143 ledblink_go=false,
Gerth 41:e9fd0670f70c 144 demo_next_step_go=false;
Gerth 1:917c07a4f3ec 145
Gerth 1:917c07a4f3ec 146 void scopedata_activate()
Gerth 1:917c07a4f3ec 147 {
Gerth 1:917c07a4f3ec 148 scopedata_go=true;
Gerth 1:917c07a4f3ec 149 }
Gerth 1:917c07a4f3ec 150 void control_activate()
Gerth 1:917c07a4f3ec 151 {
Gerth 1:917c07a4f3ec 152 control_go=true;
Gerth 1:917c07a4f3ec 153 }
Gerth 1:917c07a4f3ec 154 void filter_activate()
Gerth 1:917c07a4f3ec 155 {
Gerth 1:917c07a4f3ec 156 filter_go=true;
Gerth 1:917c07a4f3ec 157 }
Gerth 1:917c07a4f3ec 158 void safetyandthreshold_activate()
Gerth 1:917c07a4f3ec 159 {
Gerth 1:917c07a4f3ec 160 safetyandthreshold_go=true;
Gerth 1:917c07a4f3ec 161 }
Gerth 3:48438eea184e 162 void readsignal_activate()
Gerth 3:48438eea184e 163 {
Gerth 3:48438eea184e 164 readsignal_go=true;
Gerth 3:48438eea184e 165 }
Gerth 7:7fbb2c028778 166 void readbuttoncalibrate_activate()
Gerth 7:7fbb2c028778 167 {
Gerth 7:7fbb2c028778 168 readbuttoncalibrate_go=true;
Gerth 7:7fbb2c028778 169 }
Gerth 14:4c4f45a1dd23 170 void ledblink_activate()
Gerth 14:4c4f45a1dd23 171 {
Gerth 14:4c4f45a1dd23 172 ledblink_go=true;
Gerth 14:4c4f45a1dd23 173 }
Gerth 41:e9fd0670f70c 174 void demo_next_step_activate()
Gerth 41:e9fd0670f70c 175 {
Gerth 41:e9fd0670f70c 176 demo_next_step_go=true;
Gerth 41:e9fd0670f70c 177 }
Gerth 1:917c07a4f3ec 178
Gerth 1:917c07a4f3ec 179 ////////////////////////FUNCTIONS
Gerth 1:917c07a4f3ec 180 //gather data and send to scope
Gerth 32:c940f6b6a6a0 181 void scopedata(double wanted_y)
Gerth 1:917c07a4f3ec 182 {
Gerth 47:29ea2915b811 183 scope.set(0,desired_position); // desired x position
Gerth 47:29ea2915b811 184 scope.set(1,wanted_y); // desired y position
Gerth 47:29ea2915b811 185 scope.set(2,filteredsignal1); // filterded emg signal rigth arm
Gerth 47:29ea2915b811 186 scope.set(3,filteredsignal2); // filtered emg signal left arm
Gerth 35:0141a1fe8138 187 scope.set(4,threshold_value); // threshold value
Gerth 35:0141a1fe8138 188 scope.set(5,mycontroller.motor1pwm()); //pwm signal send to motor 1
Gerth 35:0141a1fe8138 189 scope.send(); // send info to HIDScope server
Gerth 4:bf7765b0f612 190 }
Gerth 1:917c07a4f3ec 191 //read potmeters and adjust the safetyfactor and threshold
Gerth 1:917c07a4f3ec 192 void safetyandthreshold()
Gerth 1:917c07a4f3ec 193 {
Gerth 3:48438eea184e 194 mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal
Gerth 28:71a90073e482 195 threshold_value=((ceil (100*threshold_pot.read()) )/100); // adjust the threshold value between 0 and 1 rounded to 2 decimals
Gerth 1:917c07a4f3ec 196 }
Gerth 1:917c07a4f3ec 197 /////filter
Gerth 28:71a90073e482 198 void filtereverything(bool makeempty)
Gerth 1:917c07a4f3ec 199 {
Gerth 1:917c07a4f3ec 200 //pass1 so f1
Gerth 2:c7707856d137 201 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 202 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 203
Gerth 1:917c07a4f3ec 204 //pass2 so f2
Gerth 2:c7707856d137 205 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 206 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 207
Gerth 1:917c07a4f3ec 208 //pass3 so f3
Gerth 2:c7707856d137 209 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 210 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 211
Gerth 1:917c07a4f3ec 212 //pass4 so f4
Gerth 2:c7707856d137 213 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 214 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 215
Gerth 1:917c07a4f3ec 216 //pass5 so f5
Gerth 2:c7707856d137 217 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 218 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 219
Gerth 1:917c07a4f3ec 220 ///// take absolute value
Gerth 2:c7707856d137 221 double pass5_emg1_abs=(fabs(pass5_emg1));
Gerth 2:c7707856d137 222 double pass5_emg2_abs=(fabs(pass5_emg2));
Gerth 1:917c07a4f3ec 223
Gerth 1:917c07a4f3ec 224 //pass6 so f6
Gerth 2:c7707856d137 225 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 226 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 227
Gerth 1:917c07a4f3ec 228
Gerth 1:917c07a4f3ec 229 //pass7 so f7
Gerth 2:c7707856d137 230 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 231 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 232
Gerth 35:0141a1fe8138 233 filteredsignal1=(pass7_emg1*9e11*filter_extragain1);//this is needed to make the signal ~1 when flexed
Gerth 35:0141a1fe8138 234 filteredsignal2=(pass7_emg2*9e11*filter_extragain2);// filter_extragain is adjusted via serial
Gerth 30:cd4d9754a357 235
Gerth 35:0141a1fe8138 236 if (makeempty==true) {//this is needed so the filtered value is not high after shooting basically it resets the filter
Gerth 28:71a90073e482 237 pass1_emg1 = pass1_emg2 =pass2_emg1 =pass2_emg2 =pass3_emg1 = pass3_emg2 =pass4_emg1 =pass4_emg2 =pass5_emg1 =pass5_emg2 =0;
Gerth 28:71a90073e482 238 pass5_emg1_abs=pass5_emg2_abs=pass6_emg1 =pass6_emg2 =pass7_emg1=pass7_emg2 =filteredsignal1=filteredsignal2=0;
Gerth 28:71a90073e482 239 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 240 v1_f5_emg2=v1_f6_emg1=v1_f6_emg2=v1_f7_emg1=v1_f7_emg2=0;
Gerth 28:71a90073e482 241 }
Gerth 28:71a90073e482 242
Gerth 1:917c07a4f3ec 243 }
Gerth 45:653370fa8b67 244 //adjust controller values (and wheter or not demo is shown) when sw2 is pressed
Gerth 1:917c07a4f3ec 245 void valuechange()
Gerth 1:917c07a4f3ec 246 {
Gerth 45:653370fa8b67 247 if (modecounter==4) {// cannot be changed if demo loop is running
Gerth 45:653370fa8b67 248 pc.printf("Not now!\n");
Gerth 45:653370fa8b67 249 } else {
Gerth 45:653370fa8b67 250 mycontroller.STOP(); // stop motors
Gerth 45:653370fa8b67 251 pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1); // print extra gain for rigth arm
Gerth 45:653370fa8b67 252 pc.scanf("%f", &filter_extragain1); //read the input from serial and write to filter_extragain1
Gerth 41:e9fd0670f70c 253
Gerth 45:653370fa8b67 254 pc.printf("Extra gain for left arm is now %f, enter new value\n",filter_extragain2); // print extra gain for left arm
Gerth 45:653370fa8b67 255 pc.scanf("%f", &filter_extragain2);//read the input from serial and write to filter_extragain2
Gerth 44:fd7b3ace6c19 256
Gerth 45:653370fa8b67 257 pc.printf("Do you want to enter demo mode after button control mode 1/0? (1 for yes, 0 for no.\n");
Gerth 45:653370fa8b67 258 pc.scanf("%i", &demo_go);//read the input from serial and write to filter_extragain2
Gerth 44:fd7b3ace6c19 259
Gerth 45:653370fa8b67 260 pc.printf("Done\n");
Gerth 45:653370fa8b67 261 }
Gerth 1:917c07a4f3ec 262 }
Gerth 11:c10651055871 263
Gerth 20:c5fb2ff5d457 264 // shoot the pod forward
Gerth 19:6f22b5687587 265 void shoot()
Gerth 14:4c4f45a1dd23 266 {
Gerth 41:e9fd0670f70c 267 ledgreen=1;
Gerth 32:c940f6b6a6a0 268 double time=0;
Gerth 35:0141a1fe8138 269 double stepsize=(0.5*PIE)/(timetoshoot*control_frequency); //calculate stepsize
Gerth 35:0141a1fe8138 270 double profile_angle=0;// used for the profilie
Gerth 35:0141a1fe8138 271 double x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());// calculate actual x positon from the encoder angles
Gerth 35:0141a1fe8138 272 double y_during_punch=y_start;//set initial y value to baseline
Gerth 35:0141a1fe8138 273 Timer shoottimer; // make a timer
Gerth 19:6f22b5687587 274 shoottimer.reset();
Gerth 19:6f22b5687587 275 shoottimer.start();
Gerth 23:1c4a18799464 276 //forward
Gerth 19:6f22b5687587 277 while (time<=timetoshoot) {
Gerth 35:0141a1fe8138 278 ledblue=!ledblue; // blink blue led (very fast, so not noticable blinking)
Gerth 25:21dcd3f9eac2 279
Gerth 32:c940f6b6a6a0 280 profile_angle+=stepsize; // add stepsize to angle
Gerth 33:1c7b498ded25 281
Gerth 32:c940f6b6a6a0 282 y_during_punch=(y_punch-y_start)*(1-cos(profile_angle))+y_start; // calculate y position for shooting
Gerth 33:1c7b498ded25 283
Gerth 20:c5fb2ff5d457 284 if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety
Gerth 19:6f22b5687587 285 y_during_punch=y_punch;
Gerth 19:6f22b5687587 286 } else {
Gerth 19:6f22b5687587 287 y_during_punch=y_during_punch;
Gerth 33:1c7b498ded25 288 }
Gerth 19:6f22b5687587 289
Gerth 29:cd47a5a772db 290 desired_angle1=anglepos.positiontoangle1(x_punch,y_during_punch);// calculate desired angles
Gerth 29:cd47a5a772db 291 desired_angle2=anglepos.positiontoangle2(x_punch,y_during_punch);
Gerth 19:6f22b5687587 292
Gerth 25:21dcd3f9eac2 293 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
Gerth 25:21dcd3f9eac2 294 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 19:6f22b5687587 295
Gerth 31:8646e853979f 296 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
Gerth 46:c03f2c576630 297 scopedata(y_during_punch);//send data to hidscope WARING higher freqyency than normal
Gerth 19:6f22b5687587 298
Gerth 20:c5fb2ff5d457 299 time+=(Ts_control);// add time it should take to calculated time
Gerth 28:71a90073e482 300 filtereverything(true);//set al filter variables to 0
Gerth 41:e9fd0670f70c 301
Gerth 35:0141a1fe8138 302 wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need,
Gerth 41:e9fd0670f70c 303 // if this is not done the loop wil go to fast and the motors can't keep up
Gerth 41:e9fd0670f70c 304 // this is because the control loop takes far less time than Ts_control
Gerth 11:c10651055871 305 }
Gerth 23:1c4a18799464 306 //back
Gerth 35:0141a1fe8138 307 time=0; // reset time
Gerth 35:0141a1fe8138 308 shoottimer.reset();//reset timer
Gerth 35:0141a1fe8138 309 stepsize=(PIE)/(timetogoback*control_frequency);//calculate stepsize
Gerth 35:0141a1fe8138 310 profile_angle=0;//reset angle for the displacement profile
Gerth 35:0141a1fe8138 311 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 312 while (time<=timetogoback) {
Gerth 35:0141a1fe8138 313 ledblue=!ledblue;// blink blue led
Gerth 25:21dcd3f9eac2 314
Gerth 32:c940f6b6a6a0 315 profile_angle+=stepsize; // add stepsize to y position
Gerth 33:1c7b498ded25 316 y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting
Gerth 23:1c4a18799464 317 if (y_during_punch<=y_start) {//to check if y position is not smaller than y_start for safety
Gerth 23:1c4a18799464 318 y_during_punch=y_start;
Gerth 23:1c4a18799464 319 } else {
Gerth 23:1c4a18799464 320 y_during_punch=y_during_punch;
Gerth 23:1c4a18799464 321 }
Gerth 23:1c4a18799464 322
Gerth 34:bc2d64e86cb9 323 desired_angle1=anglepos.positiontoangle1(desired_position,y_during_punch);// calculate desired angles
Gerth 34:bc2d64e86cb9 324 desired_angle2=anglepos.positiontoangle2(desired_position,y_during_punch);
Gerth 41:e9fd0670f70c 325
Gerth 23:1c4a18799464 326
Gerth 25:21dcd3f9eac2 327 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
Gerth 25:21dcd3f9eac2 328 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 23:1c4a18799464 329
Gerth 31:8646e853979f 330 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
Gerth 46:c03f2c576630 331 scopedata(y_during_punch);//send data to hidscope WARING higher freqyency than normal
Gerth 23:1c4a18799464 332
Gerth 23:1c4a18799464 333 time+=(Ts_control);// add time it should take to calculated time
Gerth 35:0141a1fe8138 334 filtereverything(false);//start filtering the signal (lower frewquency than normal)
Gerth 23:1c4a18799464 335 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 336 }
Gerth 19:6f22b5687587 337 shoottimer.stop();
Gerth 35:0141a1fe8138 338 ledblue=1; // blue led off
Gerth 35:0141a1fe8138 339 ledgreen=0; // green led on
Gerth 11:c10651055871 340 }
Gerth 11:c10651055871 341
Gerth 16:63320b8f79c2 342 ////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION
Gerth 3:48438eea184e 343 void readsignal()
Gerth 3:48438eea184e 344 {
Gerth 11:c10651055871 345 //check if pod has to shoot
Gerth 4:bf7765b0f612 346 if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
Gerth 35:0141a1fe8138 347 led1=led2=1; // ligth up both leds
Gerth 11:c10651055871 348 shoot();
Gerth 14:4c4f45a1dd23 349 // check if pod has to move to the right
Gerth 4:bf7765b0f612 350 } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
Gerth 35:0141a1fe8138 351 led1=1;// ligth up rigth led
Gerth 4:bf7765b0f612 352 led2=0;
Gerth 16:63320b8f79c2 353 desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
Gerth 11:c10651055871 354 if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
Gerth 11:c10651055871 355 desired_position=maxdisplacement;
Gerth 11:c10651055871 356 } else {
Gerth 11:c10651055871 357 desired_position=desired_position;
Gerth 11:c10651055871 358 }
Gerth 14:4c4f45a1dd23 359 // check if pod has to move to the left
Gerth 4:bf7765b0f612 360 } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
Gerth 35:0141a1fe8138 361 led1=0; // ligth up left led
Gerth 4:bf7765b0f612 362 led2=1;
Gerth 16:63320b8f79c2 363 desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
Gerth 11:c10651055871 364 if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
Gerth 14:4c4f45a1dd23 365 desired_position=(-1*maxdisplacement);
Gerth 11:c10651055871 366 } else {
Gerth 11:c10651055871 367 desired_position=desired_position;
Gerth 11:c10651055871 368 }
Gerth 3:48438eea184e 369 } else {
Gerth 4:bf7765b0f612 370 led1=led2=0;
Gerth 3:48438eea184e 371 }
Gerth 16:63320b8f79c2 372 }
Gerth 28:71a90073e482 373
Gerth 16:63320b8f79c2 374 ///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION
Gerth 35:0141a1fe8138 375 void readsignalbutton() // same as above, only with buttons as input
Gerth 16:63320b8f79c2 376 {
Gerth 19:6f22b5687587 377 //write value of button to variable
Gerth 17:72d3522165ac 378 int buttonr=buttonR.read();
Gerth 17:72d3522165ac 379 int buttonl=buttonL.read();
Gerth 16:63320b8f79c2 380 //check if pod has to shoot
Gerth 17:72d3522165ac 381 if (buttonr==0 && buttonl==0) {
Gerth 16:63320b8f79c2 382 led1=led2=1;
Gerth 16:63320b8f79c2 383 shoot();
Gerth 16:63320b8f79c2 384 // check if pod has to move to the right
Gerth 17:72d3522165ac 385 } else if (buttonr==0 && buttonl==1) {
Gerth 16:63320b8f79c2 386 led1=1;
Gerth 16:63320b8f79c2 387 led2=0;
Gerth 16:63320b8f79c2 388 desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
Gerth 17:72d3522165ac 389 if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
Gerth 16:63320b8f79c2 390 desired_position=maxdisplacement;
Gerth 16:63320b8f79c2 391 } else {
Gerth 16:63320b8f79c2 392 desired_position=desired_position;
Gerth 16:63320b8f79c2 393 }
Gerth 17:72d3522165ac 394 // check if pod has to move to the left
Gerth 17:72d3522165ac 395 } else if (buttonr==1 && buttonl==0) {
Gerth 16:63320b8f79c2 396 led1=0;
Gerth 16:63320b8f79c2 397 led2=1;
Gerth 16:63320b8f79c2 398 desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
Gerth 17:72d3522165ac 399 if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
Gerth 16:63320b8f79c2 400 desired_position=(-1*maxdisplacement);
Gerth 16:63320b8f79c2 401 } else {
Gerth 16:63320b8f79c2 402 desired_position=desired_position;
Gerth 16:63320b8f79c2 403 }
Gerth 16:63320b8f79c2 404 } else {
Gerth 16:63320b8f79c2 405 led1=led2=0;
Gerth 16:63320b8f79c2 406 }
Gerth 3:48438eea184e 407 }
Gerth 3:48438eea184e 408
Gerth 28:71a90073e482 409 void changemode() //this makes the counter higher to switch between modes
Gerth 6:37c94a5e205f 410 {
Gerth 35:0141a1fe8138 411 mycontroller.STOP(); // stop the controller
Gerth 35:0141a1fe8138 412 switchedmode=true; // set switchedmode to true so in the main loop some statements are executed
Gerth 35:0141a1fe8138 413 modecounter++; // increase counter
Gerth 41:e9fd0670f70c 414 if (modecounter==6) { // reset counter if counter=6
Gerth 6:37c94a5e205f 415 modecounter=0;
Gerth 6:37c94a5e205f 416 } else {
Gerth 6:37c94a5e205f 417 modecounter=modecounter;
Gerth 6:37c94a5e205f 418 }
Gerth 45:653370fa8b67 419
Gerth 6:37c94a5e205f 420 }
Gerth 0:dd66fff537d7 421
Gerth 45:653370fa8b67 422 // function takes x,y and time as input and moves pod to the desired coordinates in the desired time
Gerth 43:a5f47a1f11d2 423 void gotopos(double desired_x_pos, double desired_y_pos, double time_to_pos)
Gerth 42:386fc2fcfb22 424 {
Gerth 43:a5f47a1f11d2 425 double timepos=0;
Gerth 43:a5f47a1f11d2 426 Timer postimer;
Gerth 43:a5f47a1f11d2 427 double pos_x_start=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
Gerth 43:a5f47a1f11d2 428 double pos_y_start=anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
Gerth 43:a5f47a1f11d2 429
Gerth 43:a5f47a1f11d2 430 double pos_stepsize_x=(pos_x_start-desired_x_pos)/(time_to_pos*control_frequency);
Gerth 43:a5f47a1f11d2 431 double pos_stepsize_y=(pos_y_start-desired_y_pos)/(time_to_pos*control_frequency);
Gerth 42:386fc2fcfb22 432
Gerth 43:a5f47a1f11d2 433 double pos_x_moving=pos_x_start;
Gerth 43:a5f47a1f11d2 434 double pos_y_moving=pos_y_start;
Gerth 45:653370fa8b67 435
Gerth 43:a5f47a1f11d2 436 postimer.start();
Gerth 43:a5f47a1f11d2 437 while(timepos<=time_to_pos) {
Gerth 43:a5f47a1f11d2 438 pos_x_moving-=pos_stepsize_x;
Gerth 43:a5f47a1f11d2 439 pos_y_moving-=pos_stepsize_y;
Gerth 42:386fc2fcfb22 440
Gerth 43:a5f47a1f11d2 441 desired_angle1=anglepos.positiontoangle1(pos_x_moving,pos_y_moving);// calculate desired angles
Gerth 43:a5f47a1f11d2 442 desired_angle2=anglepos.positiontoangle2( pos_x_moving,pos_y_moving);
Gerth 42:386fc2fcfb22 443
Gerth 42:386fc2fcfb22 444 error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
Gerth 42:386fc2fcfb22 445 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 42:386fc2fcfb22 446
Gerth 42:386fc2fcfb22 447 mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
Gerth 42:386fc2fcfb22 448
Gerth 43:a5f47a1f11d2 449 timepos+=(Ts_control);// add time it should take to calculated time
Gerth 43:a5f47a1f11d2 450 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 451 }
Gerth 42:386fc2fcfb22 452 }
Gerth 41:e9fd0670f70c 453
Gerth 16:63320b8f79c2 454 ///////////////////////////////////////////////////MAIN
Gerth 7:7fbb2c028778 455
Gerth 0:dd66fff537d7 456 int main()
Gerth 0:dd66fff537d7 457 {
Gerth 35:0141a1fe8138 458 //initiate tickers
Gerth 1:917c07a4f3ec 459 safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
Gerth 1:917c07a4f3ec 460 filter_ticker.attach(&filter_activate,1.0/filter_frequency);
Gerth 1:917c07a4f3ec 461 control_ticker.attach(&control_activate,1.0/control_frequency);
Gerth 1:917c07a4f3ec 462 scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
Gerth 3:48438eea184e 463 readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
Gerth 7:7fbb2c028778 464 readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
Gerth 14:4c4f45a1dd23 465 ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
Gerth 14:4c4f45a1dd23 466
Gerth 20:c5fb2ff5d457 467 pc.baud(115200);//set baudrate to 115200
Gerth 41:e9fd0670f70c 468
Gerth 35:0141a1fe8138 469 while(1) {// while (1) so continious loop
Gerth 35:0141a1fe8138 470 valuechangebutton.fall(&valuechange);// used to change the filter gains
Gerth 44:fd7b3ace6c19 471 changemodebutton.rise(&changemode);// interruptin for next mode
Gerth 35:0141a1fe8138 472 checktimer.reset();// reset the timer to check the time it takes to run the entire control loop
Gerth 26:c935e39cce8a 473 checktimer.start();
Gerth 44:fd7b3ace6c19 474
Gerth 20:c5fb2ff5d457 475 if (scopedata_go==true) {//send scopedata
Gerth 38:df0b0abea598 476 //TIME THIS LOOP TAKES: 0.000008 SEC (PEAKS AT 0.000015)
Gerth 35:0141a1fe8138 477 scopedata(y_start);// call scopedata, use baseline as desired y position
Gerth 26:c935e39cce8a 478 scopedata_go=false;
Gerth 9:4ee354663560 479 }
Gerth 20:c5fb2ff5d457 480 if (safetyandthreshold_go==true) {// check the potmeters
Gerth 26:c935e39cce8a 481 //TIME THIS LOOP TAKES: 0.000032 SEC
Gerth 35:0141a1fe8138 482 safetyandthreshold(); // read out the potmeters on top of the biorobotics board
Gerth 9:4ee354663560 483 safetyandthreshold_go=false;
Gerth 26:c935e39cce8a 484 }
Gerth 7:7fbb2c028778 485 ///////////////////////////////////////////NORMAL RUNNING MODE
Gerth 7:7fbb2c028778 486 if(modecounter==0) {
Gerth 6:37c94a5e205f 487 if (switchedmode==true) {
Gerth 35:0141a1fe8138 488 pc.printf("Program running\n");// print to serial
Gerth 44:fd7b3ace6c19 489 led1=led2=ledgreen=0;// green led on leds on top off
Gerth 44:fd7b3ace6c19 490 ledred=ledblue=1;//rest off
Gerth 45:653370fa8b67 491 gotopos(0,y_start,1);
Gerth 42:386fc2fcfb22 492 desired_position=0;
Gerth 6:37c94a5e205f 493 switchedmode=false;
Gerth 6:37c94a5e205f 494 }
Gerth 20:c5fb2ff5d457 495 if (filter_go==true) {// filter the emg signal
Gerth 26:c935e39cce8a 496 // TIME THIS LOOP TAKES: 0.000173 SEC
Gerth 41:e9fd0670f70c 497 filtereverything(false);
Gerth 26:c935e39cce8a 498 filter_go=false;
Gerth 6:37c94a5e205f 499 }
Gerth 20:c5fb2ff5d457 500 if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position
Gerth 26:c935e39cce8a 501 // TIME THIS LOOP TAKES: 0.000005 SEC
Gerth 17:72d3522165ac 502 readsignal();
Gerth 17:72d3522165ac 503 readsignal_go=false;
Gerth 26:c935e39cce8a 504 }
Gerth 20:c5fb2ff5d457 505 if (control_go==true) {// calculate angles from positions and send error to controller
Gerth 26:c935e39cce8a 506 //TIME THIS LOOP TAKES: 0.000223 SEC
Gerth 26:c935e39cce8a 507
Gerth 20:c5fb2ff5d457 508 desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
Gerth 20:c5fb2ff5d457 509 desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
Gerth 16:63320b8f79c2 510
Gerth 39:ebcf0a60f58b 511 double error1=(desired_angle1-counttorad*encoder1.getPulses());
Gerth 39:ebcf0a60f58b 512 double error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 31:8646e853979f 513 mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
Gerth 6:37c94a5e205f 514 control_go=false;
Gerth 26:c935e39cce8a 515 }
Gerth 1:917c07a4f3ec 516 }
Gerth 7:7fbb2c028778 517 ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
Gerth 6:37c94a5e205f 518 if (modecounter==1) {
Gerth 6:37c94a5e205f 519 if(switchedmode==true) {
Gerth 6:37c94a5e205f 520 pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
Gerth 16:63320b8f79c2 521 led1=led2=ledred=0;
Gerth 16:63320b8f79c2 522 ledgreen=ledblue=1;
Gerth 6:37c94a5e205f 523 switchedmode=false;
Gerth 14:4c4f45a1dd23 524 }
Gerth 14:4c4f45a1dd23 525 if (ledblink_go==true) {
Gerth 20:c5fb2ff5d457 526 led1=!led1;// blink rigth led on biorobotics shield (because rigth arm is being calibrated)
Gerth 16:63320b8f79c2 527 ledblink_go=false;
Gerth 6:37c94a5e205f 528 }
Gerth 20:c5fb2ff5d457 529 if (readbuttoncalibrate_go==true) {//check wich button is pressed and adjust wanted angle of rigth arm
Gerth 9:4ee354663560 530 if (buttonR.read()==0 && buttonL.read()==1) {
Gerth 20:c5fb2ff5d457 531 desired_angle1 += (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 9:4ee354663560 532 readbuttoncalibrate_go=false;
Gerth 9:4ee354663560 533 }
Gerth 9:4ee354663560 534 if (buttonR.read()==1 && buttonL.read()==0) {
Gerth 20:c5fb2ff5d457 535 desired_angle1 -= (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 9:4ee354663560 536 readbuttoncalibrate_go=false;
Gerth 9:4ee354663560 537 }
Gerth 9:4ee354663560 538 }
Gerth 25:21dcd3f9eac2 539 if (control_go==true) {// calculate errors and send them to controllers
Gerth 25:21dcd3f9eac2 540 error1=(desired_angle1-counttorad*encoder1.getPulses());
Gerth 35:0141a1fe8138 541 error2=0;// only adjust rigth arm
Gerth 31:8646e853979f 542 mycontroller.PI(error1,error2,Kp_move,Ki_move);
Gerth 20:c5fb2ff5d457 543 control_go=false;
Gerth 20:c5fb2ff5d457 544 }
Gerth 8:54a7da09ccad 545 }
Gerth 8:54a7da09ccad 546 ////////////////////////////////////////////CALIBRATE LEFT ARM
Gerth 8:54a7da09ccad 547 if (modecounter==2) {
Gerth 8:54a7da09ccad 548 if(switchedmode==true) {
Gerth 8:54a7da09ccad 549 pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
Gerth 16:63320b8f79c2 550 led1=led2=ledred=0;
Gerth 16:63320b8f79c2 551 ledgreen=ledblue=1;
Gerth 8:54a7da09ccad 552 switchedmode=false;
Gerth 8:54a7da09ccad 553 }
Gerth 14:4c4f45a1dd23 554 if (ledblink_go==true) {
Gerth 20:c5fb2ff5d457 555 led2=!led2;// blink left led on biorobotics shield (because left arm is being calibrated)
Gerth 16:63320b8f79c2 556 ledblink_go=false;
Gerth 14:4c4f45a1dd23 557 }
Gerth 25:21dcd3f9eac2 558 if (readbuttoncalibrate_go==true) {//
Gerth 20:c5fb2ff5d457 559 if (buttonR.read()==0 && buttonL.read()==1) {//check wich button is pressed and adjust wanted angle of left arm
Gerth 20:c5fb2ff5d457 560 desired_angle2 += (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 8:54a7da09ccad 561 readbuttoncalibrate_go=false;
Gerth 7:7fbb2c028778 562 }
Gerth 8:54a7da09ccad 563 if (buttonR.read()==1 && buttonL.read()==0) {
Gerth 20:c5fb2ff5d457 564 desired_angle2 -= (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 8:54a7da09ccad 565 readbuttoncalibrate_go=false;
Gerth 7:7fbb2c028778 566 }
Gerth 6:37c94a5e205f 567 }
Gerth 25:21dcd3f9eac2 568 if (control_go==true) {// calculate errors and send to controller
Gerth 35:0141a1fe8138 569 error1=0; // only adjus left arm
Gerth 25:21dcd3f9eac2 570 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 31:8646e853979f 571 mycontroller.PI(error1,error2,Kp_move,Ki_move);
Gerth 20:c5fb2ff5d457 572 control_go=false;
Gerth 20:c5fb2ff5d457 573 }
Gerth 3:48438eea184e 574 }
Gerth 16:63320b8f79c2 575 ///////////////////////////////BUTTONCONTROLMODE
Gerth 15:17de575b7385 576 if (modecounter==3) {
Gerth 16:63320b8f79c2 577 if (switchedmode==true) {
Gerth 19:6f22b5687587 578 pc.printf("Buttonmode, you can use the buttons to control the robot\n");
Gerth 16:63320b8f79c2 579 led1=led2=0;
Gerth 25:21dcd3f9eac2 580 ledred=ledblue=1;
Gerth 21:6954cc25f2a7 581 encoder1.reset();// reset encoders so they are at 0 degrees
Gerth 21:6954cc25f2a7 582 encoder2.reset();
Gerth 36:3856509519cf 583 desired_position=0;//set desired position to the middle of the field, where the pod actually is.
Gerth 16:63320b8f79c2 584 switchedmode=false;
Gerth 16:63320b8f79c2 585 }
Gerth 16:63320b8f79c2 586 if (ledblink_go==true) {
Gerth 35:0141a1fe8138 587 ledgreen=!ledgreen; // blink green led
Gerth 16:63320b8f79c2 588 ledblink_go=false;
Gerth 15:17de575b7385 589 }
Gerth 35:0141a1fe8138 590 if (readsignal_go==true) {// read buttons and adjust wanted position
Gerth 17:72d3522165ac 591 readsignalbutton();
Gerth 17:72d3522165ac 592 readsignal_go=false;
Gerth 17:72d3522165ac 593 }
Gerth 20:c5fb2ff5d457 594 if (control_go==true) {// calculate wanted angles from position, errors and send to controller
Gerth 20:c5fb2ff5d457 595 desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
Gerth 20:c5fb2ff5d457 596 desired_angle2=anglepos.positiontoangle2(desired_position,y_start);
Gerth 17:72d3522165ac 597
Gerth 25:21dcd3f9eac2 598 error1=(desired_angle1-counttorad*encoder1.getPulses());
Gerth 25:21dcd3f9eac2 599 error2=(desired_angle2-counttorad*encoder2.getPulses());
Gerth 31:8646e853979f 600 mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
Gerth 17:72d3522165ac 601 control_go=false;
Gerth 16:63320b8f79c2 602 }
Gerth 16:63320b8f79c2 603 }
Gerth 41:e9fd0670f70c 604 ///////////////////////////////////DEMO MODE
Gerth 45:653370fa8b67 605 if (modecounter==4 && demo_go==!1) {//if demo mode not enabled, move to next mode
Gerth 45:653370fa8b67 606 changemode();
Gerth 45:653370fa8b67 607 }
Gerth 44:fd7b3ace6c19 608 if (modecounter==4 && demo_go==1) {
Gerth 41:e9fd0670f70c 609 if (switchedmode==true) {
Gerth 41:e9fd0670f70c 610 pc.printf("Demo mode, the pod moves around the field\n");
Gerth 45:653370fa8b67 611 led1=led2=ledred=ledblue=ledgreen=0; // rgb on, leds on top off
Gerth 43:a5f47a1f11d2 612 gotopos(0,demo_y_goal,1); // go to goal @x=0
Gerth 41:e9fd0670f70c 613 switchedmode=false;
Gerth 41:e9fd0670f70c 614 }
Gerth 45:653370fa8b67 615
Gerth 45:653370fa8b67 616 // loop trough positions to make a path along the field, con of this method is that the mode is only changed when the loop is completed.
Gerth 45:653370fa8b67 617 // the button is read, but only when the loop is completed it is checked in what mode the robot has to be, that's why this mode is made optional,
Gerth 45:653370fa8b67 618 // because this mode is only neccesary for the presentation on 30-10-2015 (or for fun)
Gerth 44:fd7b3ace6c19 619 gotopos(demo_goal_edge_left,demo_y_goal,0.5);// in front of goal to left edge of goal
Gerth 44:fd7b3ace6c19 620 gotopos(demo_goal_edge_left,demo_y_back,0.25); // to back along left egde of goal
Gerth 44:fd7b3ace6c19 621 gotopos(demo_x_edge_left,demo_y_back,0.5); //to left edge along back
Gerth 44:fd7b3ace6c19 622 gotopos(demo_x_edge_left,demo_y_front,2);//from back to front along left edge
Gerth 44:fd7b3ace6c19 623 gotopos(demo_x_edge_rigth,demo_y_front,2);// from left to rigth along front edge
Gerth 44:fd7b3ace6c19 624 gotopos(demo_x_edge_rigth,demo_y_back,2);// from front to back along rigth edge
Gerth 44:fd7b3ace6c19 625 gotopos(demo_goal_edge_rigth,demo_y_back,0.5);//from rigth edge to rigth edge of goal along back
Gerth 44:fd7b3ace6c19 626 gotopos(demo_goal_edge_rigth,demo_y_goal,0.25); // from back to in front of goal along rigth edge goal
Gerth 44:fd7b3ace6c19 627 gotopos(0,demo_y_goal,0.5); // from rigth edge goal to middle in front of goal
Gerth 45:653370fa8b67 628 gotopos(0,y_start,0.5);
Gerth 44:fd7b3ace6c19 629 demo_go=0;
Gerth 41:e9fd0670f70c 630 }
Gerth 45:653370fa8b67 631
Gerth 35:0141a1fe8138 632 //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE
Gerth 41:e9fd0670f70c 633 if(modecounter==5) {
Gerth 33:1c7b498ded25 634 if (switchedmode==true) {
Gerth 41:e9fd0670f70c 635 pc.printf("Calibrate the EMG signals and threshold\n");
Gerth 44:fd7b3ace6c19 636 ledblue=ledgreen=1;
Gerth 44:fd7b3ace6c19 637 led1=led2=ledred=0;
Gerth 33:1c7b498ded25 638 switchedmode=false;
Gerth 33:1c7b498ded25 639 }
Gerth 33:1c7b498ded25 640 if(ledblink_go==true) {
Gerth 33:1c7b498ded25 641 ledgreen=!ledgreen;
Gerth 33:1c7b498ded25 642 ledred=!ledred;
Gerth 33:1c7b498ded25 643 ledblink_go=false;
Gerth 33:1c7b498ded25 644 }
Gerth 41:e9fd0670f70c 645 if (filter_go==true) {// filter the emg signal
Gerth 33:1c7b498ded25 646 // TIME THIS LOOP TAKES: 0.000173 SEC
Gerth 33:1c7b498ded25 647 filtereverything(false);
Gerth 33:1c7b498ded25 648 filter_go=false;
Gerth 33:1c7b498ded25 649 }
Gerth 33:1c7b498ded25 650 }
Gerth 35:0141a1fe8138 651 checktimervalue=checktimer.read(); // write the time it has taken to a variable, so this variable can be displayed using hidschope.
Gerth 41:e9fd0670f70c 652 // if chectimer.read() is used in scopedata, the value is probably ~0
Gerth 41:e9fd0670f70c 653 //because scopedata is called as one of the first functoins
Gerth 25:21dcd3f9eac2 654 checktimer.stop();
Gerth 0:dd66fff537d7 655 }
Gerth 8:54a7da09ccad 656 }