message
Dependencies: Encoder FastPWM MODSERIAL Servo mbed
Fork of Angle_control_v3 by
Diff: main.cpp
- Revision:
- 4:d385669b2f50
- Parent:
- 3:1514725c8cc8
- Child:
- 5:b4ec742aa7d4
--- a/main.cpp Mon Oct 16 12:15:09 2017 +0000 +++ b/main.cpp Tue Oct 17 09:31:20 2017 +0000 @@ -35,48 +35,6 @@ 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); @@ -84,11 +42,11 @@ 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; @@ -124,7 +82,7 @@ } } -/*void servo_control (){ +void servo_control (){ motor1.period(0.02f); motor2.period(0.02f); if (k==0){ @@ -137,7 +95,7 @@ k=0; motor1.period_us(200); } -}*/ +} int main(){