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@42:386fc2fcfb22, 2015-10-30 (annotated)

Committer:
Gerth
Date:
Fri Oct 30 10:42:36 2015 +0000
Revision:
42:386fc2fcfb22
Parent:
41:e9fd0670f70c
Child:
43:a5f47a1f11d2
added a function to move slowly to the goal;

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