Delen met projectgroep, Geen eindversie met comments
Fork of Project_Program_bijna_af by
main.cpp
00001 #include "mbed.h" 00002 #include "QEI.h" 00003 #include "math.h" 00004 #include "functions.h" 00005 00006 // Digital systems 00007 DigitalOut led(LED_RED); // Notification LED once a sequential phase has ended 00008 DigitalOut LedR(D0); // Feedback red lights 00009 DigitalOut LedB(D1); // Feedback blue lights 00010 DigitalOut LedG(D9); // Feedback green lights 00011 DigitalOut magneet(D2); // Switching signal of the magnet 00012 DigitalOut Direction(D4); // Direction engine: Motor left/right 00013 DigitalOut Direction2(D7); // Direction engine: Motor front/back 00014 00015 // PWM systems 00016 PwmOut PowerMotor(D5); // Diferent engine controls: Motor left/right 00017 PwmOut PowerMotor2(D6); // Diferent engine controls: Motor front/back 00018 PwmOut PowerServo(D3); // Diferent engine controls: Servo 00019 00020 // Analog systems 00021 AnalogIn emg(A1); // EMG input top board right arm 00022 AnalogIn emg2(A0); // EMG input bottom board left arm 00023 AnalogIn StopSystemA(A2); 00024 AnalogIn StopSystemB(A3); 00025 00026 // Other systems 00027 QEI EncoderMotorLR(D13,D12,NC,64,QEI::X4_ENCODING); // Encoder motor left/right 00028 QEI EncoderMotorFB(D11,D10,NC,64,QEI::X4_ENCODING); // Encoder motor front/back 00029 Ticker MotorWrite; // Ticker to write information in fixed intervals to the motors 00030 Ticker Sender; // Ticker to send graph data in fixed intervals to the pc 00031 Ticker sample_timer; // Ticker to measure EMG data in fixed intervals 00032 Timer timer; // Timer to check if the error is small enough for a certain time 00033 Timer spierrustrechts; // Timer to reduce repeatition in EMG input 00034 Timer spierrustlinks; // Timer to reduce repeatition in EMG input 00035 Timer Safety; 00036 00037 // changeable parameters 00038 double margin=15; // Distance to the ends of the rail in mm 00039 double upperlimitLR=570; // maximum length of the rail 00040 double speed_driving=0.15; // Speed the first motor drives at when controlling manual 00041 double Speed_driving_back=0.2; // Speed the first motor drives when going to start 00042 double speed_driving2=0.2; // Speed the second motor drives when controlling manual 00043 double Speed_driving_back2=0.2; // Speed the second motor drives when going to start 00044 const double Kp = 0.2; // Parameter P controller to set motor to the middle of the chess squares 00045 const double Kd = 10; // Parameter D controller to set motor to the middle of the chess squares 00046 const double Ki = 0.2; // Parameter i controller to set motor to the middle of the chess squares 00047 const double Factor=2.363636363636363; // Factor between gears 00048 int Right=1; // Boolean for the direction of the motor 00049 int Left=0; // Boolean for the direction of the motor 00050 // filter parameters 00051 double v1 = 0, v2 = 0, u = 0,va1=0,va2=0,u2=0; // Input variables for the filters 00052 double vNF1 = 0,vNF2 = 0, vNF3 = 0, vNF4 = 0, vNF5 = 0, vNF6 = 0,vaNF1 = 0,vaNF2 = 0, vaNF3 = 0, vaNF4 = 0, vaNF5 = 0, vaNF6 = 0; 00053 // parameters motor controller 00054 bool Excecute = false; // Boolean to put the robot in the middle of the chess area in x(left/right) direction 00055 bool Excecute2 = false; // Boolean to put the robot in waiting for the y input forward/backward 00056 bool Excecute3 = false; // Boolean to put the robot in the middle of the chess area in y(forward backward) direction 00057 bool Home = true; // Return to starting position 00058 bool CheckRightEMG=false; // A control system in the robot that the robot can't save data or move in the oposite direction 00059 bool CheckLeftEMG=false; // A control system in the robot that the robot can't save data or move in the oposite direction 00060 bool SignalRight=false; // Boolean which sets to true if a signal is given on the right arm 00061 bool SignalLeft=false; // Boolean which sets to true if a signal is given on the left arm 00062 bool Correct1=false; // Boolean which sets to true if the robot is at start in x direction 00063 bool Correct2=false; // Boolean which sets to true if the robot is at start in y direction 00064 const double Fs=100; // Frequency that information is writen to the motors 00065 const double twopi = 6.2831853071795; // definition of 2pi 00066 double Rotatie=0; // angle of the motor in x direction in radials 00067 double Goal = 0; // initial goal in x direction this is zero to stop the robot from starting to drive 00068 double Rotatie2=0; // angle of the motor in y direction in radials 00069 double Goal2 = 0; // initial goal in y direction this is zero to stop the robot from starting to drive 00070 double Error = 0; // Definition of the proportional error for the x direction 00071 double Errord = 0; // Definition of the derivative error for the x direction 00072 double Errori = 0; // Definition of the integratal error for the x direction 00073 double Errorp = 0; // Definition of the previous error in x direction used to calculate the derivative error and the integral error 00074 double Error2 = 0; // Definition of the proportional error for the y direction 00075 double Errord2 = 0; // Definition of the derivative error for the y direction 00076 double Errori2 = 0; // Definition of the integratal error for the y direction 00077 double Errorp2 = 0; // Definition of the previous error in y direction used to calculate the derivative error and the integral error 00078 double speed = 0; // Speed of motor in x direction(left/right) 00079 double speed2=0; // Speed of motor in y direction(front/back) 00080 int NumberTimesPressed=1; // Parameter to help the control 00081 int NumberTimesPressed2=1; 00082 int NumberTimesPakOp=0; 00083 // Declaration 00084 double buffTot; // Declaration for the moving average of the right arm EMG signal 00085 double buffaTot; // Declaration for the moving average of the left arm EMG signal 00086 int Pulses; // Declaration for the variable to know the number of pulses from the encoder in x direction 00087 int Pulses2; // Declaration for the variable to know the number of pulses from the encoder in y direction 00088 float Vakjex; // Used to calculate in which area the robot is to put it in the middle in x direction 00089 int VakjexNatural; // "" 00090 int Vakjexround; // "" 00091 float Vakjey; // Used to calculate in which area the robot is to put it in the middle in y direction 00092 int VakjeyNatural; // "" 00093 int Vakjeyround; // "" 00094 double ValueStopSystemA; // Measurement stop x direction 00095 double ValueStopSystemB; // Measurement stop y direction 00096 00097 // functions 00098 void sample() // Function which collects and filters the EMG inputs at 500Hz 00099 { 00100 u = (emg.read() - 0.5f)*2; // Detrended signal right EMG 00101 u2=(emg2.read()-0.5f)*2; // Detrended signal left EMG 00102 y=computeBiquad(u,v1,v2); // HighPassfiltered right EMG signal 00103 y2=computeBiquad2(u2,va1,va2); // HighPassfiltered left EMG signal 00104 yNF = computeBiquadNF(y,vNF1,vNF2,vNF3,vNF4,vNF5,vNF6); // Notch filter 50 Hz over right EMG signal 00105 yNF2=computeBiquadNF2(y2,vaNF1,vaNF2,vaNF3,vaNF4,vaNF5,vaNF6); // Notch filter 50 Hz over left EMG signal 00106 buffTot=MovingAverage1(yNF); // Calculates the moving average of the right filtered right EMG signal 00107 buffaTot=MovingAverage2(yNF2); // Calculates the moving average of the right filtered left EMG signal 00108 if(buffTot>0.4f&&spierrustrechts.read()<=0.000001f) { // Checking if there is a pulse in the right arm 00109 spierrustrechts.start(); 00110 SignalRight=true; 00111 } 00112 if (spierrustrechts.read()>=0.7f) { // Preventing that from a single pulse is being stored once not used and that a single push doens't create multiple pulses 00113 spierrustrechts.stop(); 00114 spierrustrechts.reset(); 00115 SignalRight=false; 00116 } 00117 if(buffaTot>0.4f&&spierrustlinks.read()<=0.000001f) { // Checking if there is a pulse in the left arm 00118 spierrustlinks.start(); 00119 SignalLeft=true; 00120 } 00121 if (spierrustlinks.read()>=0.7f) { // Preventing that from a single pulse is being stored once not used and that a single push doens't create multiple pulses 00122 spierrustlinks.stop(); 00123 spierrustlinks.reset(); 00124 SignalLeft=false; 00125 } 00126 } 00127 00128 void PakOp() // Function which makes the servo turn and changes the state of the electromagnet 00129 { 00130 PowerServo.write(0.1); 00131 wait(1); 00132 PowerServo.write(0.25); // Servo moves to -90 degree 00133 wait(0.5); 00134 magneet=!magneet; // Electromagnet changes state (on/off) 00135 wait(3); 00136 PowerServo.write(0.1); 00137 wait(0.5); 00138 PowerServo.write(0.035); // Servo moves to 90 degrees 00139 wait(1); 00140 NumberTimesPakOp=NumberTimesPakOp+1; // Number to know if there is a even number of state changes (Pick up/Put down) 00141 } 00142 00143 void MotorSet() 00144 { 00145 if (Excecute||Excecute3) { // Situation that the PID controller should be on 00146 Errord = (Error-Errorp)/Fs; // Calculating the D part of the controller first motor 00147 Errorp = Error; // Setting the previous Error of the first motor 00148 Errord2 = (Error2-Errorp2)/Fs; // Calculating the D part of the controller second motor 00149 Errorp2 = Error2; // Setting the previous Error of the second motor 00150 if (fabs(Error) <= 0.5) { // Once the I part of the controller should start working 00151 Errori = Errori + Error*1/Fs; // Calculating the I part of the controller 00152 } else { 00153 Errori = 0; // If the I part shouldn't be active this Error is set to 0 00154 } 00155 if (fabs(Error2)<=0.5) { // Same but other direction 00156 Errori2=Errori2+Error2/Fs; 00157 } else { 00158 Errori2=0; 00159 } 00160 speed=Kp*Error + Kd*Errord + Ki*Errori; // Calculating the output of the controller with the constants for each error 00161 speed2=Kp*Error2+Kd*Errord2+Ki*Errori2; // Same but other motor 00162 if (Error>=0) { // Chosing the direction of the first motor 00163 Direction=1; 00164 } else { 00165 Direction=0; 00166 } 00167 if (Error2>=0) { // Chosing the direction of the second motor 00168 Direction2=1; 00169 } else { 00170 Direction2=0; 00171 } 00172 } 00173 //if (CheckRightEMG||CheckLeftEMG) { // Safeguard that the robot can't leave the rail with the first motor 00174 // Safety.start(); 00175 // if(Safety.read()>=0.1) { 00176 // if (ValueStopSystemA>=0.8||Rotatie<=-18) { 00177 // Excecute=true; 00178 // CheckRightEMG=false; 00179 // CheckLeftEMG=false; 00180 // NumberTimesPressed=1; 00181 // speed=0; 00182 // } 00183 // if (ValueStopSystemB>=0.8||Rotatie2<=-23.33) { 00184 // Excecute3=true; 00185 // CheckRightEMG=false; 00186 // CheckLeftEMG=false; 00187 // NumberTimesPressed2=1; 00188 // speed2=0; 00189 // } 00190 // } 00191 // } 00192 // if (Home) { 00193 // Safety.stop(); 00194 // Safety.reset(); 00195 // } 00196 PowerMotor.write(fabs(speed)); // Writing the speed to the motors 00197 PowerMotor2.write(fabs(speed2)); 00198 } 00199 void Measurements() // Measurements 00200 { 00201 ValueStopSystemA=StopSystemA.read(); 00202 ValueStopSystemB=StopSystemB.read(); 00203 Pulses = EncoderMotorLR.getPulses(); // Pulses of the encoder first motor in X4 00204 Rotatie = (Pulses*twopi)/8400; // Angle of the first motor 00205 Pulses2=EncoderMotorFB.getPulses(); // Pulses of the encoder second motor in X4 00206 Rotatie2=(Pulses2*twopi)/8400; // Angle of the second motor 00207 } 00208 int main() 00209 { 00210 LedR.write(0),LedB.write(0),LedG.write(0); // Making sure all the feedback lights are off 00211 PowerServo.period_ms(20); // Setting the PWM signal to the correct period for the servo motor 00212 magneet.write(0); // Making sure the Magnet is off in the beginning 00213 led.write(1); // Making sure the LED is off in the beginning 00214 PowerMotor.write(0); // Making sure the first motor is off 00215 PowerMotor2.write(0); // Making sure the second motor is off 00216 Sender.attach(Measurements,0.5/Fs); // Calling the Measurements function in fixed intervals at twice the frequency as the motors get writen 00217 MotorWrite.attach(MotorSet,1/Fs); // Writing data to the motors at a fixed interval 00218 sample_timer.attach(&sample, 0.002); // Looking for EMG inputs at a fixed interval 00219 wait(1); // Wait a moment to skip the first peak of the EMG signal due to opening of the analog ports 00220 while (true) { 00221 LedR.write(0),LedB.write(0),LedG.write(1); // Turning on the feedback light that the client can control the device 00222 // move input 00223 if (SignalRight && Rotatie*11*Factor<=-margin) { // Checking and locking the correct arm side for the movement of the robot 00224 LedR.write(1),LedB.write(0),LedG.write(1); // Turning on the feedback light that the robot should be moving 00225 CheckRightEMG=true; 00226 NumberTimesPressed=NumberTimesPressed+1; 00227 SignalRight=false; 00228 wait(0.5); 00229 speed=speed_driving; 00230 Direction=Right; 00231 } 00232 while (CheckRightEMG) { // Keeping the right arm signal as only nessecary signal 00233 if(SignalRight) { 00234 NumberTimesPressed=NumberTimesPressed+1; 00235 SignalRight=false; 00236 speed=speed_driving; 00237 Direction=Right; 00238 00239 } 00240 if (NumberTimesPressed%3==0) { // If moved and stopped then continue to next step, remove lock on a single signal 00241 Excecute=true; 00242 CheckRightEMG=false; 00243 speed=0; 00244 NumberTimesPressed=NumberTimesPressed+1; 00245 } 00246 } 00247 if(SignalLeft) { // Checking left arm signal and locking on only the left arm signal if this is activated once 00248 LedR.write(1),LedB.write(0),LedG.write(1); // Turning on the feedback light that the robot should be moving 00249 CheckLeftEMG=true; 00250 NumberTimesPressed2=NumberTimesPressed2+1; 00251 SignalLeft=false; 00252 wait(0.5); 00253 speed=speed_driving; 00254 Direction=Left; 00255 } 00256 while (CheckLeftEMG) { // Locks on left arm signal 00257 if(SignalLeft && Rotatie*11*Factor>=-upperlimitLR+margin) { 00258 NumberTimesPressed2=NumberTimesPressed2+1; 00259 SignalLeft=false; 00260 speed=speed_driving; 00261 Direction=Left; 00262 } 00263 if(NumberTimesPressed2%3==0) { // If moved and stopped then continue to next stop, remove lock on a single signal 00264 Excecute=true; 00265 CheckLeftEMG=false; 00266 speed=0; 00267 NumberTimesPressed2=NumberTimesPressed2+1; 00268 } 00269 } 00270 if (Excecute) { // Determine the location of the middle of selected field 00271 LedR.write(1),LedB.write(0),LedG.write(0); // Turning on the feedback lights that the robot is controlling itself 00272 Vakjex=Rotatie/1.7964285714285714285714285714286; //mapping constant 00273 VakjexNatural=Vakjex; 00274 if (Vakjex-VakjexNatural>=0.5f) { 00275 Vakjexround=Vakjex+1; 00276 } else if(Vakjex-VakjexNatural<=-0.5f) { 00277 Vakjexround=Vakjex-1; 00278 } else if(Vakjex-VakjexNatural>-0.5f&&Vakjex-VakjexNatural<0.5f) { 00279 Vakjexround=Vakjex; 00280 } 00281 Goal=Vakjexround*1.7964285714285714285714285714286; 00282 } 00283 while (Excecute ) { // Moving to determined location 00284 Error = Goal-Rotatie; 00285 if (fabs(Error)<=0.05) { 00286 timer.start(); 00287 } else { 00288 timer.stop(); 00289 timer.reset(); 00290 } 00291 if (timer.read() >= 1) { 00292 Excecute=false; 00293 Excecute2 = true; 00294 Error = 0; 00295 Errori = 0; 00296 Errord = 0; 00297 speed=0; 00298 timer.stop(); 00299 timer.reset(); 00300 } 00301 } 00302 // move y direction 00303 while (Excecute2) { 00304 LedR.write(0),LedB.write(0),LedG.write(1); // Turning on the feedback light that the client can control the device 00305 led.write(0); 00306 if(SignalRight) { 00307 LedR.write(1),LedB.write(0),LedG.write(1); // Turning on the feedback light that the robot should be moving 00308 CheckRightEMG=true; 00309 NumberTimesPressed=NumberTimesPressed+1; 00310 SignalRight=false; 00311 wait(0.5); 00312 speed2=speed_driving2; 00313 Direction2=Left; 00314 } 00315 while (CheckRightEMG) { 00316 if(SignalRight) { 00317 NumberTimesPressed=NumberTimesPressed+1; 00318 SignalRight=false; 00319 speed2=speed_driving2; 00320 Direction2=Left; 00321 } 00322 if (NumberTimesPressed%3==0) { 00323 Excecute3=true; 00324 Excecute2=false; 00325 CheckRightEMG=false; 00326 led.write(1); 00327 speed2=0; 00328 NumberTimesPressed=NumberTimesPressed+1; 00329 } 00330 } 00331 if(SignalLeft) { 00332 LedR.write(1),LedB.write(0),LedG.write(1); // Turning on the feedback light that the robot should be moving 00333 CheckLeftEMG=true; 00334 NumberTimesPressed2=NumberTimesPressed2+1; 00335 SignalLeft=false; 00336 wait(0.5); 00337 speed2=speed_driving2; 00338 Direction2=Right; 00339 } 00340 while (CheckLeftEMG) { 00341 if(SignalLeft) { 00342 NumberTimesPressed2=NumberTimesPressed2+1; 00343 SignalLeft=false; 00344 speed2=speed_driving2; 00345 Direction2=Right; 00346 } 00347 if(NumberTimesPressed2%3==0) { 00348 Excecute3=true; 00349 Excecute2=false; 00350 CheckLeftEMG=false; 00351 led.write(1); 00352 speed2=0; 00353 NumberTimesPressed2=NumberTimesPressed2+1; 00354 } 00355 } 00356 // move to the middle of the field 00357 if (Excecute3) { 00358 LedR.write(1),LedB.write(0),LedG.write(0); // Turning on the feedback lights that the robot is controlling itself 00359 Vakjey=Rotatie2/3.3333333; // mapping constant 00360 VakjeyNatural=Vakjey; 00361 if (Vakjey-VakjeyNatural>=0.5f) { 00362 Vakjeyround=Vakjey+1; 00363 } else if(Vakjey-VakjeyNatural<=-0.5f) { 00364 Vakjeyround=Vakjey-1; 00365 } else { 00366 Vakjeyround=Vakjey; 00367 } 00368 Goal2=Vakjeyround*3.3333333; 00369 } 00370 00371 } 00372 while (Excecute3 ) { 00373 Error2 = Goal2-Rotatie2; 00374 if (fabs(Error2)<=0.05) { 00375 timer.start(); 00376 } else { 00377 timer.stop(); 00378 timer.reset(); 00379 } 00380 if (timer.read() >= 1) { 00381 Excecute3=false; 00382 Error2 = 0; 00383 Errori2 = 0; 00384 Errord2 = 0; 00385 speed2=0; 00386 timer.stop(); 00387 timer.reset(); 00388 PakOp(); 00389 } 00390 } 00391 if (NumberTimesPakOp%3==0) { 00392 Home=true; 00393 } 00394 while (Home) { 00395 LedR.write(1),LedB.write(0),LedG.write(0); // Turning on the feedback lights that the robot is controlling itself 00396 if (!Correct1) { 00397 if (ValueStopSystemA<0.5) { 00398 Direction=Right; 00399 speed=Speed_driving_back; 00400 } 00401 if (ValueStopSystemA>=0.8) { 00402 speed=0; 00403 Correct1=true; 00404 EncoderMotorLR.reset(); 00405 } 00406 } 00407 if(!Correct2) { 00408 if (ValueStopSystemB<0.5) { 00409 Direction2=Right; 00410 speed2=Speed_driving_back2; 00411 } 00412 if (ValueStopSystemB>=0.8) { 00413 speed2=0; 00414 Correct2=true; 00415 EncoderMotorFB.reset(); 00416 } 00417 } 00418 if (Correct1&&Correct2) { 00419 Home=false; 00420 Correct1=false; 00421 Correct2=false; 00422 NumberTimesPakOp=NumberTimesPakOp+1; 00423 } 00424 } 00425 } 00426 }
Generated on Tue Aug 9 2022 03:51:43 by
1.7.2
