Diff: main.cpp

Revision:

16:29d3851cfd52

Parent:

11:ca91fc47ad02

Child:

20:e20c26a1f6ba

--- a/main.cpp	Mon Oct 21 14:20:28 2019 +0000
+++ b/main.cpp	Tue Oct 22 15:05:12 2019 +0000
@@ -5,146 +5,222 @@
 #include <math.h>
 #include "BiQuad.h"
 
-DigitalIn button1(D12);
+// To play with buttons and potmeters
 AnalogIn pot2(A0);
 AnalogIn pot1(A1);
-Ticker motor; //ticker to call motor values
-DigitalOut motor1Direction(D7); // is a boolean
-FastPWM motor1absolutemotorvalueocity(D6);
+
+// Usual stuff
 MODSERIAL pc(USBTX, USBRX);
-QEI Encoder(D8,D9,NC,8400);
+
+//ticker to call motor values
+Ticker motor; 
+
+// Direction and Velocity of the motors
+DigitalOut motor1Direction(D7); 
+FastPWM motor1Velocity(D6);
+DigitalOut motor2Direction(D4);
+FastPWM motor2Velocity(D5);
+
+// Encoders 1 and 2 respectively
+QEI Encoder1(D8,D9,NC,8400);
+QEI Encoder2(D11,D10,NC,8400);
 
 
 volatile double frequency = 17000.0;
 volatile double period_signal = 1.0/frequency;
 float timeinterval = 0.001f;
-int counts;
-float measuredposition;
-float motorvalue;
-double u_i;
-float potmeter1 = pot1.read();
-float yendeffector = 20;
+float motorvalue1;
+float motorvalue2;
+float yendeffector = 10.6159;
 float xendeffector = 0;
-float Kp;
 
-
-
-
+// ANTI WIND UP NEEDED
 
 // --------------------   README ------------------------------------
 // positive referenceposition corresponds to a counterclockwise motion
 // negative referenceposition corresponds to a clockwise motion 
 
 //PID control implementation (BiQuead)
-double PID_controller(double error)
+double PID_controller1(float error1)
 {
-    static double error_integral = 0;
-    static double error_prev = error;
+    // Define errors for motor 1 and 2
+    static double error_integral1 = 0;
+    static double error_prev1 = error1;
+    
+    // Low-pass filter
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-    float Kp = 30;
-    float Ki = 2;
-    float Kd = 0.6;
+    
+    // PID variables: we assume them to be the same for both motors
+    float Kp = 10;
+    float Ki = 1;
+    float Kd = 0.01;
     
     //Proportional part:
-    double u_k = Kp * error;
+    double u_k1 = Kp * error1;
     
     // Integreal part
-    error_integral = error_integral + error * timeinterval;
-    double u_i = Ki*error_integral;
-    // anti wind up
-    if (u_i > 10)
-    {
-        u_i = 10 ;   
-    }
-    
+    error_integral1 = error_integral1 + error1 * timeinterval;
+    double u_i1 = Ki*error_integral1;
+
     // Derivate part
-    double error_derivative = (error - error_prev)/timeinterval;
-    double filtered_error_derivative = LowPassFilter.step(error_derivative);
-    double u_d = Kd * filtered_error_derivative;
-    error_prev = error;
+    double error_derivative1 = (error1 - error_prev1)/timeinterval;
+    double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
+    double u_d1 = Kd * filtered_error_derivative1;
+    error_prev1 = error1;
     
     //sum all parts and return it
-    return u_k + u_i + u_d;
+    return u_k1 + u_i1 + u_d1;
+}
+
+double PID_controller2(float error2)
+{
+    // Define errors for motor 1 and 2
+    static double error_integral2 = 0;
+    static double error_prev2 = error2;
+    
+    // Low-pass filter
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    
+    // PID variables: we assume them to be the same for both motors
+    float Kp = 1;
+    float Ki = 0.8;
+    float Kd = 1;
+    
+    //Proportional part:
+    double u_k2 = Kp * error2;
+    
+    // Integreal part
+    error_integral2 = error_integral2 + error2 * timeinterval;
+    double u_i2 = Ki*error_integral2;
+
+    // Derivate part
+    double error_derivative2 = (error2 - error_prev2)/timeinterval;
+    double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
+    double u_d2 = Kd * filtered_error_derivative2;
+    error_prev2 = error2;
+    
+    //sum all parts and return it
+    return u_k2 + u_i2 + u_d2;
 }
 
 //get the measured position
-double getmeasuredposition()
+void getmeasuredposition(float & measuredposition1, float & measuredposition2)
 {   
-    counts = Encoder.getPulses();
-    measuredposition = ((float)counts) / 8400.0f * 2.0f;
+    // Obtain the counts of motors 1 and 2 from the encoder
+    int countsmotor1;
+    int countsmotor2;
+    countsmotor1 = Encoder1.getPulses();
+    countsmotor2 = Encoder2.getPulses();
     
-    return measuredposition;
+    // Obtain the measured position for motor 1 and 2
+    measuredposition1 = ((float)countsmotor1) / 8400.0f * 2.0f;
+    measuredposition2 = ((float)countsmotor2) / 8400.0f * 2.0f;
 }
 
-//get the reference of the absolutemotorvalueocity
-double getreferenceposition()
+//get the reference of the 
+void getreferenceposition(float & referenceposition1, float & referenceposition2)
 {
-    float L0 = 1.95;
+    //Measurements of the arm
+    float L0=1.95;
     float L1=15;
     float L2=20;
-    float motorcounts1;
-    float tangens = atan2(yendeffector,(L0+xendeffector))*180/3.1415;
-    float cosinus = acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
-    motorcounts1=tangens+cosinus;
-    //printf("motorcounts1 is %f\r\n", motorcounts1);
-    float tangens2 = atan2(yendeffector,(L0-xendeffector))*180/3.1415;
-    float cosinus2 = acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
-    //motorcounts2=tangens2+cosinus2;
-    float referenceposition;
-    float variable;
-    variable = motorcounts1/360;
-    referenceposition = variable; //this determines the amount of spins
-    return referenceposition;
+    
+    //Define the new counts that are needed
+    float desiredmotorangle1;
+    float desiredmotorangle2;
+    
+    //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
+    desiredmotorangle1 = atan2(yendeffector,(L0+xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
+    desiredmotorangle2 = atan2(yendeffector,(L0-xendeffector))*180/3.1415 + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
+
+    //Convert motor angles to counts
+    float desiredmotorrounds1;
+    float desiredmotorrounds2;
+    desiredmotorrounds1 = (desiredmotorangle1-180)/360;
+    desiredmotorrounds2 = (desiredmotorangle2-180)/360;
+    
+    //Assign this to new variables because otherwise it doesn't work
+    referenceposition1 = desiredmotorrounds1;
+    referenceposition2 = desiredmotorrounds2;
 }   
 
 //send value to motor
-void sendtomotor(float motorvalue)
+void sendtomotor(float & motorvalue1, float & motorvalue2)
 {   
-    float absolutemotorvalue = 0.0f;
-    absolutemotorvalue = fabs(motorvalue);
-    absolutemotorvalue = absolutemotorvalue > 1.0f ? 1.0f : absolutemotorvalue;   //if absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
-    motor1absolutemotorvalueocity = absolutemotorvalue;
-
-    motor1Direction = (motorvalue > 0.0f); //boolean output: true gives counterclockwise direction, false gives clockwise direction
+    // Define the absolute motor values
+    float absolutemotorvalue1 = 0.0f;
+    float absolutemotorvalue2 = 0.0f;
+    absolutemotorvalue1 = fabs(motorvalue1);
+    absolutemotorvalue2 = fabs(motorvalue2);
+    
+    // If absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
+    absolutemotorvalue1 = absolutemotorvalue1 > 1.0f ? 1.0f : absolutemotorvalue1;  
+    absolutemotorvalue2 = absolutemotorvalue2 > 1.0f ? 1.0f : absolutemotorvalue2;
+    
+    // Send the absolutemotorvalue to the motors
+    motor1Velocity = absolutemotorvalue1;
+    motor2Velocity = absolutemotorvalue2;
+    
+    // Determine the motor direction. Boolean output: true gives counterclockwise direction, false gives clockwise direction
+    motor1Direction = (motorvalue1 > 0.0f); 
+    motor2Direction = (motorvalue2 > 0.0f);
 }
 
 // function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
 void measureandcontrol(void)
 {
-    float referenceposition = getreferenceposition(); 
-    measuredposition = getmeasuredposition();
-    motorvalue = PID_controller(referenceposition - measuredposition);
-    sendtomotor(motorvalue);
+    // Get the reference positions of motor 1 and 2
+    float reference1, reference2;
+    getreferenceposition(reference1, reference2); 
+    
+    // Get the measured positions of motor 1 and 2
+    float measured1, measured2;
+    getmeasuredposition(measured1, measured2);
+    
+    // Calculate the motor values
+    float motorvalue1, motorvalue2;
+    motorvalue1 = PID_controller1(reference1 - measured1);
+    motorvalue2 = PID_controller2(reference2 - measured2);
+    sendtomotor(motorvalue1, motorvalue2);
 }
 int main()
 {
+    // Usual stuff
     pc.baud(115200);
     pc.printf("Starting...\r\n");
-    motor1absolutemotorvalueocity.period(period_signal);
+    
+    // Something with pulses
+    motor1Velocity.period(period_signal);
+    motor2Velocity.period(period_signal);
+    
+    // Call the ticker function
     motor.attach(measureandcontrol, timeinterval); 
     while(true)
     {
         wait(0.5);
-        pc.printf("number of counts %i\r\n", counts);
-        pc.printf("measured position is %f\r\n", measuredposition);
-        pc.printf("motorvalue is %f\r\n", motorvalue);
-        pc.printf("u_i is %d\r\n", u_i);
-        pc.printf("potmeter 1 gives %f\r\n", potmeter1);
+        //pc.printf("number of counts %i\r\n", counts);
+        //pc.printf("measured position is %f\r\n", measuredposition);
+        //pc.printf("motorvalue is %f\r\n", motorvalue);
+        //pc.printf("u_i is %d\r\n", u_i);
+        float potmeter1 = 5+pot1.read()*20;
+        float potmeter2 = pot2.read()*10;
+        pc.printf("Kp gives %f\r\n", potmeter1);
+        pc.printf("Ki gives %f\r\n", potmeter2);
         pc.printf("x is %f\r\n",xendeffector);
         pc.printf("y is %f\r\n",yendeffector);
-        pc.printf("Kp is %f\r\n", Kp);
+        //pc.printf("Kp is %f\r\n", Kp);
         
             
-    char a = pc.getc();
+    char a = pc.getcNb();
     
     if(a == 'r'){
-        xendeffector=xendeffector+5;}
+        xendeffector=xendeffector+1;}
         else if(a=='l'){
-            xendeffector=xendeffector-5;}
+            xendeffector=xendeffector-1;}
         else if(a=='u'){
-            yendeffector=yendeffector+5;}
+            yendeffector=yendeffector+1;}
         else if(a=='d'){
-            yendeffector=yendeffector-5;}
+            yendeffector=yendeffector-1;}
             
         
         
@@ -154,8 +230,9 @@
         if (c == 'c')
         {
             pc.printf("Program stopped running");
-            motorvalue = 0;
-            sendtomotor(motorvalue);
+            motorvalue1 = 0;
+            motorvalue2 = 0;
+            sendtomotor(motorvalue1, motorvalue2);
             break;
         }
     }