Diff: main.cpp

Revision:

0:7c80cce5d0d3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 15 17:40:27 2018 +0000
@@ -0,0 +1,59 @@
+#include "HMC5983.h"
+#include "mbed.h"
+#include "steppercontroller.h"
+
+
+Serial pc(USBTX, USBRX);
+
+I2C i2c(D14, D15);
+HMC5983 compass(i2c);
+
+Timer displayTimer;
+Timer stepperRelaxTimer;
+Timer compassPollTimer;
+int crtState, prvState;
+StepperController stepper(D7, D6, D5, D4);
+double desiredAngle, actualAngle;
+
+double angleDiff(double a, double b);
+
+int main()
+{
+    prvState = 0;
+    stepper.setSequenceType(StepperController::Interleaved);
+    displayTimer.start();
+    stepperRelaxTimer.start();
+    compassPollTimer.start();
+    stepper.setPulseWidth(0.2);
+    desiredAngle = 0.0;
+    while (1) {
+        if (compassPollTimer.read() > 1) {
+            compassPollTimer.reset();
+            actualAngle = compass.read();
+            
+            pc.printf("Compass: %f\n", actualAngle);
+            pc.printf("Difference: %f\n", angleDiff(actualAngle, desiredAngle));
+            if (abs(angleDiff(actualAngle, desiredAngle)) > 5) {
+                if (angleDiff(actualAngle, desiredAngle) > 0) {
+                    stepper.setDirection(StepperController::DirectionCCW);
+                    stepper.advance();        
+                }
+                else {
+                    stepper.setDirection(StepperController::DirectionCW);
+                    stepper.advance(); 
+                }    
+            }                 
+        }
+    }
+}
+
+double angleDiff(double a, double b)
+{
+    double diff = a - b;
+    
+    if (diff > 180)
+        diff -= 360;
+    if (diff < -180)
+        diff += 360;
+    return diff;
+}
\ No newline at end of file