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

--- 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;