Diff: main.cpp

Revision:

3:695daa59763d

Parent:

2:3feeeb434275

--- a/main.cpp	Mon Oct 14 09:48:55 2019 +0000
+++ b/main.cpp	Thu Oct 24 12:12:58 2019 +0000
@@ -1,33 +1,324 @@
 #include "mbed.h"
-//#include "HIDScope.h"
+#include "MODSERIAL.h"
 #include "QEI.h"
-#include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
+//#include "FilterDesign.h"
+#include "BiQuad.h"
+#include "BiQuad4.h"
+#include "HIDScope.h"
+
+
+QEI Encoder1(D12,D13,NC,32);
+QEI Encoder2(D10,D11,NC,32);
+
+DigitalOut M1(D4);
+DigitalOut M2(D7);
+DigitalIn button(SW3);
+
+PwmOut E1(D5);
+PwmOut E2(D6);
+
+
+// GLOBAL VALUES
+float pi = 3.14159265359;
+int counts1;                        // Counts of encoder 1
+int counts2;                        // Counts of encoder 2
+float theta1;                       // Angle of motor 1 (rad)
+float theta2;                       // Angle of motor 2 (rad)
+float vel_1;                        // Velocity of motor 1
+float vel_2;                        // Velocity of motor 2
+float theta1_prev = 0.0;            // Previous angles set to zero
+float theta2_prev = 0.0;             
+double dirx = 1.0;                  //determine the direction of the setpoint placement
+double diry = 1.0;                  //determine the direction of the setpoint placement
+volatile double  U1;                // Motor voltage for motor 1        
+volatile double  U2;                // Motor voltage for motor 2
+float tijd = 0.005;
 
 
-MODSERIAL pc(USBTX, USBRX);
-InterruptIn button(SW3);
+// Inverse kinematics
+const double La = 200.0;            // length of the first link
+const double Lb = 200.0;            // length of the second link
+volatile double q1_diff;
+volatile double q2_diff;
+const double sq = 2.0;              // to square numbers
+
+
+// DEMO
+double  point1x = 0;            // Coordinates of first prescribed point
+double  point1y = 0;
+double  point2x = 0;            // Coordinates of second prescribed point
+double  point2y = 80;
+double  point3x = -50;            // Coordinates of third prescribed point
+double  point3y = 80;
+double  point4x = -50;            // Coordinates of fourth prescribed point
+double  point4y = 30;
+double  point5x = 0;                 // Coordinates of fifth prescribed point
+double  point5y = 30;
+volatile int track = 1;             // Start with the track from zero to first prescribed point
 
-Ticker motor;
-volatile int counts;
-void readencoder()
-{
-QEI Encoder(D12,D13,NC,32);
-counts = Encoder.getPulses();
-}
-void print()
-{
-    pc.printf("%i",counts);
-    }
+const double stepsize1 = 0.15;
+const double stepsize2 = 0.25;
+const double setpoint_error = 0.3;
+double setpointx = 0 ;
+double setpointy = 0 ;
+
+void ReadEncoder1() ;
+void ReadEncoder2() ;
+double counts2angle1() ;
+double counts2angle2() ;
+void ChangeDirectionX();
+void ChangeDirectionY() ;
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
+double determinedemosetx(double setpointx, double setpointy) ;
+double determinedemosety(double setpointx, double setpointy) ;
+double calcRot1(double xDes, double yDes) ;
+double calcRot2(double xDes, double yDes) ;
+
+    
+
+
+// ----------------------------------------------
+// ------- MAIN FUNCTION ------------------------
+// ----------------------------------------------
 
 int main()
 {
-    button.mode(PullUp);
-    pc.baud(115200);
-    button.rise(print);
-    motor.attach(readencoder, 0.002);
+    
     while (true)
     {
+    moveMotorTo(&M1,&E1,&Encoder1,calcRot1(setpointx,setpointy)) ;
+    moveMotorTo(&M2,&E2,&Encoder2,calcRot2(setpointx,setpointy)) ;
 }
-}
\ No newline at end of file
+}
+
+
+// Encoders
+void ReadEncoder1()                              // Read Encoder of motor 1.
+{
+    counts1 = Encoder1.getPulses();              // Counts of outputshaft of motor 1
+    theta1 = (float(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
+    vel_1 = (theta1 - theta1_prev) / tijd;       // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
+    theta1_prev = theta1;                        // Define theta_prev
+}
+
+void ReadEncoder2()                              // Read encoder of motor 2.
+{
+    counts2 = Encoder2.getPulses();              // Counts of outputshaft of motor 2
+    theta2 = (float(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
+    vel_2 = (theta2 - theta2_prev) / tijd;       // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
+    theta2_prev = theta2;                        // Define theta_prev
+}
+double  counts2angle1()
+{
+    counts1 = Encoder1.getPulses();              // Counts of outputshaft of motor 1
+    theta1 = -(double(counts1)/4200) * 2*pi;     // Angle of outputshaft of motor 1
+    return theta1;
+}
+
+double counts2angle2()
+{
+    counts2 = Encoder2.getPulses();               // Counts of outputshaft of motor 2
+    theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
+    return theta2;
+}
+
+
+
+
+//function to change the moving direction of the setpoint
+void ChangeDirectionX(){
+    dirx = -1*dirx;
+    }
+    
+void ChangeDirectionY(){
+    diry = -1*diry;
+    } 
+
+// DEMO set
+
+double determinedemosetx(double setpointx, double setpointy)
+{
+
+    if (setpointx < point1x && track == 1){ 
+        setpointx = setpointx + stepsize1;    
+    }
+    
+    // From point 1 to 2
+    if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 51)) {
+        track = 12;
+    }
+    
+    if (setpointx < point2x && track == 12){
+        setpointx = point2x;
+    }
+    
+    // From point 2 to 3 
+    if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && track == 12){ 
+        track = 23;
+    }
+
+    if (setpointy > point3y && track == 23){
+        setpointx = setpointx + stepsize1;          
+        // Stay on the same y value from point 2 to 3 
+    } 
+ 
+    // From point 3 to 4
+    if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)) {
+        setpointy = point4y;
+        track = 34;
+    }
+    
+    if (setpointx > point4x && track == 34){
+        setpointx = setpointx - stepsize2;
+    }
+    
+    // From point 4 to 5     
+    if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){
+        setpointx = point4x;
+        track = 45;
+    }
+    
+    
+    if (setpointy < point5y && track == 45){
+        setpointx = point5x;        
+        // From point 4 to 5, stay on the same x value
+    }
+    
+    // From point 5 to 1     
+    if ((fabs(setpointx - point5x) <= setpoint_error) && (fabs(setpointy - point5y) <= setpoint_error) && (track == 45)){
+        setpointx = point5x;
+        track = 51;
+    }
+    
+    
+    if (setpointy < point1y && track == 51){
+        setpointx = point1x;        
+        // From point 4 to 5, stay on the same x value
+    }
+    return setpointx;
+}
+
+double determinedemosety(double setpointx, double setpointy)
+{
+    // From reference to point 1
+    if(setpointy < point1y && track == 1){
+        setpointy = setpointy + (stepsize2);
+    } 
+
+    // From point 1 to 2
+    if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 51)){
+        setpointy = point2y;          
+        // Stay on the same y from point 1 to 2. 
+        track = 12;
+    }
+    if (setpointx < point2x && track == 12){
+        setpointy = point2y;
+    }
+    
+    // From point 2 to 3
+    if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){
+        setpointx = point3x;
+        track = 23;
+    }
+    if ((setpointy > point3y) && (track == 23)){
+        setpointy = setpointy + (-stepsize2);
+        track = 23;
+    }    
+    
+    // From point 3 to 4
+    if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)){
+        setpointy = setpointy;
+        track = 34;
+    }
+    if (setpointx > point4x && track == 34){
+        setpointy = setpointy;
+    }     
+    
+    // From point 4 to 5
+    if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){
+        track = 45;
+    }
+    
+    if (setpointy > point5y && track == 45){
+        setpointy = setpointy - (stepsize2);        
+        // From point 4 to 5, stay on the same x value.
+    }
+    // From point 5 to 1
+    if ((fabs(setpointx - point5x) <= setpoint_error) && (fabs(setpointy - point5y) <= setpoint_error) && (track == 45)){
+        track = 51;
+    }
+    
+    if (setpointy < point1y && track == 51){
+        setpointy = setpointy + (stepsize2);        
+        // From point 4 to 5, stay on the same x value.
+    }
+    return setpointy;
+    
+}
+
+//function to mave a motor to a certain number of rotations, counted from the start of the program.
+//parameters:
+//DigitalOut *M = pointer to direction cpntol pin of motor
+//PwmOut *E = pointer to speed contorl pin of motor
+//QEI *Enc = pointer to encoder of motor
+//float rotDes = desired rotation
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
+{
+    float pErrorC;
+    float pErrorP = 0;
+    float iError = 0;
+    float dError;
+
+    float Kp = 5;
+    float Ki = 0.01;
+    float Kd = 0.7;
+
+    float rotC = Enc->getPulses()/(32*131.25);
+    float rotP = 0;
+    float MotorPWM;
+
+    Timer t;
+    float tieme = 0;
+
+    t.start();
+    do {
+        pErrorP = pErrorC;
+        pErrorC = rotDes - rotC;
+        iError = iError + pErrorC*tieme;
+        dError = (pErrorC - pErrorP)/tieme;
+
+        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
+
+        if(MotorPWM > 0) {
+            *M = 0;
+            *E = MotorPWM;
+        } else {
+            *M = 1;
+            *E = -MotorPWM;
+        }
+
+        rotP = rotC;
+        rotC = Enc->getPulses()/(32*131.25);
+        tieme = t.read();
+        t.reset();
+    } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
+    t.stop();
+}
+
+//double that calculates the rotation of one arm.
+//parameters:
+//double xDes = ofset of the arm's shaft in cm in the x direction
+//double yDes = ofset of the arm's shaft in cm in the y direction
+double calcRot1(double xDes, double yDes)
+{
+    return 6*((atan(yDes/xDes) - 0.5*(pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*pi));
+};
+
+//double that calculates the rotation of the other arm.
+//parameters:
+//double xDes = ofset of the arm's shaft in cm in the x direction
+//double yDes = ofset of the arm's shaft in cm in the y direction
+double calcRot2(double xDes, double yDes)
+{
+    return 6*((atan(yDes/xDes) + 0.5*(pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*pi));
+};
\ No newline at end of file