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@39:ebcf0a60f58b, 2015-10-29 (annotated)

Committer:
Gerth
Date:
Thu Oct 29 14:03:35 2015 +0000
Revision:
39:ebcf0a60f58b
Parent:
38:df0b0abea598
Child:
40:8b25c0531340
converted floats to doubles

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