Diff: main.cpp

Revision:

1:864a5f8bb886

Parent:

0:46cf63cba59a

Child:

2:69bfc537508f

--- a/main.cpp	Wed Nov 01 09:47:36 2017 +0000
+++ b/main.cpp	Thu Nov 02 11:00:44 2017 +0000
@@ -5,6 +5,7 @@
 // include all necessary libraries
 #include "mbed.h"
 #include "QEI.h"
+#include "math.h"
 
 //intialize all pins
 PwmOut motor1(D5);
@@ -17,7 +18,8 @@
 DigitalIn button2(D9); //button to move ccw
 
 //initialize variables
-double angleIncrement = 0.036; // increment of angle when button pressed (1 is a whole rotation (360 degrees))
+const float pi = 3.14159265358979323846; //value for pi
+double positionIncrement = 30; // 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
@@ -33,14 +35,20 @@
 
 double motor1ErrorInt=0; //error of motor1 for the integrating part of PI controller
 double motor1ErrorDif=0; //error of motor1 for the integrating part of PI controller
-double desiredPos1 =0; //desired position of motor1
+double desiredAngle1 =0; //desired position of motor1
 
 double motor2ErrorInt=0; //error of motor1 for the integrating part of PI controller
 double motor2ErrorDif=0; //error of motor1 for the integrating part of PI controller
-double desiredPos2 =0; //desired position of motor1
+double desiredAngle2 =0; //desired position of motor1
 //initialize ticker for checking and correcting the angle
 Ticker myControllerTicker;
 
+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 xe, ye, D, phi, q2, beta, alpha, q1; //other variables used in calculating the angle for the motors
+//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){
     const double a1 = -4/(N*Ts+2);
@@ -51,15 +59,56 @@
 
     double v = error - a1*intError - a2*DifError;
     double u = b0*v + b1*intError + b2*DifError;
-    DifError = intError; intError = v;
+    DifError = intError;
+    intError = v;
     return u;
 }
 
+//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(){
+//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.
+    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
+        }
+        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;
+    } 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;
+    desiredAngle1 = q1 * rad2rot;
+    desiredAngle2 = q2 * rad2rot;
+}
 
 void motorButtonController(){
     double position1= encoderToMotor*motor1Encoder.getPulses();
-    double posError1 = desiredPos1 - position1; 
-    
+    double posError1 = desiredAngle1 - position1;
+
     //change direction based on error sign
     if(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) {
         motor1Dir=0;
@@ -68,10 +117,10 @@
     }
     //set motor speed based on PI controller error
     motor1 = fabs(PIDController( posError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif));
-    
+
     double position2= encoderToMotor*motor2Encoder.getPulses();
-    double posError2 = desiredPos2 - position2; 
-    
+    double posError2 = desiredAngle2 - position2;
+
     //change direction based on error sign
     if(PIDController( posError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) {
         motor2Dir=0;
@@ -83,19 +132,18 @@
 
 }
 
-int main(){
+int main()
+{
     wait(2);
     myControllerTicker.attach(&motorButtonController, controllerTickerTime);
     while(1) {
         if(!button1) {
-            desiredPos1+=angleIncrement;
-            desiredPos2-=angleIncrement;
+            y_des+=positionIncrement;
             wait(0.5f);
         }
 
         if(!button2) {
-            desiredPos1-=angleIncrement;
-            desiredPos2+=angleIncrement;
+            y_des-=positionIncrement;
             wait(0.5f);
         }
     }