Diff: main.cpp

Revision:

2:69bfc537508f

Parent:

1:864a5f8bb886

Child:

3:c63c0a23ea21

--- a/main.cpp	Thu Nov 02 11:00:44 2017 +0000
+++ b/main.cpp	Thu Nov 02 18:37:18 2017 +0000
@@ -6,6 +6,8 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "math.h"
+#include "iostream"
+#include "MODSERIAL.h"
 
 //intialize all pins
 PwmOut motor1(D5);
@@ -16,18 +18,21 @@
 QEI motor2Encoder (D12,D13, NC, 624,QEI::X4_ENCODING);
 DigitalIn button1(D8); //button to move cw
 DigitalIn button2(D9); //button to move ccw
+DigitalIn button3(D2);
+DigitalIn button4(D3);
+MODSERIAL pc(USBTX, USBRX);
 
 //initialize variables
 const float pi = 3.14159265358979323846; //value for pi
-double positionIncrement = 30; // increment of angle when button pressed (1 is a whole rotation (360 degrees))
+double positionIncrement = 20; // increment of angle when button pressed (1 is a whole rotation (360 degrees))
 
-const double motor1KP=1.3; //Proportional gain of motor1 PI control
-const double motor1KI=0.5; //Integral gain of motor1 PI control
-const double motor1KD=0.5; // Differential gain of motor1 PID control
+const double motor1KP=7.0; //Proportional gain of motor1 PI control
+const double motor1KI=3.0; //Integral gain of motor1 PI control
+const double motor1KD=3.0; // Differential gain of motor1 PID control
 
 const double motor2KP=1.3; //Proportional gain of motor1 PI control
 const double motor2KI=0.5; //Integral gain of motor1 PI control
-const double motor2KD=0.5; // Differential gain of motor1 PID control
+const double motor2KD=1.0; // Differential gain of motor1 PID control
 
 const double N=100; //LP filter coefficient
 const double encoderToMotor= 0.000119047619047619; //proportion of the rotation of the motor to the rotation of the encoder
@@ -45,12 +50,15 @@
 
 const float l1 = 460; //Length of the arm from base to joint 1 (arm1)   ENTER MANUALLY [mm]
 const float l2 = 450; //length of the arm from joint 1 to the end-effector.(arm2)   ENTER MANUALLY [mm]
-float x_des = 0; //(initial)desired x location of the end-effector (ee)
-float y_des = l1+l2; //(initial) desired y location of the end-effector (ee)
+float x_des = l1+l2; //(initial)desired x location of the end-effector (ee)
+float y_des = 0; //(initial) desired y location of the end-effector (ee)
 float xe, ye, D, phi, q2, beta, alpha, q1; //other variables used in calculating the angle for the motors
+float radDeg, rad2rot;
 //q1 is the angle for the base motor, q2 is the angle for the elbow motor, both in [rad]
 
-double PIDController(double error, const double Kp, const double Ki, const double Kd, double Ts, const double N,double &intError, double &DifError){
+
+double PIDController(double error, const double Kp, const double Ki, const double Kd, double Ts, const double N,double &intError, double &DifError)
+{
     const double a1 = -4/(N*Ts+2);
     const double a2 = -(N*Ts-2)/(N*Ts+2);
     const double b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
@@ -65,86 +73,133 @@
 }
 
 //Code for motor angles as a function of the length and positions of the arms, for a double revolutional joint arm in 2D plane
-void motorAngle(){
+void motorAngle()
+{
 //Function for making sure the arm does not exceed its maximum reach
 //if it tries to go beyond its max. reach
 //it will try to reach a point within reach in the same direction as desired.
+//Works for all 4 quadrants of the (unit) circle
     xe = x_des;
     ye = y_des;
-    while(pow(xe, 2)+pow(ye,2) > pow(l1+l2, 2)) {
-        if (y_des == 0) {   //make sure you do not divide by 0 if ye == 0
-            xe = x_des - 1;
-        } else {
-            xe = x_des - (x_des/y_des)/10;    //go to a smaller xe point on the same line
+
+    while(pow(xe, 2)+pow(ye, 2) > pow(l1+l2, 2)) {
+        if (xe > 0) {            //right hand plane
+            if (ye > 0) {        //positive x & y            QUADRANT 1
+                xe = x_des - (x_des/y_des);   //go to a smaller xe point on the same line
+                ye = y_des - (y_des/x_des);   //go to a smaller ye point on the same line
+            } else if (ye < 0) { //positive x, negative y    QUADRANT 2
+                xe = x_des - (x_des/y_des);   //smaller xe
+                ye = y_des + (y_des/x_des);   //greater ye
+            } else if (ye == 0) { //positive x, y == 0
+                xe = x_des - 1;                  //stay on the x-axis but at a smaller value (within reach)
+            }
+        } else if (xe < 0) {     //left hand plane
+            if (ye > 0) {        //negative x, positive y    QUADRANT 4
+                xe = x_des + (x_des/y_des);   //greater xe
+                ye = y_des - (y_des/x_des);   //smaller ye
+            } else if (ye < 0) { //negative x & y            QUADRANT 3
+                xe = x_des + (x_des/y_des);   //greater xe
+                ye = y_des + (y_des/x_des);   //greater ye
+            } else if (ye ==0) { //negative x, y == 0
+                xe = x_des + 1;                  //stay on the x-axis but at a greater value (within reach)
+            }
+
+        } else if (xe == 0) {    //on the y-axis
+            if (ye > 0) {        //x == 0, positive y
+                ye = y_des - 1;                  //stay on the y-axis but at a smaller value (within reach)
+            } else if (ye < 0) { //x == 0, negative y
+                ye = y_des + 1;                  //stay on the y-axis but at a greater value (within reach)
+                //x == 0 & y == 0 is a state that is unable to be reached, no need to cover that case
+            }
+            x_des = xe;
+            y_des = ye;
         }
-        if (x_des == 0) {   //make sure you do not divide by 0 if xe == 0
-            ye = y_des - 1;
-        } else {
-            ye = y_des - (y_des/x_des)/10;   //go to a smaller ye point on the same line
-        }
-        x_des = xe;
-        y_des = ye;
     }
 
+
 //from here on is the code for setting the angles for the motors
     D = ((pow(l1,2)+pow(l2,2))-pow(xe,2)-pow(ye,2))/(2*l1*l2); //D = cos(phi)
     phi = atan2(sqrt(1 - pow(D, 2)), D); //angle between arm1 and arm2, from arm1 to arm2 [rad]
     //Use atan2(sqrt(1 - pow(D, 2)),D) for "elbow down" position (like your right arm)
     q2 = pi - phi; //angle of arm2 with respect to the orientation of arm1, motor2 [rad]
     if (-pi/2 > q2) {       //Make sure the angle of motor2 doesn’t wreck our setup (max -90 or 90 degrees w.r.t. arm1)
-        q2 = -pi/2;
+        q2 = 0;
     } else if ( q2 > pi/2) {
         q2 = pi/2;
     }
     beta = atan2(ye, xe); //angle between "line from origin to ee" and x-axis [rad]
     alpha = atan2(l2*sin(q2), l1+l2*cos(q2)); //angle between "line from origin to ee" and arm1 [rad]
     q1 = beta - alpha; //angle of arm 1 with respect to the x-axis, motor1 [rad]
-    float 1radDeg = 180/pi; //amount of degrees in 1 radian
-    float rad2rot = 1radDeg/360;
+    radDeg = 180/pi;
+    rad2rot = radDeg/360;
     desiredAngle1 = q1 * rad2rot;
     desiredAngle2 = q2 * rad2rot;
+    pc.printf("\n New values \n\r");
+    pc.printf("xloc = %f, yloc = %f \n \r", xe, ye);
+    pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n\r", q1, desiredAngle1, q2, desiredAngle2);
 }
 
-void motorButtonController(){
-    double position1= encoderToMotor*motor1Encoder.getPulses();
-    double posError1 = desiredAngle1 - position1;
+void motorButtonController()
+{
+    double angle1= encoderToMotor*motor1Encoder.getPulses();
+    double angleError1 = (desiredAngle1 - angle1)*1.25;
 
     //change direction based on error sign
-    if(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) {
+    if(PIDController( angleError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) {
         motor1Dir=0;
     } else {
         motor1Dir =1;
     }
     //set motor speed based on PI controller error
-    motor1 = fabs(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif));
+    motor1 = fabs(PIDController( angleError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif));
 
-    double position2= encoderToMotor*motor2Encoder.getPulses();
-    double posError2 = desiredAngle2 - position2;
+    double angle2= encoderToMotor*motor2Encoder.getPulses();
+    double angleError2 = desiredAngle2 - angle2;
 
     //change direction based on error sign
-    if(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) {
+    if(PIDController( angleError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) {
         motor2Dir=0;
     } else {
         motor2Dir =1;
     }
     //set motor speed based on PI controller error
-    motor2 = fabs(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif));
+    motor2 = fabs(PIDController( angleError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif));
 
 }
 
 int main()
 {
     wait(2);
+    xe = x_des;
+    ye = y_des;
+    pc.baud(115200);
+    pc.printf("\n Start up complete \n \r");
+    pc.printf("initial values: \n \r");
+    pc.printf("xloc = %f, yloc = %f \n \r", xe, ye);
+    pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n \r", q1, desiredAngle1, q2, desiredAngle2);
     myControllerTicker.attach(&motorButtonController, controllerTickerTime);
     while(1) {
         if(!button1) {
-            y_des+=positionIncrement;
-            wait(0.5f);
+            y_des += positionIncrement;
+            motorAngle();
+            wait(0.3f);
         }
 
         if(!button2) {
-            y_des-=positionIncrement;
-            wait(0.5f);
+            y_des -= positionIncrement;
+            motorAngle();
+            wait(0.3f);
+        }
+
+        if(button3) {
+            x_des += positionIncrement;
+            motorAngle();
+            wait(0.3f);
+        }
+        if(button4) {
+            x_des -= positionIncrement;
+            motorAngle();
+            wait(0.3f);
         }
     }
 }
\ No newline at end of file