Angle control and Servo control with kinematics
Dependencies: Encoder FastPWM Servo mbed
Fork of Angle_control_v2 by
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 }
Generated on Sat Jul 23 2022 20:04:08 by 1.7.2