Diff: global.cpp

Revision:

5:f655435d0782

Parent:

3:dfb6733ae397

Child:

6:52686c25e4af

--- a/global.cpp	Fri Mar 18 17:05:22 2016 +0000
+++ b/global.cpp	Fri Mar 25 13:36:14 2016 +0000
@@ -16,30 +16,17 @@
 Serial pc(USBTX, USBRX); // PC serial channel
 Serial bt(p9, p10); // Bluetooth serial channel
 
+// Control
+PI leftMotorPi(MOTOR_PERIOD, MOTOR_KP, MOTOR_KI);
+PI rightMotorPi(MOTOR_PERIOD, MOTOR_KP, MOTOR_KI);
+PI targetHeightPi(NAVIGATION_PERIOD, SPEED_KP SPEED_KI);
+PI targetXPi(NAVIGATION_PERIOD, STEERING_KP, STEERING_KI);
+
 // Other
 PwmOut leftPwm(p21); 
 PwmOut rightPwm(p22); 
 InterruptIn bumper(p8);
 
-// Generic PI controller function used by the sensor control thread
-void PI(float error, float *output, float *integral, float kP, float kI, float bound) {
-    
-    // Avoid integrator wind up
-    if((*output >= bound)||(*output <= -bound));
-    else {
-        *integral = *integral + error;
-    }
-
-    *output = kI * (*integral) + kP * error;
-    
-    // Limit output to bounds
-    if (*output > bound) {
-        *output = bound;
-    } else if (*output < -bound) {
-        *output = -bound;   
-    } 
-}
-
 // Converts measurements from the QE2 to rads/sec
 float QE2RadsPerSec(short counts, short time) {  
     int cnt32;