Revision 1:d57a502fb30f, committed 2017-10-31

Comitter:

Shivan_1997

Date:

Tue Oct 31 19:00:47 2017 +0000

Parent:

0:077896c03576

Commit message:

Code with changes in kinematic model angles

Changed in this revision

--- a/main.cpp	Tue Oct 31 12:52:29 2017 +0000
+++ b/main.cpp	Tue Oct 31 19:00:47 2017 +0000
@@ -28,24 +28,24 @@
 bool         direction1 = 1; // direction positive, 0 is negative
 bool         direction2 = 1;
 const double RAD_PER_PULSE = 0.002991; // Value for RAD_PER_PULSE given through the slides (wrong?)
-const double DEG_PER_RAD = 180 / M_Pi; // Basic knowledge of how many degrees are in 1 radian.
+const double DEG_PER_RAD = 180 / M_Pi; 
 
 double       q1 = 0;                // Angle of arm 1 (upper) in starting position is 0 degrees
 double       q2 = 179/DEG_PER_RAD;  // Angle of arm 2 (lower) in starting position is 180 degrees (but can't be 0 or 180 because of determinant = 0)
-int          L1 = 47;               // Length of arm 1 (upper) in cm
-int          L2 = 29;               // Length of arm 2 (lower) in cm
+int          L2 = 47;               // Length of arm 1 (upper) in cm
+int          L1 = 29;               // Length of arm 2 (lower) in cm
 double       xdes = L1-L2;           // Desired x coordinate, arm is located at x = L1-L2 in starting position
 double       ydes = 0;              // Disired y coordinate, arm is located at y = 0 in starting position
-double       MotorValue1 = 0;
+double       MotorValue1 = 0;       
 double       MotorValue2 = 0;
 
 // Sample time (motor1-timestep)
-const double M1_Ts = 0.01; //100 Hz systems
+const double M1_Ts = 0.01; 
 const double M2_Ts = 0.01;
 
 // Controller gains (motor1-Kp,-Ki,...)
-const double M1_Kp = 0.1, M1_Ki = 0.02, M1_Kd = 0.00125,  M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT
-const double M2_Kp = 0.1, M2_Ki = 0.02, M2_Kd = 0.00125,  M2_N = 100; // Inspired by the Ziegler-Nichols Method
+const double M1_Kp = 0.2, M1_Ki = 40, M1_Kd = 0.000666666,  M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT
+const double M1_Kp = 0.2, M1_Ki = 40, M1_Kd = 0.000666666,  M1_N = 100; // I was hoger na opnieuw uitrekenen met ZN methode
 
 // Filter variables (motor1-filter-v1,-v2)
 double M1_f_v1 = 0.0, M1_f_v2 = 0.0;
@@ -54,8 +54,8 @@
 // PROGRAM THAT CALCULATES ANGLE CHANGES
 
 //Xdes and Ydes changer
-void Counter(double &des, double dir, double sig){
-    if (sig == 0){
+void Counter(double &des, double dir, double sig){//wat doet dit???
+    if (sig == 0){   
         if (dir == 1)
             des = des + 0.1;
         else if (dir == 0)
@@ -64,55 +64,49 @@
 }
 
 //Kinematic model
-void Kinematic_referencer( double &xdes, double &ydes, double &q1, double &q2)
+void Kinematic_referencer( double &xdes, double &ydes, double &q1, double &q2, double &Angle1, double &Angle2 )
     {
-    double qt = q1 + q2;                               // current total angle
-    double xcurrent = L1 * cos (q1) + L2 * cos (qt);   // current x position
-    double ycurrent = L1 * sin (q1) + L2 * sin (qt);   // current y position
+    double Anglet = Angle1 + Angle2;                               // current total angle
+    double xcurrent = L1 * cos (Angle1) + L2 * cos (Anglet);   // current x position
+    double ycurrent = L1 * sin (Angle1) + L2 * sin (Anglet);   // current y position
     
     //Initial twist
-    double vx = (xdes - xcurrent)/1;  // Running on 100 Hz
-    double vy = (ydes - ycurrent)/1;
+    double vx = (xdes - xcurrent)/0.01;  // Running on 100 Hz
+    double vy = (ydes - ycurrent)/0.01;
     
     //Jacobians
     double J11 = -ycurrent;
-    double J12 = -L2 * sin (qt);
+    double J12 = -L2 * sin (Anglet);
     double J21 = xcurrent;
-    double J22 = L2 * cos (qt);
+    double J22 = L2 * cos (Anglet);
     double Determinant = J11 * J22 - J21 * J12;   // calculate determinant
     
-    pc.printf("D = %.3f \r\n", Determinant);
- 
-    //Calculate angular velocities
     double q1der = (J22 * vx - J12 * vy) / Determinant;
     double q2der = (-J21 * vx + J11 * vy) / Determinant;
-    
-    //Calculate new angles
-    double q1new = q1 + q1der/100;    //nog fixen met die tijdstappen?
-    double q2new = q2 + q2der/100;      //hier ook
-    //printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
-    double qtnew = q1new + q2new;
-    
-    //Calculate new positions
-    double xnew = L1 * cos (q1new) + L2 * cos (qtnew);
-    double ynew = L1 * sin (q1new) + L2 * sin (qtnew);
-    //printf ("x=%f, y=%f\n", x, y);
+
+    double Angle1new = Angle1 + q1der/100;    //nog fixen met die tijdstappen?
+    double Angle2new = Angle2 + q2der/100;    
+
+    double Angletnew = Angle1new + Angle2new;
+ 
+    double xnew = L1 * cos (Angle1new) + L2 * cos (Angletnew);
+    double ynew = L1 * sin (Angle1new) + L2 * sin (Angletnew);
     
     // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
     // New y may not be negative, this means the arm is located in on the plate
-    // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
-    // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
-    if (ynew > -5 && q1new > -45 / DEG_PER_RAD && q2new < 185 / DEG_PER_RAD )//&& Determinant < 0.01)
+    // New q1 may not be less than -55 degrees, less means the arm will crash into the base plate
+    // New q2 may not be more than 195 degrees, more means the lower arm will crash into the upper arm
+    if (ynew > -10 && qAngle1new > -55 / DEG_PER_RAD && qAngle2new < 195 / DEG_PER_RAD ))
     {
         // If desired, change the angles
-        q1 = q1new;
-        q2 = q2new;
+       Angle1 = Angle1new;
+       Angle2 = Angle2new;
     }
     else
     {
         // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
         xdes = xcurrent;
-        ydes = ycurrent;
+        ydes = ycurrent;//loopt het hier niet vast??
     }
 }
 
@@ -132,7 +126,7 @@
 }
 
 // PROGRAMS THAT CONTROLS THE VALUE OUTPUT
-void M_Controller(double Angle1, double Angle2, double &MotorValue1, double &MotorValue2) {
+void M_Controller(double Angle1, double Angle2, double &MotorValue1, double &MotorValue2) {// waarom gebruik je pass by reference
     if (potmeter1 > 0.5f) {
         direction1 = 1;
         led_red = 0; }
@@ -150,12 +144,10 @@
     Counter(xdes, direction1, button1.read());
     Counter(ydes, direction2, button2.read()); 
     
-    Kinematic_referencer(xdes, ydes, q1, q2);
+    Kinematic_referencer(xdes, ydes, q1, q2, Angle1, Angle2);
     
-    //pc.printf("%.2f %.2f \r\n", xdes, ydes); 
-    
-    double ref_q1 = 2 * q1 * DEG_PER_RAD;
-    double ref_q2 = (q2 - M_Pi) * DEG_PER_RAD;
+    double ref_q1 = 2 * Angle1 * DEG_PER_RAD;
+    double ref_q2 = (Angle2 - M_Pi) * DEG_PER_RAD;// waarom -Pi???
     
         
     MotorValue1 = PID( ref_q1 - Angle1 , M1_Kp, M1_Ki, M1_Kd, M1_Ts, M1_N, M1_f_v1, M1_f_v2); //Find the motorvalue by going through the PID
@@ -194,7 +186,7 @@
 void MeasureAndControl() // Pure values being calculated and send to the Mbed.
 {     
     double Angle1 = DEG_PER_RAD * RAD_PER_PULSE * motor1.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
-    double Angle2 = DEG_PER_RAD * RAD_PER_PULSE * motor2.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
+    double Angle2 = DEG_PER_RAD * RAD_PER_PULSE * motor2.getPosition(); 
     
     M_Controller(Angle1, Angle2, MotorValue1, MotorValue2 ); //Perhaps call the Motorvalues themselves inside this function and edit them that way...