Diff: global.cpp

Revision:

2:2bc519e14bae

Child:

3:dfb6733ae397

diff -r cc5636894b95 -r 2bc519e14bae global.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/global.cpp	Sun Mar 13 17:55:42 2016 +0000
@@ -0,0 +1,42 @@
+#include "global.h"
+
+// IO Port
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+DigitalOut leftDir(p29);
+DigitalOut rightDir(p30);
+DigitalOut spiReset(p11);
+DigitalOut ioReset(p12);
+
+// Comunication 
+PwmOut leftPwm(p21); 
+PwmOut rightPwm(p22); 
+SPI deSpi(p5, p6, p7);
+Serial pc(USBTX, USBRX); // PC serial channel
+Serial bt(p9, p10); // Bluetooth serial channel
+
+// 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;
+    
+    e = setPoint - feedback; 
+
+    // Avoid integrator wind up
+    if((*u>=bound)||(*u<=-bound)) ; // turns off integrator when u maxed
+    else {
+        *xState = *xState + e;
+    }
+
+    *u = kI*(*xState) + kP*e;
+    
+    if (fabs(*u) > bound) {
+        *u = bound;
+    }
+}
+
+// Converts measurements from the QE2 to rads/sec
+float QE2RadsPerSec(int counts, int time) {
+    return (counts*122.62)/time;
+}
\ No newline at end of file