Diff: global.cpp

Revision:

3:dfb6733ae397

Parent:

2:2bc519e14bae

Child:

5:f655435d0782

--- a/global.cpp	Sun Mar 13 17:55:42 2016 +0000
+++ b/global.cpp	Mon Mar 14 00:40:28 2016 +0000
@@ -11,32 +11,42 @@
 DigitalOut ioReset(p12);
 
 // Comunication 
-PwmOut leftPwm(p21); 
-PwmOut rightPwm(p22); 
 SPI deSpi(p5, p6, p7);
+Pixy pixy(Pixy::SPI, p11, p12, p13);
 Serial pc(USBTX, USBRX); // PC serial channel
 Serial bt(p9, p10); // Bluetooth serial channel
 
+// Other
+PwmOut leftPwm(p21); 
+PwmOut rightPwm(p22); 
+InterruptIn bumper(p8);
+
 // Generic PI controller function used by the sensor control thread
-void PI(float *xState, float *u, float setPoint, float kP, float kI, float feedback, float bound) {
-    float e;
+void PI(float error, float *output, float *integral, float kP, float kI, float bound) {
     
-    e = setPoint - feedback; 
-
     // Avoid integrator wind up
-    if((*u>=bound)||(*u<=-bound)) ; // turns off integrator when u maxed
+    if((*output >= bound)||(*output <= -bound));
     else {
-        *xState = *xState + e;
+        *integral = *integral + error;
     }
 
-    *u = kI*(*xState) + kP*e;
+    *output = kI * (*integral) + kP * error;
     
-    if (fabs(*u) > bound) {
-        *u = bound;
-    }
+    // 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(int counts, int time) {
-    return (counts*122.62)/time;
+float QE2RadsPerSec(short counts, short time) {  
+    int cnt32;   
+    if (counts & 0x00008000) {
+        cnt32 = counts | 0xFFFF0000;
+    } else {
+        cnt32 = counts;
+    }   
+    return (cnt32*122.62)/time;
 }
\ No newline at end of file