Revision 0:5502fbcdba50, committed 2018-10-31

Comitter:

ekiliptiay

Date:

Wed Oct 31 16:04:21 2018 +0000

Commit message:

MovingX;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 31 16:04:21 2018 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+    
+double GetActualPosition1(){
+    // Gets the actual position from motor 1 in rad. The difference with the
+    // reference position is the error we are going to use in  the PID-control.
+    double ActualPosition = Encoder1.getPulses()/4200*2*3.14159265358979; 
+    return ActualPosition;
+}
+
+double GetReferencePosition1(double ActualPosition, double DesiredChange){
+        ReferencePosition1 = ActualPosition + DesiredChange; // Gives the position it should be in.
+    return ReferencePosition1;
+}
+
+double GetReferencePosition2(double ActualPosition, double DesiredChange){
+    MotAng2 = IK();
+    ReferencePosition2 = ActualPosition + DesiredChange; // Gives the position it should be in.
+    return ReferencePosition2;
+}
+    
+double GetActualPosition2(){
+    double ActualPosition = Encoder2.getPulses()/4200*2*3.14159265358979;
+    return ActualPosition;
+}
+
+double PID_controller1(double error){
+    static double error_integral = 0;
+    static double error_prev = error; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    double Ts = 0.01;
+    
+    // Proportional part
+        
+        double Kp = 0.1;
+        double u_k = Kp*error;
+
+    // Integral part
+        double Ki = 0.1;
+        error_integral = error_integral + error * Ts;
+        double u_i = Ki * error_integral;
+    
+    // Derivative part
+        double Kd = 10.1;
+        double error_derivative = (error - error_prev)/Ts;
+        double filtered_error_derivative = LowPassFilter.step(error_derivative);
+        double u_d = Kd * filtered_error_derivative;
+        error_prev = error;
+    // Sum all parts and return it
+    return u_k + u_i + u_d;
+}
+
+// Controller for motor 2
+double PID_controller2(double error){
+    static double error_integral = 0;
+    static double error_prev = error; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    double Ts = 0.01;
+    
+    // Proportional part
+        
+        double Kp = 0.1;
+        double u_k = Kp*error;
+
+    // Integral part
+        double Ki = 0.1;
+        error_integral = error_integral + error * Ts;
+        double u_i = Ki * error_integral;
+    
+    // Derivative part
+        double Kd = 10.1;
+        double error_derivative = (error - error_prev)/Ts;
+        double filtered_error_derivative = LowPassFilter.step(error_derivative);
+        double u_d = Kd * filtered_error_derivative;
+        error_prev = error;
+    // Sum all parts and return it
+    return u_k + u_i + u_d;
+}
+void SetMotor1(double motorValue){
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range. Motor value = error from the PID controller.
+    if (motorValue >=0) directionpin1=1;
+        else directionpin1=0;
+    if (fabs(motorValue)>1) pwmpin1 = 1;
+        else pwmpin1 = fabs(motorValue);
+}
+
+void SetMotor2(double motorValue){
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 2. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range
+    if (motorValue >=0) directionpin2=1;
+        else directionpin2=0;
+    if (fabs(motorValue)>1) pwmpin2 = 1;
+        else pwmpin2 = fabs(motorValue);
+}
+
+void do_state_movingX(double EMG1, double EMG2){
+    // EMG 1 and 2 are being read at 100 Hz, which we will use. Script
+    // checks whether it should move negative, positive or not at all.
+    
+    // When EMG1 is on, it should move in the -X direction.
+    if(EMG1==1){
+        
+        // Get the position of motor 1 and get the wanted position, where the
+        // difference is the error for our PID controllers.
+        AP1 = GetActualPosition1();
+        DesiredChange = IK(-X velocity);
+        RP1 = GetReferencePosition1(double AP1, DesiredChange);
+        pwm1 = double PID1(RP1-AP1);
+        SetMotor1(pwm1);
+
+        }
+    if(EMG2==1){
+        // Same for motor 2.
+        AP2 = GetActualPosition1();
+        DesiredChange = IK(+X velocity);
+        RP2 = GetReferencePosition1(double AP1, DesiredChange);
+        pwm2 = double PID2(RP2-AP2);    
+        SetMotor2(pwm2);
+}
+int main()
+{
+    while (true) 
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Oct 31 16:04:21 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file