CROTUS / Mbed 2 deprecated ProjetCasque

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed CROTUS_XBee mbed-rtos Crotus_Com

main.cpp@3:39b24d902aa7, 2017-04-02 (annotated)

Committer:
libv2001
Date:
Sun Apr 02 14:25:56 2017 +0000
Revision:
3:39b24d902aa7
Parent:
1:3591e7df4ff4
Child:
4:b6d8445792cc
State Machine impl?ment?e

Who changed what in which revision?

UserRevisionLine numberNew contents of line
libv2001 0:28d5622d1a3e 1 #include "mbed.h"
libv2001 0:28d5622d1a3e 2 #include "Magneto.h"
libv2001 0:28d5622d1a3e 3 #include "Acc.h"
libv2001 0:28d5622d1a3e 4
libv2001 0:28d5622d1a3e 5 DigitalOut myled(LED1);
libv2001 0:28d5622d1a3e 6
libv2001 0:28d5622d1a3e 7 I2C i2c(p28, p27);
libv2001 0:28d5622d1a3e 8
libv2001 0:28d5622d1a3e 9 Magneto magneto(i2c);
libv2001 0:28d5622d1a3e 10 Acc acc(i2c);
libv2001 0:28d5622d1a3e 11
libv2001 3:39b24d902aa7 12 InterruptIn calibrateOrientation(p13);
libv2001 3:39b24d902aa7 13
libv2001 0:28d5622d1a3e 14 Serial pc(USBTX, USBRX);
libv2001 0:28d5622d1a3e 15
libv2001 3:39b24d902aa7 16 #define ABS(a) ((a)<0 ? -(a) : (a))
libv2001 3:39b24d902aa7 17
libv2001 3:39b24d902aa7 18 enum INCLINATION_STATE {IDLE_INC = 0, SLOW = 1, FAST = 2};
libv2001 3:39b24d902aa7 19 enum DIRECTION_STATE {FORWARD = 0, BACKWARD = 1};
libv2001 3:39b24d902aa7 20 enum ORIENTATION_STATE {IDLE_OR = 0, LEFT = 1, RIGHT = 2};
libv2001 3:39b24d902aa7 21
libv2001 3:39b24d902aa7 22 struct CurrentState_t {
libv2001 3:39b24d902aa7 23 bool emergencyStop;
libv2001 3:39b24d902aa7 24 INCLINATION_STATE inclinationState;
libv2001 3:39b24d902aa7 25 DIRECTION_STATE directionState;
libv2001 3:39b24d902aa7 26 ORIENTATION_STATE orientationState;
libv2001 3:39b24d902aa7 27 uint16_t forwardOrientation;
libv2001 3:39b24d902aa7 28
libv2001 3:39b24d902aa7 29 CurrentState_t() : emergencyStop(false),
libv2001 3:39b24d902aa7 30 inclinationState(IDLE_INC),
libv2001 3:39b24d902aa7 31 directionState(FORWARD),
libv2001 3:39b24d902aa7 32 orientationState(IDLE_OR),
libv2001 3:39b24d902aa7 33 forwardOrientation(0) {}
libv2001 3:39b24d902aa7 34 };
libv2001 3:39b24d902aa7 35
libv2001 3:39b24d902aa7 36 CurrentState_t currentState;
libv2001 3:39b24d902aa7 37
libv2001 3:39b24d902aa7 38 bool calibrateOrientationOnNextLoop = false;
libv2001 3:39b24d902aa7 39
libv2001 3:39b24d902aa7 40 void CalibrateOrientationInterrupt(){
libv2001 3:39b24d902aa7 41 calibrateOrientationOnNextLoop = true;
libv2001 3:39b24d902aa7 42 }
libv2001 3:39b24d902aa7 43
libv2001 3:39b24d902aa7 44 INCLINATION_STATE GetNextInclinationState(int16_t inclination){
libv2001 3:39b24d902aa7 45 uint16_t absInc = ABS(inclination);
libv2001 3:39b24d902aa7 46
libv2001 3:39b24d902aa7 47 if (absInc > 45){
libv2001 3:39b24d902aa7 48 currentState.emergencyStop = true;
libv2001 3:39b24d902aa7 49 }
libv2001 3:39b24d902aa7 50
libv2001 3:39b24d902aa7 51 if (currentState.emergencyStop){
libv2001 3:39b24d902aa7 52 return IDLE_INC;
libv2001 3:39b24d902aa7 53 }
libv2001 3:39b24d902aa7 54
libv2001 3:39b24d902aa7 55 switch (currentState.inclinationState){
libv2001 3:39b24d902aa7 56 case IDLE_INC:
libv2001 3:39b24d902aa7 57 if (absInc > 30){
libv2001 3:39b24d902aa7 58 return FAST;
libv2001 3:39b24d902aa7 59 } else if (absInc > 22){
libv2001 3:39b24d902aa7 60 return SLOW;
libv2001 3:39b24d902aa7 61 }
libv2001 3:39b24d902aa7 62 break;
libv2001 3:39b24d902aa7 63 case SLOW:
libv2001 3:39b24d902aa7 64 if (absInc > 30){
libv2001 3:39b24d902aa7 65 return FAST;
libv2001 3:39b24d902aa7 66 } else if (absInc < 18){
libv2001 3:39b24d902aa7 67 return IDLE_INC;
libv2001 3:39b24d902aa7 68 }
libv2001 3:39b24d902aa7 69 break;
libv2001 3:39b24d902aa7 70 case FAST:
libv2001 3:39b24d902aa7 71 if (absInc < 18){
libv2001 3:39b24d902aa7 72 return IDLE_INC;
libv2001 3:39b24d902aa7 73 } else if (absInc < 25){
libv2001 3:39b24d902aa7 74 return SLOW;
libv2001 3:39b24d902aa7 75 }
libv2001 3:39b24d902aa7 76 break;
libv2001 3:39b24d902aa7 77 }
libv2001 3:39b24d902aa7 78 return currentState.inclinationState;
libv2001 3:39b24d902aa7 79 }
libv2001 3:39b24d902aa7 80
libv2001 3:39b24d902aa7 81 DIRECTION_STATE GetNextDirectionState(int16_t inclination){
libv2001 3:39b24d902aa7 82 return (inclination > 0 ? FORWARD : BACKWARD);
libv2001 3:39b24d902aa7 83 }
libv2001 3:39b24d902aa7 84
libv2001 3:39b24d902aa7 85 ORIENTATION_STATE GetNextOrientationState(int16_t orientation){
libv2001 3:39b24d902aa7 86 // Bring the world orientation to a local reference
libv2001 3:39b24d902aa7 87 int16_t localOrientation = orientation - currentState.forwardOrientation;
libv2001 3:39b24d902aa7 88 // Be sure to have a value from 0 to 360
libv2001 3:39b24d902aa7 89 localOrientation += localOrientation < 0 ? 360 : 0;
libv2001 3:39b24d902aa7 90 // Devide the range from 0 to 180 for the right and from -180 to 0 for the left
libv2001 3:39b24d902aa7 91 localOrientation -= localOrientation > 180 ? 360 : 0;
libv2001 3:39b24d902aa7 92
libv2001 3:39b24d902aa7 93 if (ABS(localOrientation) > 90){
libv2001 3:39b24d902aa7 94 currentState.emergencyStop = true;
libv2001 3:39b24d902aa7 95 }
libv2001 3:39b24d902aa7 96
libv2001 3:39b24d902aa7 97 if (currentState.emergencyStop){
libv2001 3:39b24d902aa7 98 return IDLE_OR;
libv2001 3:39b24d902aa7 99 }
libv2001 3:39b24d902aa7 100
libv2001 3:39b24d902aa7 101 switch(currentState.orientationState){
libv2001 3:39b24d902aa7 102 case IDLE_OR:
libv2001 3:39b24d902aa7 103 if (localOrientation < -20){
libv2001 3:39b24d902aa7 104 return LEFT;
libv2001 3:39b24d902aa7 105 } else if (localOrientation > 20){
libv2001 3:39b24d902aa7 106 return RIGHT;
libv2001 3:39b24d902aa7 107 }
libv2001 3:39b24d902aa7 108 break;
libv2001 3:39b24d902aa7 109 case LEFT:
libv2001 3:39b24d902aa7 110 if (localOrientation > 20){
libv2001 3:39b24d902aa7 111 return RIGHT;
libv2001 3:39b24d902aa7 112 } else if (localOrientation > -15){
libv2001 3:39b24d902aa7 113 return IDLE_OR;
libv2001 3:39b24d902aa7 114 }
libv2001 3:39b24d902aa7 115 break;
libv2001 3:39b24d902aa7 116 case RIGHT:
libv2001 3:39b24d902aa7 117 if (localOrientation < -20){
libv2001 3:39b24d902aa7 118 return LEFT;
libv2001 3:39b24d902aa7 119 } else if (localOrientation < 15){
libv2001 3:39b24d902aa7 120 return IDLE_OR;
libv2001 3:39b24d902aa7 121 }
libv2001 3:39b24d902aa7 122 break;
libv2001 3:39b24d902aa7 123 }
libv2001 3:39b24d902aa7 124 return currentState.orientationState;
libv2001 3:39b24d902aa7 125 }
libv2001 3:39b24d902aa7 126
libv2001 0:28d5622d1a3e 127 int main() {
libv2001 3:39b24d902aa7 128 calibrateOrientation.rise(CalibrateOrientationInterrupt);
libv2001 3:39b24d902aa7 129
libv2001 1:3591e7df4ff4 130 if(!magneto.TestDeviceConnection() || !acc.TestDeviceConnection()){
libv2001 0:28d5622d1a3e 131 pc.printf("SCRUB!!\r\n");
libv2001 0:28d5622d1a3e 132 return -1;
libv2001 0:28d5622d1a3e 133 }
libv2001 0:28d5622d1a3e 134
libv2001 0:28d5622d1a3e 135 magneto.ActivateDevice();
libv2001 0:28d5622d1a3e 136 acc.ActivateDevice();
libv2001 0:28d5622d1a3e 137
libv2001 0:28d5622d1a3e 138 wait(0.2);
libv2001 0:28d5622d1a3e 139
libv2001 0:28d5622d1a3e 140 while(true){
libv2001 0:28d5622d1a3e 141 int16_t heading = magneto.GetHeadingXY();
libv2001 0:28d5622d1a3e 142 int16_t inclination = acc.GetInclinationYZ();
libv2001 3:39b24d902aa7 143
libv2001 3:39b24d902aa7 144 if (calibrateOrientationOnNextLoop){
libv2001 3:39b24d902aa7 145 currentState.forwardOrientation = heading;
libv2001 3:39b24d902aa7 146 calibrateOrientationOnNextLoop = false;
libv2001 3:39b24d902aa7 147 }
libv2001 3:39b24d902aa7 148
libv2001 3:39b24d902aa7 149 INCLINATION_STATE nextInc = GetNextInclinationState(inclination);
libv2001 3:39b24d902aa7 150 DIRECTION_STATE nextDir = GetNextDirectionState(inclination);
libv2001 3:39b24d902aa7 151
libv2001 3:39b24d902aa7 152 if (nextInc != currentState.inclinationState){
libv2001 3:39b24d902aa7 153 pc.printf("Changing Inclination from %d to %d\r\n", currentState.inclinationState, nextInc);
libv2001 3:39b24d902aa7 154 currentState.inclinationState = nextInc;
libv2001 3:39b24d902aa7 155 }
libv2001 3:39b24d902aa7 156
libv2001 3:39b24d902aa7 157 if (nextDir != currentState.directionState){
libv2001 3:39b24d902aa7 158 pc.printf("Changing Direction from %d to %d\r\n", currentState.directionState, nextDir);
libv2001 3:39b24d902aa7 159 currentState.directionState = nextDir;
libv2001 3:39b24d902aa7 160 }
libv2001 3:39b24d902aa7 161
libv2001 0:28d5622d1a3e 162 pc.printf("Heading : %d, Inclination : %d\r\n", heading, inclination);
libv2001 0:28d5622d1a3e 163 wait(0.1);
libv2001 0:28d5622d1a3e 164 }
libv2001 0:28d5622d1a3e 165 }