A compass demo using HMC5983
Dependencies: HMC5983 StepperController mbed
main.cpp
- Committer:
- acracan
- Date:
- 2018-06-15
- Revision:
- 0:7c80cce5d0d3
File content as of revision 0:7c80cce5d0d3:
#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; }