Diff: main.cpp

Revision:

8:3d2228402c71

Parent:

7:fb3da4df4269

Child:

9:7443c37f0f7b

--- a/main.cpp	Tue Nov 06 16:12:17 2018 +0000
+++ b/main.cpp	Tue Nov 06 20:25:40 2018 +0000
@@ -8,7 +8,7 @@
 DigitalOut motor1Direction(D7);
 DigitalOut motor2Direction(D4);
 DigitalOut Led(LED_GREEN);
-DigitalIn button2(SW3); //dit is nog button op mbed bor
+DigitalIn button2(SW3);
 FastPWM motor1PWM(D6);
 FastPWM motor2PWM(D5);
 AnalogIn potMeter1(A4);
@@ -18,25 +18,23 @@
 
 // Tickers
 Ticker potmeterTicker;
-Ticker allesineenticker;
+Ticker measurecontrolTicker;
 Ticker printTicker;
 //------------------------------------------------------------------------------
 //Global Variables
 
-float currentPosition1 = 0.1;          // Starting position of motor 1 [rad]
-float currentPosition2 = -(0.5*3.14);  // Starting position of motor 2 wrt motor 1 [rad]
-// ^ waarom?
-int a = 0;
+volatile float currentPosition1 = 0.1;          // Starting position of motor 1 [rad]
+volatile float currentPosition2 = -(0.5*3.14);  // Starting position of motor 2 wrt motor 1 [rad]
 
 //Inverse Kinematics
-const float L0 = 0.1;              // Horizontal length from base to first joint [m]   NAMETEN!!
-const float L1 = 0.3;              // Length link 1 [m]                                NAMETEN!!
-const float L2 = 0.3;              // Length link 2 [m]                                NAMETEN!!
-const float L3 = 0.1;              // Vertical length from base to first joint [m]     NAMETEN!
+const float L0 = 0.1;              // Horizontal length from base to first joint [m]  
+const float L1 = 0.3;              // Length link 1 [m]                       
+const float L2 = 0.3;              // Length link 2 [m]                               
+const float L3 = 0.1;              // Vertical length from base to first joint [m]   
 volatile float q1 = 0.1;           // Starting reference angle of first link [rad]
 volatile float q2 = -(0.5*3.12);   // Starting reference angle of second link wrt first link [rad]
-// ^ start buiten bereik, waarom?
-float t = 0.01;                     // Time interval [s]                                SHOULD BE REPLACED          SAMPLING TIJD!!
+
+float t = 0.02;                     // Time interval [s]
 volatile float vx_des = 0.0;       // Starting velocity in x-direction [rad/s]
 volatile float vy_des = 0.0;       // Starting velocity in y-direction [rad/s]
 volatile float errorIK1 = 0.0;
@@ -57,10 +55,10 @@
 //volatile float potMeterPosition2 = 0.0;
 volatile float motorValue1 = 0.01;
 volatile float motorValue2 = 0.01;
-volatile float Kp = 4.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
-volatile float Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
+volatile float Kp = 0.34;
+volatile float Ki = 0.0;
 volatile float Kd = 0.0;
-volatile float Ts = 0.01;                      //                                                          SAMPLING TIJD!!
+volatile float Ts = 0.02;                    
 
 
 //------------------------------------------------------------------------------
@@ -68,9 +66,7 @@
 void GetPotMeterVelocity1()  //Potmeter standard to control X-DIRECTION
 {
     float potMeter1In = potMeter1.read();
-    vx_des = 4.0*3.14*potMeter1In - 2.0*3.14; // Reference value y, scaled to -0.25 to 0.25 revolutions
-    //vx_des = 0.5;
-    //return vx_des;
+    vx_des = 4.0*3.14*potMeter1In - 2.0*3.14;
 }
 
 float SwitchPotmeterVelocity1()    //Button has been pressed,
@@ -127,6 +123,13 @@
 }
 //------------------------------------------------------------------------------
 // MOTOR PART
+// Failure function
+void TurnMotorsOff()                                   // Turn motors off immediately
+{
+    motor1PWM = 0;
+    motor2PWM = 0;
+}
+
 //Encoder Posities
 void MeasureEncoderPosition1()        // Read current position of motor 1, returns value in [rad]
 {
@@ -216,28 +219,7 @@
     }
 }
 
-/*void MeasureAndControl1(void)
-{
-    // This function determines the desired velocity, measures the
-    // actual velocity, and controls the motor with
-    // a simple Feedback controller. Call this from a Ticker.
-    MeterPosition1 = q1;
-    currentPosition1 = MeasureEncoderPosition1();
-    motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1);
-    SetMotor1(motorValue1);
-}
-
-void MeasureAndControl2(void)
-{
-    // This function determines the desired velocity, measures the
-    // actual velocity, and controls the motor with
-    // a simple Feedback controller. Call this from a Ticker.
-    potMeterPosition2 = q2;
-    currentPosition2 = MeasureEncoderPosition2();
-    motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2);
-    SetMotor2(motorValue2);
-}*/
-void AllesinEen() //HIER GAAT IETS MIS!!
+void MeasureAndInverseK() 
 {
     MeasureEncoderPosition1();   // You want to know the current angle of the motors to get the right Jacobian
     MeasureEncoderPosition2();   // You want to know the current angle of the motors to get the right Jacobian
@@ -246,34 +228,35 @@
     ComputeQ_set();                         // Compute Q_set (stores Q1 and Q2)
     q1 = IntegrateQ1();                                  // Compute required angle to go to desired position of end-effector
     q2 = IntegrateQ2();                                  // Compute required angle to go to desired position of end-effector
-    if (q > 0.0) {              // q1 cannot be greater than 0
-        q = 0.0;
-    } else if (q < - 1.047) {   // q1 cannot be smaller than 60 degrees
-        q1 = -1.047;
-    } else {                    // q1 can be computed q1 between intervals
-        q1 = q1;
-    }
-    if (q2 > -0.61) {                                // q2 cannot be smaller than 0.61 rad
-        q2 = -0.61;                                  // Stay at mimimum angle
-    } else if (q2 < -1.57) {                         // q2 cannot be greater than 1.57 rad
-        q2 = -1.57;                                  // Stay at maximum angle
-    } else {                                        // If q2 is in the right range, let calculated q2 be q2
-        q2 = q2;
-    }
+    if (q1 < 1.047) {                               // q1 can only be smaller than 1.047 rad
+                q1 = q1;
+            } else {                                // If value of q1 is greater than 1.047 rad, then stay at maximum angle
+                q1 = 1.047;
+            }
+            if (q2 < 0.61) {                        // q2 cannot be smaller than 0.61 rad
+                q2 = 0.61;                          // Stay at mimimum angle
+            } else if (q2 > 1.57) {                 // q2 cannot be greater than 1.57 rad
+                q2 = 1.57;                          // Stay at maximum angle
+            } else {                                // If q2 is in the right range, let calculated q2 be q2
+                q2 = q2;
+            }
+
 // Determine error and add PID control
     errorIK1 = ErrorInverseKinematics1();       // Determine difference between current angle motor 1 and desired angle
     errorIK2 = ErrorInverseKinematics2();        // Determine difference between current angle motor 2 and desired angle
 // Determine motorValues
     motorValue1 = FeedbackControl1(errorIK1);                       // Calculate motorValue1
     motorValue2 = FeedbackControl2(errorIK2);                       // Calculate motorValue2
+    if (errorIK1<0.5);
+            {
+                TurnMotorsOff();
+            }
 // Make Motor move
     SetMotor1(motorValue1);
     SetMotor2(motorValue2);
 
 // Press button to switch velocity direction
-//if (!button2.read()) {
-    if (!button2.read()) { //button2 blijven indrukken
-        //SwitchPotmeterVelocity1();
+    if (!button2.read()) { //keep pressing button2
         vy_des=vx_des;
         vx_des=0.0;
     } else {
@@ -281,25 +264,24 @@
     }
 }
 
-void printen()
+/*void PrintValues()
 {
     pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des);
     pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2);
     pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2);
     pc.printf("q1  Qset = %f \t q2 Qset = %f \r\n", Q1, Q2);
-}
+}*/
 //------------------------------------------------------------------------------
 // MAIN
 int main()
 {
     pc.baud(115200);
-    potmeterTicker.attach(GetPotMeterVelocity1, 0.01);
-    allesineenticker.attach(AllesinEen, 0.01);
-    printTicker.attach(printen, 0.5);
+    potmeterTicker.attach(GetPotMeterVelocity1, 0.02);
+    measurecontrolTicker.attach(MeasureAndInverseK, 0.01);
+    /*printTicker.attach(printen, 0.5);*/
 
     while (true) {
         Led = !Led;
         wait(0.5);
-        //wait(0.01);
     }
 }
\ No newline at end of file