message
Dependencies: Encoder FastPWM MODSERIAL Servo mbed
Fork of Angle_control_v3 by
Diff: main.cpp
- Revision:
- 3:1514725c8cc8
- Parent:
- 2:47ec66bbe8f9
- Child:
- 4:d385669b2f50
--- a/main.cpp Wed Oct 11 12:14:37 2017 +0000 +++ b/main.cpp Mon Oct 16 12:15:09 2017 +0000 @@ -1,11 +1,12 @@ #include "mbed.h" #include "encoder.h" #include "Servo.h" +#include "FastPWM.h" Ticker MyControllerTicker1; Ticker MyControllerTicker2; const double PI = 3.141592653589793; -const double RAD_PER_PULSE = 0.000476190; +const double RAD_PER_PULSE = 0.000749425; const double CONTROLLER_TS = 0.01; @@ -16,8 +17,8 @@ DigitalIn ENC2B(D13); Encoder encoder1(D13,D12); AnalogIn potmeter1(A3); -const double MOTOR1_KP = 0.5; -const double MOTOR1_KI = 1.5; +const double MOTOR1_KP = 15; +const double MOTOR1_KI = 10; double m1_err_int = 0; const double motor1_gain = 2*PI; @@ -29,22 +30,65 @@ DigitalIn ENC1B(D11); Encoder encoder2(D10,D11); AnalogIn potmeter2(A4); -const double MOTOR2_KP = 0.5; -const double MOTOR2_KI = 1.5; +const double MOTOR2_KP = 15; +const double MOTOR2_KI = 10; double m2_err_int = 0; const double motor2_gain = 2*PI; +//________________________________________________________________ +//Test kinematica + +//Motor offsets (kinematica implementation) +float alphaoffset = 10; +float betaoffset = 35; +float x_target, y_target; +float x, y; +int max_rangex = 800; +int max_rangey = 500; + +float L1 = 450; +float L2 = 490; +float x_offset = 0.0; +float y_offset = 0.0; + + +float getreferencepositionx(double potmeter){ + x_target = potmeter * max_rangex; + return x_target; +} +float getreferencepositiony(double potmeter){ + y_target = potmeter * max_rangey; + return y_target; +} + +float getreferenceangle(){ + x_target = getreferencepositionx(potmeter1); + y_target = getreferencepositiony(potmeter2); + float x = x_target - x_offset; + float y = y_target - y_offset; + float L3_ = sqrt(x*x + y*y); + float L3 = L3_; + + float beta = 3.1415 - acos(((L1*L1 + L2*L2 - L3*L3)/(2 * L1 * L2))); + beta = beta * 180 / 3.1415; + + float alpha = (acos((L1*L1+L3*L3-L2*L2)/(2*L1*L3))) *180 / 3.1415; + float beta_ = beta - alpha; +} + +//________________________________________________________________ + //Servo Servo MyServo(D9); InterruptIn But1(D8); int k=0; -float getreferenceangle(const double PI, double potmeter){ +/*float getreferenceangle(const double PI, double potmeter){ float max_angle = 2*PI; float reference_angle = max_angle * potmeter; return reference_angle; -} +}*/ double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) { e_int =+ Ts * error; @@ -80,31 +124,29 @@ } } -void servo_control (){ +/*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(){ - //motor1.period(0.01); + But1.rise(&servo_control); + motor1.period(0.0002f); + motor2.period(0.0002f); MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); - But1.rise(&servo_control); + while(1) {} -} - -//{while(1) -// motor1_control(motor1_gain); -// wait(0.005f); -// motor2_control(motor2_gain); -// wait(0.005f); - // } - \ No newline at end of file +} \ No newline at end of file