Dustin Berendsen / Mbed 2 deprecated kinematics_control_v3

1st Revision of clean control code for project biorobotics

Dependencies:   Encoder FastPWM MODSERIAL Servo mbed

Fork of kinematics_control by Dustin Berendsen

Files at this revision

API Documentation at this revision

Revision 2:2c2c9d8238d4, committed 2017-10-24

Comitter:
DBerendsen
Date:
Tue Oct 24 08:21:51 2017 +0000
Parent:
1:570c0d599b9e
Commit message:
Nieuwe link

Changed in this revision

FastPWM.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 570c0d599b9e -r 2c2c9d8238d4 FastPWM.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Tue Oct 24 08:21:51 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/FastPWM/#c0b2265cff9c

diff -r 570c0d599b9e -r 2c2c9d8238d4 main.cpp
--- a/main.cpp	Fri Oct 20 09:32:21 2017 +0000
+++ b/main.cpp	Tue Oct 24 08:21:51 2017 +0000
@@ -53,7 +53,7 @@
 //------------------------------------------------------------------------------
 AnalogIn potmeter1(A3);
 AnalogIn potmeter2(A4);
-const float max_rangex = 500.0;
+const float max_rangex = 450.0;
 const float max_rangey = 300.0;
 const float x_offset = 156.15;
 const float y_offset = -76.97;
@@ -122,8 +122,8 @@
 DigitalIn ENC2A(D12);
 DigitalIn ENC2B(D13);
 Encoder encoder1(D13,D12);
-const float MOTOR1_KP = 50.0;
-const float MOTOR1_KI = 30.0;
+const float MOTOR1_KP = 40.0;
+const float MOTOR1_KI = 0.0;
 double m1_err_int = 0;
 const double motor1_gain = 2*PI;
 
@@ -132,7 +132,7 @@
     float reference_alpha = getreferencebeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2);
     float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
     float error_alpha = reference_alpha-position_alpha;
-    float magnitude1 = PI_controller(error_alpha, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int)/ motor1_gain;
+    float magnitude1 = PI_controller(error_alpha, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int)/ (motor1_gain);
     motor1 = fabs(magnitude1);
     pc.printf("err_a = %f  alpha = %f   mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
     pc.printf("\r\n");
@@ -161,8 +161,8 @@
 DigitalIn ENC1A(D10);
 DigitalIn ENC1B(D11);
 Encoder encoder2(D10,D11);
-const float MOTOR2_KP = 50.0;
-const float MOTOR2_KI = 30.0;
+const float MOTOR2_KP = 60.0;
+const float MOTOR2_KI = 0.0;
 double m2_err_int = 0;
 const double motor2_gain = 2*PI;
 
@@ -171,7 +171,7 @@
     float reference_beta = getreferencealpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2);
     float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
     float error_beta = reference_beta-position_beta;
-    float magnitude2 = PI_controller(error_beta, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int)/ motor2_gain;
+    float magnitude2 = PI_controller(error_beta, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int)/ (motor2_gain);
     motor2 = fabs(magnitude2);
     pc.printf("err2 = %f  beta = %f   mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
     pc.printf("\r\n");