Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Revision 40:cac08f589732, committed 2015-10-29
- Comitter:
- Fernon
- Date:
- Thu Oct 29 15:15:45 2015 +0000
- Parent:
- 39:e5bf4b1293fa
- Commit message:
- Works well
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 29 09:45:12 2015 +0000 +++ b/main.cpp Thu Oct 29 15:15:45 2015 +0000 @@ -4,18 +4,18 @@ // Motor 1 & 2 -DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down -PwmOut PowerMotor(D5); //van 0 tot 1 -QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING); //Encoder +DigitalOut Direction(D4); +PwmOut PowerMotor(D5); +QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING); DigitalOut Direction2(D7); PwmOut PowerMotor2(D6); QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING); PwmOut PowerServo(D3); // Buttons & EMG -AnalogIn Button(A3); // Calibration Button +AnalogIn Button(A3); // Calibration button AnalogIn Button2(A4); // Fire button -DigitalIn Button3(D9); +DigitalIn Button3(D9); // Home button AnalogIn EMG(A0); AnalogIn EMG2(A1); @@ -32,37 +32,31 @@ -// Waardes +// Values const double twopi = 6.2831853071795; -const double Fs=100; +const double Fs=100; // Sample Frequency int Fired = 0; -// EMG -double T1 = 0; -double T2 = 0; -double T3 = 0; -double T4 = 0; - -// Motor 1 (Translatie) -const double n1 = 3.3; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog) +// Motor 1 (Translation) +const double n1 = 3.3; // The amount of the rotations that is allowed int Pulses; -double Rotatie = 0; //aantal graden dat de motor is gedraaid -double Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan -double Error = 0; +double Rotatie = 0; //Rotation in radial +double Goal; // Setpoint for Motor +double Error = 0; // Setting initial values double Errord = 0; double Errori = 0; double Errorp = 0; -const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides +const double Kp = 0.2; // PID controll parameters const double Kd = 10; const double Ki = 0.2; double v = 0; -double upperlimit; //max aantal rotaties omhoog -const double downlimit = 0.8; -const double margin = 0.8; -bool OutRange = false; +double upperlimit; //Maximum rounds up +const double downlimit = 0.8; //Lowest limit of the robot +const double margin = 0.8; //Margin for safe range +bool OutRange = false; //In Range or not -// Motor 2 (Rotatie) -const double n2 = 0.4; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie) +// Motor 2 (Rotation) +const double n2 = 0.334; int Pulses2; double Rotatie2 = 0; double Goal2; @@ -74,22 +68,26 @@ const double Kd2 = 10; const double Ki2 = 0.2; double v2 = 0; -double turnlimit = 0.4; // max aantal rotaties voor roteren hele robot -// Margin 2 is in ons geval 0 +double turnlimit = 0.4; +// Margin 2 = 0 bool OutRange2 = false; -// Activatie tussen het schietgedeelte en terugkeergedeelte +// Switch between firing mode (Excecute) and return to Home mode bool Excecute = false; bool Home = false; // Filter -double Fs2 = 500; // in Hz -const double TijdCal = 5; -double Max = 0; -double Max2 = 0; -bool Cali = false; +double Fs2 = 500; // Frequency in Hz +const double TijdCal = 5; // Time for calibration +double Max = 0; // Max value for EMG +double Max2 = 0; // Max value for EMG2 +bool Cali = false; // Switch for Calibration +double T1 = 0; // Thresholds for EMG +double T2 = 0; +double T3 = 0; +double T4 = 0; -double u1; +double u1; //All variables for filtering double y1; double y2; double y3; @@ -149,7 +147,7 @@ -// Voids voor berekeningen in het hoofdprogramma +// Voids for calculations in the main programme double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) { @@ -207,27 +205,28 @@ void MotorSet() { + // Calculation for position of motor Pulses = Encoder.getPulses(); Rotatie = (Pulses*twopi)/4200; Pulses2 = Encoder2.getPulses(); Rotatie2 = (Pulses2*twopi)/4200; - // Eerst motor 1 (translatie) - if (OutRange) { - Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal + // Motor 1 (translation) + if (OutRange) { //If the Robot is OutRange or is going Home + Error = Goal-Rotatie; Errord = (Error-Errorp)/Fs; Errorp = Error; - if (fabs(Error) <= 0.5) { + if (fabs(Error) <= 0.5) { // Only activate the I part of PID when the Error is small Errori = Errori + Error*1/Fs; } else { Errori = 0; } - if (Error>=0) { + if (Error>=0) { // Determine the direction Direction=1; } else { Direction=0; } - v=Kp*Error + Kd*Errord + Ki*Errori; - if (Home == true) { + v=Kp*Error + Kd*Errord + Ki*Errori; //Calculation of speed + if (Home == true) { //Maximum speed when OutRange if (v >0.15) { v = 0.15; } @@ -237,7 +236,7 @@ // Dan motor 2 (rotatie) if (OutRange2) { - Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal + Error2 = Goal2-Rotatie2; Errord2 = (Error2-Errorp2)/Fs; Errorp2 = Error2; if (fabs(Error2) <= 0.5) { @@ -257,10 +256,10 @@ void Calibration () { - if (OutRange == false && OutRange2 == false) { + if (OutRange == false && OutRange2 == false) { //Only allow callibration during firing mode and when in Range if (Button >= 0.8) { wait (0.2); - Cali = true; + Cali = true; // Calibration part in the EMG void is being activated TijdEMGCal.start(); Excecute = false; ledC = 0; @@ -269,15 +268,14 @@ } } if (TijdEMGCal.read() >= TijdCal) { - Cali = false; + Cali = false; // Calibration part in the EMG void is stopped ledC = 1; TijdEMGCal.stop(); TijdEMGCal.reset(); - T1 = 0.35*Max; + T1 = 0.35*Max; // Determination of thresholds T2 = 0.5*Max; T3 = 0.35*Max2; T4 = 0.5*Max2; - //pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4); wait (3); Excecute = true; } @@ -285,31 +283,31 @@ int main() { - upperlimit = n1*twopi; + upperlimit = n1*twopi; // Calculation for limits of the robot turnlimit = n2*twopi; ledF = 1; ledC = 1; PowerMotor.write(0); PowerMotor2.write(0); - MotorWrite.attach(MotorSet,1/Fs); + MotorWrite.attach(MotorSet,1/Fs); // Activate the tickers biquadTicker.attach(myController,1/Fs2); PowerServo.period_ms(20); while (true) { - Calibration(); + Calibration(); // Makes it able to calibrate during firing mode while (Excecute) { - // Eerst wordt motor 1 aangestuurd - if (Rotatie >= upperlimit) { //Als hij buiten bereik is + // Motor 1 + if (Rotatie >= upperlimit) { // If OutRange Goal = upperlimit - margin-0.2; OutRange = true; } - if (Rotatie <= downlimit) { //Als hij buiten bereik is + if (Rotatie <= downlimit) { // If OutRange Goal = 0 + margin +0.2; OutRange = true; } - if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is + if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // If in range / not OutRange OutRange = false; } - if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing + if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing when in range / not OutRange if (y8 >= T2 ) { Direction = 1; v = 0.13; @@ -324,41 +322,45 @@ } } - // Vanaf hier wordt motor 2 aangestuurd - if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is + // Motor + if (Rotatie2 >= turnlimit) { // If OutRange Goal2 = turnlimit-0.1; OutRange2 = true; } - if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is + if (Rotatie2 <= -turnlimit) { // If OutRange Goal2 = -turnlimit+0.1; OutRange2 = true; } - if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is + if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // If in range / not OutRange OutRange2 = false; } - if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing + if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { //EMG aansturing when in range / not OutRange if (y18 >= T4 ) { Direction2 = 1; - v2 = 0.07; + v2 = 0.06; } if (y18 > T3 && y18 < T4) { Direction2 = 0; - v2 = 0.07; + v2 = 0.06; } if (y18 <= T3) { Direction2 = 0; v2 = 0; } } - if (Button2 >= 0.8) { //Afvuren van de RBG + if (Button2 >= 0.8) { // Firing Rubber Band Gun wait(0.2); ledF = 0; PowerServo.write(0.27); wait (1); PowerServo.write(0.04); + wait (1); + PowerServo.write(0.27); + wait (1); + PowerServo.write(0.04); ledF = 1; Fired=Fired+1; - if (Fired == 4) { + if (Fired == 3) { wait (1); Home = true; Excecute = false; @@ -371,18 +373,18 @@ Calibration(); } - while (Home) { //Terugkeren naar vaste positie - OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd. + while (Home) { // Return to base position + OutRange = true; // Activation of PID control part OutRange2 = true; Goal = margin; Goal2 = 0; - if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { + if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { // If error is small enough, then firing mode will be enabled again timer.start(); } else { timer.stop(); timer.reset(); } - if (timer.read() >= 3) { + if (timer.read() >= 3) { // Firing mode is being enabled again and values are reset. Errori = 0; Errord = 0; Errorp = 0;