Eki Liptiay / Mbed 2 deprecated do_state_movingX

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:5502fbcdba50, 2018-10-31 (annotated)

Committer:
ekiliptiay
Date:
Wed Oct 31 16:04:21 2018 +0000
Revision:
0:5502fbcdba50
MovingX;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ekiliptiay 0:5502fbcdba50 1 #include "mbed.h"
ekiliptiay 0:5502fbcdba50 2
ekiliptiay 0:5502fbcdba50 3 double GetActualPosition1(){
ekiliptiay 0:5502fbcdba50 4 // Gets the actual position from motor 1 in rad. The difference with the
ekiliptiay 0:5502fbcdba50 5 // reference position is the error we are going to use in the PID-control.
ekiliptiay 0:5502fbcdba50 6 double ActualPosition = Encoder1.getPulses()/4200*2*3.14159265358979;
ekiliptiay 0:5502fbcdba50 7 return ActualPosition;
ekiliptiay 0:5502fbcdba50 8 }
ekiliptiay 0:5502fbcdba50 9
ekiliptiay 0:5502fbcdba50 10 double GetReferencePosition1(double ActualPosition, double DesiredChange){
ekiliptiay 0:5502fbcdba50 11 ReferencePosition1 = ActualPosition + DesiredChange; // Gives the position it should be in.
ekiliptiay 0:5502fbcdba50 12 return ReferencePosition1;
ekiliptiay 0:5502fbcdba50 13 }
ekiliptiay 0:5502fbcdba50 14
ekiliptiay 0:5502fbcdba50 15 double GetReferencePosition2(double ActualPosition, double DesiredChange){
ekiliptiay 0:5502fbcdba50 16 MotAng2 = IK();
ekiliptiay 0:5502fbcdba50 17 ReferencePosition2 = ActualPosition + DesiredChange; // Gives the position it should be in.
ekiliptiay 0:5502fbcdba50 18 return ReferencePosition2;
ekiliptiay 0:5502fbcdba50 19 }
ekiliptiay 0:5502fbcdba50 20
ekiliptiay 0:5502fbcdba50 21 double GetActualPosition2(){
ekiliptiay 0:5502fbcdba50 22 double ActualPosition = Encoder2.getPulses()/4200*2*3.14159265358979;
ekiliptiay 0:5502fbcdba50 23 return ActualPosition;
ekiliptiay 0:5502fbcdba50 24 }
ekiliptiay 0:5502fbcdba50 25
ekiliptiay 0:5502fbcdba50 26 double PID_controller1(double error){
ekiliptiay 0:5502fbcdba50 27 static double error_integral = 0;
ekiliptiay 0:5502fbcdba50 28 static double error_prev = error; // initialization with this value only done once!
ekiliptiay 0:5502fbcdba50 29 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
ekiliptiay 0:5502fbcdba50 30 double Ts = 0.01;
ekiliptiay 0:5502fbcdba50 31
ekiliptiay 0:5502fbcdba50 32 // Proportional part
ekiliptiay 0:5502fbcdba50 33
ekiliptiay 0:5502fbcdba50 34 double Kp = 0.1;
ekiliptiay 0:5502fbcdba50 35 double u_k = Kp*error;
ekiliptiay 0:5502fbcdba50 36
ekiliptiay 0:5502fbcdba50 37 // Integral part
ekiliptiay 0:5502fbcdba50 38 double Ki = 0.1;
ekiliptiay 0:5502fbcdba50 39 error_integral = error_integral + error * Ts;
ekiliptiay 0:5502fbcdba50 40 double u_i = Ki * error_integral;
ekiliptiay 0:5502fbcdba50 41
ekiliptiay 0:5502fbcdba50 42 // Derivative part
ekiliptiay 0:5502fbcdba50 43 double Kd = 10.1;
ekiliptiay 0:5502fbcdba50 44 double error_derivative = (error - error_prev)/Ts;
ekiliptiay 0:5502fbcdba50 45 double filtered_error_derivative = LowPassFilter.step(error_derivative);
ekiliptiay 0:5502fbcdba50 46 double u_d = Kd * filtered_error_derivative;
ekiliptiay 0:5502fbcdba50 47 error_prev = error;
ekiliptiay 0:5502fbcdba50 48 // Sum all parts and return it
ekiliptiay 0:5502fbcdba50 49 return u_k + u_i + u_d;
ekiliptiay 0:5502fbcdba50 50 }
ekiliptiay 0:5502fbcdba50 51
ekiliptiay 0:5502fbcdba50 52 // Controller for motor 2
ekiliptiay 0:5502fbcdba50 53 double PID_controller2(double error){
ekiliptiay 0:5502fbcdba50 54 static double error_integral = 0;
ekiliptiay 0:5502fbcdba50 55 static double error_prev = error; // initialization with this value only done once!
ekiliptiay 0:5502fbcdba50 56 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
ekiliptiay 0:5502fbcdba50 57 double Ts = 0.01;
ekiliptiay 0:5502fbcdba50 58
ekiliptiay 0:5502fbcdba50 59 // Proportional part
ekiliptiay 0:5502fbcdba50 60
ekiliptiay 0:5502fbcdba50 61 double Kp = 0.1;
ekiliptiay 0:5502fbcdba50 62 double u_k = Kp*error;
ekiliptiay 0:5502fbcdba50 63
ekiliptiay 0:5502fbcdba50 64 // Integral part
ekiliptiay 0:5502fbcdba50 65 double Ki = 0.1;
ekiliptiay 0:5502fbcdba50 66 error_integral = error_integral + error * Ts;
ekiliptiay 0:5502fbcdba50 67 double u_i = Ki * error_integral;
ekiliptiay 0:5502fbcdba50 68
ekiliptiay 0:5502fbcdba50 69 // Derivative part
ekiliptiay 0:5502fbcdba50 70 double Kd = 10.1;
ekiliptiay 0:5502fbcdba50 71 double error_derivative = (error - error_prev)/Ts;
ekiliptiay 0:5502fbcdba50 72 double filtered_error_derivative = LowPassFilter.step(error_derivative);
ekiliptiay 0:5502fbcdba50 73 double u_d = Kd * filtered_error_derivative;
ekiliptiay 0:5502fbcdba50 74 error_prev = error;
ekiliptiay 0:5502fbcdba50 75 // Sum all parts and return it
ekiliptiay 0:5502fbcdba50 76 return u_k + u_i + u_d;
ekiliptiay 0:5502fbcdba50 77 }
ekiliptiay 0:5502fbcdba50 78 void SetMotor1(double motorValue){
ekiliptiay 0:5502fbcdba50 79 // Given -1<=motorValue<=1, this sets the PWM and direction
ekiliptiay 0:5502fbcdba50 80 // bits for motor 1. Positive value makes motor rotating
ekiliptiay 0:5502fbcdba50 81 // clockwise. motorValues outside range are truncated to
ekiliptiay 0:5502fbcdba50 82 // within range. Motor value = error from the PID controller.
ekiliptiay 0:5502fbcdba50 83 if (motorValue >=0) directionpin1=1;
ekiliptiay 0:5502fbcdba50 84 else directionpin1=0;
ekiliptiay 0:5502fbcdba50 85 if (fabs(motorValue)>1) pwmpin1 = 1;
ekiliptiay 0:5502fbcdba50 86 else pwmpin1 = fabs(motorValue);
ekiliptiay 0:5502fbcdba50 87 }
ekiliptiay 0:5502fbcdba50 88
ekiliptiay 0:5502fbcdba50 89 void SetMotor2(double motorValue){
ekiliptiay 0:5502fbcdba50 90 // Given -1<=motorValue<=1, this sets the PWM and direction
ekiliptiay 0:5502fbcdba50 91 // bits for motor 2. Positive value makes motor rotating
ekiliptiay 0:5502fbcdba50 92 // clockwise. motorValues outside range are truncated to
ekiliptiay 0:5502fbcdba50 93 // within range
ekiliptiay 0:5502fbcdba50 94 if (motorValue >=0) directionpin2=1;
ekiliptiay 0:5502fbcdba50 95 else directionpin2=0;
ekiliptiay 0:5502fbcdba50 96 if (fabs(motorValue)>1) pwmpin2 = 1;
ekiliptiay 0:5502fbcdba50 97 else pwmpin2 = fabs(motorValue);
ekiliptiay 0:5502fbcdba50 98 }
ekiliptiay 0:5502fbcdba50 99
ekiliptiay 0:5502fbcdba50 100 void do_state_movingX(double EMG1, double EMG2){
ekiliptiay 0:5502fbcdba50 101 // EMG 1 and 2 are being read at 100 Hz, which we will use. Script
ekiliptiay 0:5502fbcdba50 102 // checks whether it should move negative, positive or not at all.
ekiliptiay 0:5502fbcdba50 103
ekiliptiay 0:5502fbcdba50 104 // When EMG1 is on, it should move in the -X direction.
ekiliptiay 0:5502fbcdba50 105 if(EMG1==1){
ekiliptiay 0:5502fbcdba50 106
ekiliptiay 0:5502fbcdba50 107 // Get the position of motor 1 and get the wanted position, where the
ekiliptiay 0:5502fbcdba50 108 // difference is the error for our PID controllers.
ekiliptiay 0:5502fbcdba50 109 AP1 = GetActualPosition1();
ekiliptiay 0:5502fbcdba50 110 DesiredChange = IK(-X velocity);
ekiliptiay 0:5502fbcdba50 111 RP1 = GetReferencePosition1(double AP1, DesiredChange);
ekiliptiay 0:5502fbcdba50 112 pwm1 = double PID1(RP1-AP1);
ekiliptiay 0:5502fbcdba50 113 SetMotor1(pwm1);
ekiliptiay 0:5502fbcdba50 114
ekiliptiay 0:5502fbcdba50 115 }
ekiliptiay 0:5502fbcdba50 116 if(EMG2==1){
ekiliptiay 0:5502fbcdba50 117 // Same for motor 2.
ekiliptiay 0:5502fbcdba50 118 AP2 = GetActualPosition1();
ekiliptiay 0:5502fbcdba50 119 DesiredChange = IK(+X velocity);
ekiliptiay 0:5502fbcdba50 120 RP2 = GetReferencePosition1(double AP1, DesiredChange);
ekiliptiay 0:5502fbcdba50 121 pwm2 = double PID2(RP2-AP2);
ekiliptiay 0:5502fbcdba50 122 SetMotor2(pwm2);
ekiliptiay 0:5502fbcdba50 123 }
ekiliptiay 0:5502fbcdba50 124 int main()
ekiliptiay 0:5502fbcdba50 125 {
ekiliptiay 0:5502fbcdba50 126 while (true)
ekiliptiay 0:5502fbcdba50 127 }
ekiliptiay 0:5502fbcdba50 128 }