Diff: main.cpp

Revision:

4:89f7d7f3a7ca

Parent:

3:339b6f68b317

Child:

5:ca4f81348c30

--- a/main.cpp	Tue Oct 25 12:36:58 2016 +0000
+++ b/main.cpp	Tue Oct 25 15:23:25 2016 +0000
@@ -2,11 +2,18 @@
 #include "math.h"
 #include "stdio.h"
 #include "MODSERIAL.h"
+#include "QEI.h"
 // Angle limits 215, 345 lower arm, 0, 145 upper arm
 // Robot arm x,y limits (limit to table top)
 
 
 #define M_PI 3.141592653589793238462643383279502884L // Pi
+enum r_states{R_INIT, R_HORIZONTAL, R_VERTICAL, R_GRIPPER};
+r_states state;
+
+/*
+** Table and robot limits
+*/
 const double R_XTABLE_LEFT = -30.0;     // Robot in the middle, table width=60
 const double R_XTABLE_RIGHT = 30.0;
 const double R_YTABLE_BOTTOM = -18.0;
@@ -16,11 +23,41 @@
 const double R_ROBOT_BOTTOM = -18.0;
 const double R_ROBOT_TOP = 2.0;
 
-const double ARM1_ALIMLO = (2*M_PI/360)*120;
-const double ARM1_ALIMHI = -(2*M_PI/360)*120;
+/*
+** Robot arm angle limits
+*/
+const double ARM1_ALIMLO = (2*M_PI/360)*125;
+const double ARM1_ALIMHI = -(2*M_PI/360)*125;
 const double ARM2_ALIMLO = 0;
 const double ARM2_ALIMHI = (2*M_PI/360)*145;
 
+/*
+** Ticker flags
+*/
+
+bool flag_switch;
+bool flag_PID;
+bool flag_output;
+
+Ticker mainticker;  // Main ticker
+
+/*
+** Motor variables
+*/
+DigitalOut motor1dir(D7);
+PwmOut motor1pwm(D6);
+QEI motor1sense(D13,D12,NC, 8400);
+float motor1pos;
+float motor1int;
+float motor1olderr;
+DigitalOut motor2dir(D4);
+PwmOut motor2pwm(D5);
+QEI motor2sense(D11,D10, NC, 8400);
+float motor2pos;
+float motor2int;
+float motor2olderr;
+
+
 float vx=0;
 float vy=0;
 // Struct r_link:
@@ -29,13 +66,7 @@
     float length;   // link length
     float x;        // link x position
     float y;        // link y position
-    float vx;       // link x speed
-    float vy;       // link y speed
-    float ax;       // link x accelleration
-    float ay;       // link y accelleration
     float theta;    // link angle
-    float omega;    // link angular velocity
-    float alpha;    // link angular accelleration
 };
     
 MODSERIAL pc(USBTX, USBRX);
@@ -97,32 +128,56 @@
 
 /*
 ** =============================================================================
-** robot_setposition
+** robot_applyAngleLimits
+** =============================================================================
+** Function:
+** Limits the angles of the robot's arms to prevent them from moving into 
+** themselves
+** Input: -
+** Output: -
+** =============================================================================
+*/
+void robot_applyAngleLimits(){
+    if (arm1.theta>ARM1_ALIMLO){    // Limit angle arm1
+        arm1.theta = ARM1_ALIMLO;
+    }
+    else if (arm1.theta<ARM1_ALIMHI){
+        arm1.theta = ARM1_ALIMHI;
+    }
+
+    if (arm2.theta>ARM2_ALIMHI){    // Limit angle arm 2
+        arm2.theta = ARM2_ALIMHI;
+    } else if (arm2.theta < ARM2_ALIMLO) {
+        arm2.theta = ARM2_ALIMLO;
+    }
+}
+
+/*
+** =============================================================================
+** robot_setSetpoint
 ** =============================================================================
 ** Function:
 ** Transforms cartesian coordinates into rotadions of the joints of the robot's 
 ** arms.
 ** Input:
-** float x, float y: position in cartesian coordinates
+** float x, float y: setpoint position in cartesian coordinates
+** =============================================================================
+**       (x,y)
+**        |\
+**        | \
+**        |b2\
+**     z2 |   \arm2.length
+**        |    \
+**  z     |_ _ _\
+**        |     /
+**        |    /
+**     z1 |   /arm1.length
+**        |b1/
+**        | /
+**        |/
 ** =============================================================================
 */
 void robot_setSetpoint (float x,float y){
-/*
-       (x,y)
-        |\
-        | \
-        |b2\
-     z2 |   \arm2.length
-        |    \
-  z     |_ _ _\
-        |     /
-        |    /
-     z1 |   /arm1.length
-        |b1/
-        | /
-        |/
-        
-*/
     float z;        // length z of vector (x,y)
     float z1;       // part z1
     float z2;       // part z2
@@ -130,14 +185,11 @@
     float angle;    // angle of vector (x,y), !pointing up (x==0) the angle is defined as 0!
     float b1,b2;    // angle b1 and b2 (see drawing)
     
-    float old;      // 'old' helper variable used for calculating speeds
-        
-    
     robot_applySetpointLimits(x,y); // Apply limits
     
-    z = sqrt(pow(x,2)+pow(y,2));    // Calculate (x,y) length
+    z = sqrt(pow(x,2)+pow(y,2));    // Calculate vector (x,y) length
     
-    // Calculate (x,y) angle
+    // Calculate vector (x,y) angle
     angle = atan2(y,x);                 // Calculate (x,y) angle using atan 2
     if (angle<0.5*M_PI && angle>0){     // Rotate result of atan2 90 degrees counter clockwise, so up pointing vector (x==0, y>0) has a zero degree rotation instead of 90 degrees.
 
@@ -167,76 +219,38 @@
     b1 = acos(z1/arm1.length);      // Calculate angle b1
     b2 = acos(z2/arm2.length);      // Calculate angle b2
     
-    // Arm 1 angle and rotational speed
-    old = arm1.theta;               // Preserve old angle arm 1
     arm1.theta = angle - b1;        // Calculate new angle arm 1
-    
-    if (arm1.theta>ARM1_ALIMLO){    // Limit angle arm1
-        arm1.theta = ARM1_ALIMLO;
-    }
-    else if (arm1.theta<ARM1_ALIMHI){
-        arm1.theta = ARM1_ALIMHI;
-    }
-  
-    arm1.omega = arm1.theta - old;  // Calculate rotational speed arm 1
-    
-    // Arm 2 angle and rotational speed
-    old = arm2.theta;               // Preserve old angle arm 2
     arm2.theta = b1 + b2;           // Calculate new angle arm 2
-    if (arm2.theta>ARM2_ALIMHI){    // Limit angle arm 2
-        arm2.theta = ARM2_ALIMHI;
-    } else if (arm2.theta < 0) {
-        arm2.theta = 0;
-    }
-
-    arm2.omega = arm2.theta - old;  // Calculate rotational speed arm 2
-    
-    // Arm 2 position
-    old = arm2.x;                           // Preserve old x position arm 2
+    robot_applyAngleLimits();
+        
     arm2.x = arm1.length*-sin(arm1.theta);  // Calculate new x position arm 2
-    arm2.vx = arm2.x - old;                 // Calculate new x speed arm 2
-    old = arm2.y;                           // Preserve old y position arm 2
     arm2.y = arm1.length*cos(arm1.theta);   // Calculate new y position arm 2
-    arm2.vy = arm2.y - old;                 // Calculate new y speed arm 2
     
-    old = end.x;                                                // Preserve old x position end effector
     end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; // Calculate new x position end effector
-    end.vx = end.x - old;                                       // Calculate new x speed end effector
-    old = end.y;                                                // Preserve old y position end effector
     end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y;  // Calculate new y position end effector
-    end.vy = old - end.y;                                       // Calculate new y speed end effector
-
 }
 
 // robot_init
 // Initialise robot
 void robot_init() {
+    state = R_HORIZONTAL;
     // Init arm1 (upper arm) first link
     arm1.length = 28.0f;
-    arm1.x = 0; arm1.y = 0;
-    arm1.vx = 0; arm1.vy = 0;
-    arm1.ax = 0; arm1.ay = 0;
+    arm1.x = 0;
+    arm1.y = 0;
     arm1.theta = 0;
-    arm1.omega = 0;
-    arm1.alpha = 0;
 
     // Init arm2 (lower arm) second link
     arm2.length = 35.0f;
-    arm2.x = 0; arm2.y = arm1.length;
-    arm2.vx = 0; arm2.vy = 0;
-    arm2.ax = 0; arm2.ay = 0;
+    arm2.x = 0; 
+    arm2.y = arm1.length;
     arm2.theta = 0;
-    arm2.omega = 0;
-    arm2.alpha = 0;
 
     // Init end (end effector) third/last link
-    end.length = 30.0f;
-    end.x = 0; end.y = arm1.length+arm2.length;
-    end.vx = 0; end.vy = 0;
-    end.ax = 0; end.ay = 0;
+    end.length = 5.0f;
+    end.x = 0; 
+    end.y = arm1.length+arm2.length;
     end.theta = 0;
-    end.omega = 0;
-    end.alpha = 0;
 }
 
 int lr_state=0;
@@ -331,30 +345,117 @@
         */
 }
 
+void r_moveHorizontal (){
+    if (flag_switch){   // Read switches and set setpoint once per switch ticker milliseconds
+        flag_switch = false;
+        if(switch1.read())
+        {
+            if (lr_state==0){
+                vx=0;
+                ledr.write(1);
+            }
+        }
+        else
+        {
+            lr_state=0;
+            vx = vx - ax;
+            if (vx<-3.0f){
+                vx=-3.0f;
+            }
+            ledr.write(0);
+        }
+        
+        if(switch2.read())
+        {
+            if(lr_state==1){
+                vx=0;
+                ledr.write(1);
+            }
+        }
+        else
+        {
+            lr_state=1;
+            vx = vx + ax;
+            if (vx>3.0f){
+                vx=3.0f;
+            }
+            ledr.write(0);
+        }
+        robot_setSetpoint(end.x+vx, end.y+vy);
+    }
+}
+
+void setmotor1(float val){
+    motor1dir.write(val>=0?1:0);
+    motor1pwm.write(fabs(val)>1?1.0f:fabs(val));
+}
+void setmotor2(float val){
+    motor2dir.write(val>=0?1:0);
+    motor2pwm.write(fabs(val)>1?1.0f:fabs(val));
+}
+
+
+double PID_control(float& olderr, float position, float setpoint, float& integrator, float dt, float P, float I, float D){
+    float error;
+    float derivative;
+    error = setpoint - position;
+    integrator += error * dt;
+    derivative = (error - olderr) / dt;
+    olderr = error;
+    return (P * error + I * integrator + D * derivative);
+}
+
+void r_doPID(){
+    float dt = 1/50.0;
+    float m1, m2;
+    if (flag_PID){
+        flag_PID = false;
+        motor1pos = (float)motor1sense.getPulses()/8400;
+        motor2pos = (float)motor2sense.getPulses()/8400;
+        m1 = PID_control(motor1olderr, motor1pos, arm1.theta, motor1int, dt, 1.0, 0,0);
+        m2 = PID_control(motor2olderr, motor2pos, arm2.theta, motor2int, dt, 1.0, 0,0);
+        setmotor1(m1);
+        setmotor2(m2);
+    }
+}
+
+void r_outputToMatlab(){
+    if (flag_output){
+        flag_output = false;    
+        printf("Angle1\n\r");
+        printf("%f\n\r", motor1pos);
+        printf("Angle2\n\r");
+        printf("%f\n\r", motor2pos);
+        printf("x: %f | y: %f\n\r", end.x, end.y);
+    }
+}
+
+void maintickerfunction (){
+    static int cnt=0;
+    if (cnt%20 == 0){   // 50 times per second
+        flag_switch = true;
+    }
+    if (cnt%40 == 0){   // 25 times per second
+        flag_PID = true;
+    }
+    if (cnt%100 == 0){  // 10 times per second
+        flag_output = true;
+    }
+}
+
 int main(){
+    mainticker.attach(&maintickerfunction,0.001f);
     pc.baud(115200);
     robot_init();
     while(true){
-        inputswitches();
-        robot_setSetpoint(end.x+vx,end.y+vy);
-        printf("Angle1\n\r");
-        printf("%f\n\r", arm1.theta);
-        printf("Angle2\n\r");
-        printf("%f\n\r", arm2.theta);
-        printf("x: %f | y: %f\n\r", end.x, end.y);
-//        printf("%f || %f", ARM1_ALIMLO, ARM1_ALIMHI);
-
- //       for(int i=0;i<30;i++)
- //       {
- //           printf("\n");
- //       }
- /*
-        printf("speed %f\n\r",vx);
-        printf("end(x,y,theta)     %f.2 : %f.2 : %f.2\n\r",end.x, end.y, arm2.theta);
-        printf("arm2(x,y,theta)    %f.2 : %f.2 : %f.2\n\r",arm2.x,arm2.y, arm1.theta);
-        printf("end(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", end.vx, end.vy, arm2.omega);
-        printf("arm2(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", arm2.vx, arm2.vy, arm1.omega);
-*/
-        wait(0.1f);
+        switch(state){
+            case R_INIT:
+                break;
+            case R_HORIZONTAL:
+                r_moveHorizontal();
+                break;
+        }
+        r_doPID();
+        r_outputToMatlab();
     }
 }
\ No newline at end of file