Diff: main.cpp

Revision:

2:2c2c9d8238d4

Parent:

1:570c0d599b9e

--- 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");