message
Dependencies: Encoder FastPWM MODSERIAL Servo mbed
Fork of Angle_control_v3 by
main.cpp
- Committer:
- DBerendsen
- Date:
- 2017-10-19
- Revision:
- 6:6ae6256cf234
- Parent:
- 5:b4ec742aa7d4
File content as of revision 6:6ae6256cf234:
#include "mbed.h" #include "encoder.h" #include "Servo.h" #include "FastPWM.h" #include "MODSERIAL.h" MODSERIAL pc(USBTX, USBRX); Ticker MyControllerTicker1; Ticker MyControllerTicker2; const double PI = 3.141592653589793; const double RAD_PER_PULSE = 0.00074799825; const double CONTROLLER_TS = 0.01; //0.01 //Motor1 PwmOut motor1(D5); DigitalOut motor1DirectionPin(D4); DigitalIn ENC2A(D12); DigitalIn ENC2B(D13); Encoder encoder1(D13,D12); AnalogIn potmeter1(A3); const double MOTOR1_KP = 20; const double MOTOR1_KI = 10; double m1_err_int = 0; const double motor1_gain = 2*PI; //Motor2 PwmOut motor2(D6); DigitalOut motor2DirectionPin(D7); DigitalIn ENC1A(D10); DigitalIn ENC1B(D11); Encoder encoder2(D10,D11); AnalogIn potmeter2(A4); const double MOTOR2_KP = 20; const double MOTOR2_KI = 10; double m2_err_int = 0; const double motor2_gain = 2*PI; //________________________________________________________________ //Kinematica //Motor offsets (kinematica implementation) int max_rangex = 400; int max_rangey = 250; int L1 = 450; int L2 = 490; float alphaoffset = 0.577872; float betaoffset = -0.165529+0.577872; //21.52; float x_offset = 0.0;//165.07; float y_offset = 0.0;//106.37; float getreferencepositionx(double potmeter){ float x_target = potmeter * max_rangex; return x_target; } float getreferencepositiony(double potmeter){ float y_target = potmeter * max_rangey; return y_target; } float getreferenceanglealpha(const double PI, float x_offset, float y_offset, float alphaoffset){ float x = getreferencepositionx(potmeter1) + x_offset; float y = getreferencepositiony(potmeter2) - y_offset; float theta2 = acos( (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2) ); float theta1 = asin( (L2*sin(theta2)) / sqrt(x*x +y*y)) + atan(y/x); float alpha = (0.5*PI) - theta1 - alphaoffset; return alpha; } float getreferenceanglebeta(const double PI, float x_offset, float y_offset, float betaoffset, float alphaoffset){ float x = getreferencepositionx(potmeter1) + x_offset; float y = getreferencepositiony(potmeter2) - y_offset; float theta2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2)); float theta1 = asin( (L2*sin(theta2)) / sqrt(x*x +y*y)) + atan(y/x); float alpha = (0.5*PI) - theta1 - alphaoffset; float beta = PI - alpha - theta2 - betaoffset; return beta; } //________________________________________________________________ //Servo Servo MyServo(D9); InterruptIn But1(D8); int k=0; double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) { e_int =+ Ts * error; return Kp * error + Ki * e_int ; } void motor1_control(){ double referenceangle1 = getreferenceanglebeta(PI, x_offset, y_offset, betaoffset, alphaoffset); double position1 = RAD_PER_PULSE * encoder1.getPosition(); double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain; motor1 = fabs(magnitude1); if (magnitude1 < 0){ motor1DirectionPin = 1; } else{ motor1DirectionPin = 0; } } void motor2_control(){ double referenceangle2 = getreferenceanglealpha(PI, x_offset, y_offset, alphaoffset); double position2 = RAD_PER_PULSE * encoder2.getPosition(); double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain; motor2 = fabs(magnitude2); if (magnitude2 < 0){ motor2DirectionPin = 1; } else{ motor2DirectionPin = 0; } } /*void servo_control (){ motor1.period(0.02f); motor2.period(0.02f); if (k==0){ MyServo = 0; k=1; motor1.period_us(200); } else{ MyServo = 2; k=0; motor1.period_us(200); } }*/ int main(){ pc.baud(115200); pc.printf("Hello World!\r\n"); //But1.rise(&servo_control); motor1.period(0.0002f); motor2.period(0.0002f); MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); const double PI = 3.141592653589793; float x_offset = 165.07; float y_offset = 106.37; float alphaoffset = 0.577872; float betaoffset = -0.165529+0.577872; //21.52; float alpha = getreferenceanglealpha(PI, x_offset, y_offset, alphaoffset); pc.printf("a %f\n", alpha); float beta = getreferenceanglebeta(PI, x_offset, y_offset, betaoffset, alphaoffset); pc.printf("b %f\n", beta); while(1) {} }