main.cpp

00001 #include "mbed.h"
00002     
00003 double GetActualPosition1(){
00004     // Gets the actual position from motor 1 in rad. The difference with the
00005     // reference position is the error we are going to use in  the PID-control.
00006     double ActualPosition = Encoder1.getPulses()/4200*2*3.14159265358979; 
00007     return ActualPosition;
00008 }
00009 
00010 double GetReferencePosition1(double ActualPosition, double DesiredChange){
00011         ReferencePosition1 = ActualPosition + DesiredChange; // Gives the position it should be in.
00012     return ReferencePosition1;
00013 }
00014 
00015 double GetReferencePosition2(double ActualPosition, double DesiredChange){
00016     MotAng2 = IK();
00017     ReferencePosition2 = ActualPosition + DesiredChange; // Gives the position it should be in.
00018     return ReferencePosition2;
00019 }
00020     
00021 double GetActualPosition2(){
00022     double ActualPosition = Encoder2.getPulses()/4200*2*3.14159265358979;
00023     return ActualPosition;
00024 }
00025 
00026 double PID_controller1(double error){
00027     static double error_integral = 0;
00028     static double error_prev = error; // initialization with this value only done once!
00029     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
00030     double Ts = 0.01;
00031     
00032     // Proportional part
00033         
00034         double Kp = 0.1;
00035         double u_k = Kp*error;
00036 
00037     // Integral part
00038         double Ki = 0.1;
00039         error_integral = error_integral + error * Ts;
00040         double u_i = Ki * error_integral;
00041     
00042     // Derivative part
00043         double Kd = 10.1;
00044         double error_derivative = (error - error_prev)/Ts;
00045         double filtered_error_derivative = LowPassFilter.step(error_derivative);
00046         double u_d = Kd * filtered_error_derivative;
00047         error_prev = error;
00048     // Sum all parts and return it
00049     return u_k + u_i + u_d;
00050 }
00051 
00052 // Controller for motor 2
00053 double PID_controller2(double error){
00054     static double error_integral = 0;
00055     static double error_prev = error; // initialization with this value only done once!
00056     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
00057     double Ts = 0.01;
00058     
00059     // Proportional part
00060         
00061         double Kp = 0.1;
00062         double u_k = Kp*error;
00063 
00064     // Integral part
00065         double Ki = 0.1;
00066         error_integral = error_integral + error * Ts;
00067         double u_i = Ki * error_integral;
00068     
00069     // Derivative part
00070         double Kd = 10.1;
00071         double error_derivative = (error - error_prev)/Ts;
00072         double filtered_error_derivative = LowPassFilter.step(error_derivative);
00073         double u_d = Kd * filtered_error_derivative;
00074         error_prev = error;
00075     // Sum all parts and return it
00076     return u_k + u_i + u_d;
00077 }
00078 void SetMotor1(double motorValue){
00079     // Given -1<=motorValue<=1, this sets the PWM and direction
00080     // bits for motor 1. Positive value makes motor rotating
00081     // clockwise. motorValues outside range are truncated to
00082     // within range. Motor value = error from the PID controller.
00083     if (motorValue >=0) directionpin1=1;
00084         else directionpin1=0;
00085     if (fabs(motorValue)>1) pwmpin1 = 1;
00086         else pwmpin1 = fabs(motorValue);
00087 }
00088 
00089 void SetMotor2(double motorValue){
00090     // Given -1<=motorValue<=1, this sets the PWM and direction
00091     // bits for motor 2. Positive value makes motor rotating
00092     // clockwise. motorValues outside range are truncated to
00093     // within range
00094     if (motorValue >=0) directionpin2=1;
00095         else directionpin2=0;
00096     if (fabs(motorValue)>1) pwmpin2 = 1;
00097         else pwmpin2 = fabs(motorValue);
00098 }
00099 
00100 void do_state_movingX(double EMG1, double EMG2){
00101     // EMG 1 and 2 are being read at 100 Hz, which we will use. Script
00102     // checks whether it should move negative, positive or not at all.
00103     
00104     // When EMG1 is on, it should move in the -X direction.
00105     if(EMG1==1){
00106         
00107         // Get the position of motor 1 and get the wanted position, where the
00108         // difference is the error for our PID controllers.
00109         AP1 = GetActualPosition1();
00110         DesiredChange = IK(-X velocity);
00111         RP1 = GetReferencePosition1(double AP1, DesiredChange);
00112         pwm1 = double PID1(RP1-AP1);
00113         SetMotor1(pwm1);
00114 
00115         }
00116     if(EMG2==1){
00117         // Same for motor 2.
00118         AP2 = GetActualPosition1();
00119         DesiredChange = IK(+X velocity);
00120         RP2 = GetReferencePosition1(double AP1, DesiredChange);
00121         pwm2 = double PID2(RP2-AP2);    
00122         SetMotor2(pwm2);
00123 }
00124 int main()
00125 {
00126     while (true) 
00127     }
00128 }