Diff: main.cpp

Revision:

0:312f6a44fcac

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 30 17:21:03 2016 +0000
@@ -0,0 +1,68 @@
+#include "mbed.h"
+
+Serial pc(USBTX, USBRX); // tx, rx
+
+AnalogIn pot1( p19 );
+PwmOut servo( p21 );
+I2C compass( p28, p27);
+Ticker tick100hz;
+Ticker tick5hz;
+
+const int compass_addr = 0x42;
+char cmd[3];
+char compass_raw[2];
+float pos = 0;
+float ctrlval = 0;
+float PWM_zero = .000061;
+
+float north = 0.0;
+float err, error_int;
+float error_old = 0.0;
+float error_int_old = 0.0;
+float T = 0.01;
+
+float kp = 0.0002;
+float ki = 0.0;
+
+
+void s100hz_task(void) {
+    
+    compass.read (compass_addr, compass_raw, 2 );
+    pos = 0.1*((compass_raw[0]<<8) + compass_raw[1]) -90;
+    if (pos>180) {
+        pos = pos -360;
+         
+    }
+    ctrlval = pot1.read() * .05;
+    err = north-pos;
+    error_int = error_int_old + .5 * (err + error_old) * T ;
+    ctrlval = kp * err + ki * error_int;
+    ctrlval = ctrlval + PWM_zero;
+    error_old = err;
+    error_int_old = error_int;    
+    
+
+    
+    servo = ctrlval;    
+}
+
+void s5hz_task ( void ) {
+    
+    pc.printf ( "Position: %f, Control Duty Cycle: %f\n", pos, ctrlval );
+}
+
+int main() {
+    
+    cmd[0] = 0x47;
+    cmd[1] = 0x74;
+    cmd[2] = 0x72;
+    
+    compass.write ( compass_addr, cmd, 3 );
+    
+    tick100hz.attach ( &s100hz_task, .01 );
+    tick5hz.attach ( &s5hz_task, .2 );
+    
+    while (1) {
+        
+    }
+}