main.cpp

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