main.cpp

00001 #include "mbed.h"
00002 #include "encoder.h"
00003 #include "Servo.h"
00004 #include "FastPWM.h"
00005 
00006 Ticker MyControllerTicker1;
00007 Ticker MyControllerTicker2;
00008 const double PI = 3.141592653589793;
00009 const double RAD_PER_PULSE = 0.000749425;
00010 const double CONTROLLER_TS = 0.01;
00011 
00012 
00013 //Motor1
00014 PwmOut motor1(D5);
00015 DigitalOut motor1DirectionPin(D4);
00016 DigitalIn ENC2A(D12);
00017 DigitalIn ENC2B(D13);
00018 Encoder encoder1(D13,D12);
00019 AnalogIn potmeter1(A3);
00020 const double MOTOR1_KP = 15;
00021 const double MOTOR1_KI = 10;
00022 double m1_err_int = 0;
00023 const double motor1_gain = 2*PI;
00024 
00025 
00026 //Motor2
00027 PwmOut motor2(D6);
00028 DigitalOut motor2DirectionPin(D7);
00029 DigitalIn ENC1A(D10);
00030 DigitalIn ENC1B(D11);
00031 Encoder encoder2(D10,D11);
00032 AnalogIn potmeter2(A4);
00033 const double MOTOR2_KP = 15;
00034 const double MOTOR2_KI = 10;
00035 double m2_err_int = 0;
00036 const double motor2_gain = 2*PI;
00037 
00038 //________________________________________________________________
00039 //Kinematica
00040 
00041 //Motor offsets (kinematica implementation)
00042 int max_rangex = 800;
00043 int max_rangey = 500;
00044 int L1 = 450;
00045 int L2 = 490;
00046 float alphaoffset = 10;
00047 float betaoffset = 35;
00048 float x_offset = 0.0;
00049 float y_offset = 0.0;
00050 
00051 
00052 float getreferencepositionx(double potmeter){
00053     float x_target = potmeter * max_rangex;
00054     return x_target;
00055 }
00056 float getreferencepositiony(double potmeter){
00057     float y_target = potmeter * max_rangey;
00058     return y_target;
00059 }
00060 
00061 float getreferenceanglealpha(const double PI){
00062     float x = getreferencepositionx(potmeter1);
00063     float y = getreferencepositiony(potmeter2);
00064     float theta2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
00065     float theta1 = asin(L2*sin(theta2)/sqrt(x*x +y*y) + atan(y/x));
00066     float alpha = (05.*PI) - theta1;
00067     return alpha;
00068 }
00069 
00070 float getreferenceanglebeta(const double PI){
00071     float x = getreferencepositionx(potmeter1);
00072     float y = getreferencepositiony(potmeter2);
00073     float theta2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
00074     float theta1 = asin(L2*sin(theta2)/sqrt(x*x +y*y) + atan(y/x));
00075     float alpha = (05.*PI) - theta1;
00076     float beta = PI - alpha - theta2;
00077     return beta;
00078 }     
00079 
00080 //________________________________________________________________
00081 
00082 
00083 //Servo
00084 Servo MyServo(D9);
00085 InterruptIn But1(D8);
00086 int k=0;
00087 
00088 double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) {
00089     e_int =+ Ts * error;
00090     return Kp * error + Ki * e_int ;
00091 }
00092 
00093 void motor1_control(){
00094     double referenceangle1 = getreferenceanglealpha(PI);
00095     double position1 = RAD_PER_PULSE * encoder1.getPosition();
00096     double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
00097     motor1 = fabs(magnitude1);
00098     
00099     if (magnitude1 < 0){
00100         motor1DirectionPin = 1;
00101     }
00102     else{
00103         motor1DirectionPin = 0;
00104     }
00105 }
00106 
00107 
00108 void motor2_control(){
00109     double referenceangle2 = getreferenceanglebeta(PI);
00110     double position2 = RAD_PER_PULSE * encoder2.getPosition();
00111     double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
00112     motor2 = fabs(magnitude2);
00113     
00114     if (magnitude2 < 0){
00115         motor2DirectionPin = 1;
00116     }
00117     else{
00118         motor2DirectionPin = 0;
00119     }
00120 }
00121 
00122 /*void servo_control (){
00123     motor1.period(0.02f);
00124     motor2.period(0.02f);
00125     if (k==0){
00126         MyServo = 0;
00127         k=1;
00128         motor1.period_us(200);
00129     }
00130     else{
00131         MyServo = 2;
00132         k=0;
00133         motor1.period_us(200);
00134     }        
00135 }*/
00136 
00137 
00138 int main(){
00139     //But1.rise(&servo_control);
00140     motor1.period(0.0002f);
00141     motor2.period(0.0002f);
00142     MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
00143     MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
00144     
00145        
00146     while(1) {}   
00147 }