Arcadie Cracan / Mbed 2 deprecated compassDemo

A compass demo using HMC5983

Dependencies:   HMC5983 StepperController mbed

main.cpp@0:7c80cce5d0d3, 2018-06-15 (annotated)

Committer:
acracan
Date:
Fri Jun 15 17:40:27 2018 +0000
Revision:
0:7c80cce5d0d3
A demo with HMC5983

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acracan 0:7c80cce5d0d3 1 #include "HMC5983.h"
acracan 0:7c80cce5d0d3 2 #include "mbed.h"
acracan 0:7c80cce5d0d3 3 #include "steppercontroller.h"
acracan 0:7c80cce5d0d3 4
acracan 0:7c80cce5d0d3 5
acracan 0:7c80cce5d0d3 6 Serial pc(USBTX, USBRX);
acracan 0:7c80cce5d0d3 7
acracan 0:7c80cce5d0d3 8 I2C i2c(D14, D15);
acracan 0:7c80cce5d0d3 9 HMC5983 compass(i2c);
acracan 0:7c80cce5d0d3 10
acracan 0:7c80cce5d0d3 11 Timer displayTimer;
acracan 0:7c80cce5d0d3 12 Timer stepperRelaxTimer;
acracan 0:7c80cce5d0d3 13 Timer compassPollTimer;
acracan 0:7c80cce5d0d3 14 int crtState, prvState;
acracan 0:7c80cce5d0d3 15 StepperController stepper(D7, D6, D5, D4);
acracan 0:7c80cce5d0d3 16 double desiredAngle, actualAngle;
acracan 0:7c80cce5d0d3 17
acracan 0:7c80cce5d0d3 18 double angleDiff(double a, double b);
acracan 0:7c80cce5d0d3 19
acracan 0:7c80cce5d0d3 20 int main()
acracan 0:7c80cce5d0d3 21 {
acracan 0:7c80cce5d0d3 22 prvState = 0;
acracan 0:7c80cce5d0d3 23 stepper.setSequenceType(StepperController::Interleaved);
acracan 0:7c80cce5d0d3 24 displayTimer.start();
acracan 0:7c80cce5d0d3 25 stepperRelaxTimer.start();
acracan 0:7c80cce5d0d3 26 compassPollTimer.start();
acracan 0:7c80cce5d0d3 27 stepper.setPulseWidth(0.2);
acracan 0:7c80cce5d0d3 28 desiredAngle = 0.0;
acracan 0:7c80cce5d0d3 29 while (1) {
acracan 0:7c80cce5d0d3 30 if (compassPollTimer.read() > 1) {
acracan 0:7c80cce5d0d3 31 compassPollTimer.reset();
acracan 0:7c80cce5d0d3 32 actualAngle = compass.read();
acracan 0:7c80cce5d0d3 33
acracan 0:7c80cce5d0d3 34 pc.printf("Compass: %f\n", actualAngle);
acracan 0:7c80cce5d0d3 35 pc.printf("Difference: %f\n", angleDiff(actualAngle, desiredAngle));
acracan 0:7c80cce5d0d3 36 if (abs(angleDiff(actualAngle, desiredAngle)) > 5) {
acracan 0:7c80cce5d0d3 37 if (angleDiff(actualAngle, desiredAngle) > 0) {
acracan 0:7c80cce5d0d3 38 stepper.setDirection(StepperController::DirectionCCW);
acracan 0:7c80cce5d0d3 39 stepper.advance();
acracan 0:7c80cce5d0d3 40 }
acracan 0:7c80cce5d0d3 41 else {
acracan 0:7c80cce5d0d3 42 stepper.setDirection(StepperController::DirectionCW);
acracan 0:7c80cce5d0d3 43 stepper.advance();
acracan 0:7c80cce5d0d3 44 }
acracan 0:7c80cce5d0d3 45 }
acracan 0:7c80cce5d0d3 46 }
acracan 0:7c80cce5d0d3 47 }
acracan 0:7c80cce5d0d3 48 }
acracan 0:7c80cce5d0d3 49
acracan 0:7c80cce5d0d3 50 double angleDiff(double a, double b)
acracan 0:7c80cce5d0d3 51 {
acracan 0:7c80cce5d0d3 52 double diff = a - b;
acracan 0:7c80cce5d0d3 53
acracan 0:7c80cce5d0d3 54 if (diff > 180)
acracan 0:7c80cce5d0d3 55 diff -= 360;
acracan 0:7c80cce5d0d3 56 if (diff < -180)
acracan 0:7c80cce5d0d3 57 diff += 360;
acracan 0:7c80cce5d0d3 58 return diff;
acracan 0:7c80cce5d0d3 59 }