Diff: main.cpp

Revision:

49:0ada5a721686

Parent:

48:1eb0f9ed6cd9

Child:

51:78c75cc72d17

--- a/main.cpp	Thu Nov 01 18:46:33 2018 +0000
+++ b/main.cpp	Thu Nov 01 20:13:44 2018 +0000
@@ -410,7 +410,7 @@
     }
 
 // ------------------------- MOTOR FUNCTIONS FOR MOTOR LEFT --------------------
-    float PIDControllerL(float angleReference,float angleCurrent)  // PID controller for the motors, based on the reference value and the current angle of the motor
+float PIDControllerL(float angleReference,float angleCurrent)  // PID controller for the motors, based on the reference value and the current angle of the motor
     {   float   Kp = 19.24f; 
         float   Ki = 1.02f;
         float   Kd = 0.827f;
@@ -433,8 +433,8 @@
     }
 
     float angleDesiredL()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-    {   float   angle = (pot1*2.0f*PI)-PI;
-        return  angle;
+    {   float   angleL = (pot1*2.0f*PI)-PI;
+        return  angleL;
     }
     
     int countsInputCalibratedL()      // Gets the counts from encoder 1
@@ -475,13 +475,80 @@
         countsL = countsL - countsOffsetL + countsCalibration; 
         //countsL = 0; 
         pin6 = 0.0f; 
-        // BUTTON PRESS: NAAR VOLGENDE STATE 
+        // BUTTON PRESS: TO NEXT STATE 
+    }         
+}    
+    
+// ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
+    float PIDControllerR(float angleReference,float angleCurrent)  // PID controller for the motors, based on the reference value and the current angle of the motor
+    {   float   Kp = 19.24f; 
+        float   Ki = 1.02f;
+        float   Kd = 0.827f;
+        float   error = errorCalc(angleReference,angleCurrent);
+        static float    errorIntegralR = 0.0;
+        static float    errorPreviousR = error; // initialization with this value only done once!
+        static  BiQuad  PIDFilterR(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+        // Proportional part:
+        float   u_k = Kp * error;
+        // Integral part
+        errorIntegralR = errorIntegralR + error * dt;
+        float   u_i = Ki * errorIntegralR;
+        // Derivative part
+        float   errorDerivative = (error - errorPreviousR)/dt;
+        float   errorDerivativeFiltered = PIDFilterR.step(errorDerivative);
+        float   u_d = Kd * errorDerivativeFiltered;
+        errorPreviousR = error;
+        // Sum all parts and return it
+        return  u_k + u_i + u_d;
+    }
+
+    float angleDesiredR()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+    {   float   angleR = (pot2*2.0f*PI)-PI;
+        return  angleR;
+    }
+    
+    int countsInputCalibratedR()      // Gets the counts from encoder 1
+    {   countsR = encoderR.getPulses()- countsOffsetR + countsCalibration;
+        return  countsR;
+    }  
+            
+void    motorTurnR()    // main function for movement of motor 1, all above functions with an extra tab are called
+{   
+    float   angleReferenceR = angleDesiredR();                              // insert kinematics output here instead of angleDesiredR()
+    angleReferenceR = -angleReferenceR;                                     // different minus sign per motor
+    countsR = countsInputCalibratedR();                                       // different encoder pins per motor
+    angleCurrentR = angleCurrent(countsR);                                  // different minus sign per motor
+    errorR = errorCalc(angleReferenceR,angleCurrentR);                      // same for every motor
+    
+    float   PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR);    // same for every motor
+    pin5 = fabs(PIDControlR);                                               // different pins for every motor
+    pin4 = PIDControlR > 0.0f;                                              // different pins for every motor
+}
+
+void calibrationR()        // Partially the same as motorTurnR, only with potmeter input
+// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
+// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
+// Do this for every motor and after calibrated all motors, press a button
+{   float   angleReferenceR = angleDesiredR();                              // insert kinematics output here instead of angleDesiredR()
+    angleReferenceR = -angleReferenceR;                                     // different minus sign per motor
+    countsR = countsInputR();                                       // different encoder pins per motor
+    angleCurrentR = angleCurrent(countsR);                                  // different minus sign per motor
+    errorR = errorCalc(angleReferenceR,angleCurrentR);                      // same for every motor
+    
+    if (fabs(errorR) >= 0.01f)
+    {   float   PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR);    // same for every motor
+        pin5 = fabs(PIDControlR);                                               // different pins for every motor
+        pin4 = PIDControlR > 0.0f;                                              // different pins for every motor
+    }
+    else if (fabs(errorR) < 0.01f)
+    {   countsOffsetR = countsR;
+        countsR = countsR - countsOffsetR + countsCalibration; 
+        //countsR = 0; 
+        pin5 = 0.0f; 
+        // BUTTON PRESS: TO NEXT STATE 
     }         
 }
 
-// ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
-
-
 // ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
 
 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
@@ -507,7 +574,7 @@
                     // Actions for each loop iteration
                     /* */
                     calibrationL();
-                    //calibrationR();
+                    calibrationR();
 //      --------------------------- transition ----------------------------
                     // Transition condition: when all motor errors smaller than 0.01,
                     //      start calibrating EMG
@@ -696,7 +763,7 @@
                     ReadUseEMG1();                  //Start the use of EMG
                     kinematics();
                     motorTurnL();
-                    //motorTurnR();
+                    motorTurnR();
 
 //      --------------------------- transition ----------------------------
                     // Transition condition: with button press, back to homing mode