Delen met projectgroep, Geen eindversie met comments

Dependencies:   QEI mbed

Fork of Project_Program_bijna_af by Yorick Fredrix

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }