![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Angle control and Servo control with kinematics
Dependencies: Encoder FastPWM Servo mbed
Fork of Angle_control_v2 by
Revision 5:b4ec742aa7d4, committed 2017-10-17
- Comitter:
- peterknoben
- Date:
- Tue Oct 17 10:09:51 2017 +0000
- Parent:
- 4:d385669b2f50
- Commit message:
- Inclusief kinematica (nog niet getest)
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r d385669b2f50 -r b4ec742aa7d4 main.cpp --- a/main.cpp Tue Oct 17 09:31:20 2017 +0000 +++ b/main.cpp Tue Oct 17 10:09:51 2017 +0000 @@ -35,26 +35,63 @@ double m2_err_int = 0; const double motor2_gain = 2*PI; +//________________________________________________________________ +//Kinematica + +//Motor offsets (kinematica implementation) +int max_rangex = 800; +int max_rangey = 500; +int L1 = 450; +int L2 = 490; +float alphaoffset = 10; +float betaoffset = 35; +float x_offset = 0.0; +float y_offset = 0.0; + + +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 = getreferencepositionx(potmeter1); + float y = getreferencepositiony(potmeter2); + 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 = (05.*PI) - theta1; + return alpha; +} + +float getreferenceanglebeta(const double PI){ + float x = getreferencepositionx(potmeter1); + float y = getreferencepositiony(potmeter2); + 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 = (05.*PI) - theta1; + float beta = PI - alpha - theta2; + return beta; +} + +//________________________________________________________________ + //Servo Servo MyServo(D9); InterruptIn But1(D8); int k=0; - -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; return Kp * error + Ki * e_int ; } void motor1_control(){ - double referenceangle1 = getreferenceangle(PI, potmeter1); + double referenceangle1 = getreferenceanglealpha(PI); 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); @@ -69,7 +106,7 @@ void motor2_control(){ - double referenceangle2 = getreferenceangle(PI, potmeter2); + double referenceangle2 = getreferenceanglebeta(PI); 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); @@ -82,7 +119,7 @@ } } -void servo_control (){ +/*void servo_control (){ motor1.period(0.02f); motor2.period(0.02f); if (k==0){ @@ -95,11 +132,11 @@ k=0; motor1.period_us(200); } -} +}*/ int main(){ - But1.rise(&servo_control); + //But1.rise(&servo_control); motor1.period(0.0002f); motor2.period(0.0002f); MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);