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@5:8142f455454b, 2017-04-03 (annotated)
- Committer:
- libv2001
- Date:
- Mon Apr 03 18:55:55 2017 +0000
- Revision:
- 5:8142f455454b
- Parent:
- 4:b6d8445792cc
- Child:
- 6:2605aa78ef9f
Mise en commun
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:b6d8445792cc | 4 | #include "communication.h" |
libv2001 | 4:b6d8445792cc | 5 | #include "xbee.h" |
libv2001 | 4:b6d8445792cc | 6 | #include "rtos.h" |
libv2001 | 0:28d5622d1a3e | 7 | |
libv2001 | 0:28d5622d1a3e | 8 | DigitalOut myled(LED1); |
libv2001 | 0:28d5622d1a3e | 9 | |
libv2001 | 0:28d5622d1a3e | 10 | I2C i2c(p28, p27); |
libv2001 | 0:28d5622d1a3e | 11 | |
libv2001 | 0:28d5622d1a3e | 12 | Magneto magneto(i2c); |
libv2001 | 0:28d5622d1a3e | 13 | Acc acc(i2c); |
libv2001 | 0:28d5622d1a3e | 14 | |
libv2001 | 3:39b24d902aa7 | 15 | InterruptIn calibrateOrientation(p13); |
libv2001 | 4:b6d8445792cc | 16 | InterruptIn exitEmergencyStop(p14); |
libv2001 | 4:b6d8445792cc | 17 | |
libv2001 | 4:b6d8445792cc | 18 | Ticker logger; |
libv2001 | 4:b6d8445792cc | 19 | |
libv2001 | 4:b6d8445792cc | 20 | Thread xbeeTransmitter; |
libv2001 | 3:39b24d902aa7 | 21 | |
libv2001 | 0:28d5622d1a3e | 22 | Serial pc(USBTX, USBRX); |
libv2001 | 0:28d5622d1a3e | 23 | |
libv2001 | 3:39b24d902aa7 | 24 | #define ABS(a) ((a)<0 ? -(a) : (a)) |
libv2001 | 3:39b24d902aa7 | 25 | |
libv2001 | 3:39b24d902aa7 | 26 | struct CurrentState_t { |
libv2001 | 3:39b24d902aa7 | 27 | bool emergencyStop; |
libv2001 | 5:8142f455454b | 28 | uint8_t inclinationState; |
libv2001 | 5:8142f455454b | 29 | uint8_t directionState; |
libv2001 | 5:8142f455454b | 30 | uint8_t orientationState; |
libv2001 | 3:39b24d902aa7 | 31 | uint16_t forwardOrientation; |
libv2001 | 3:39b24d902aa7 | 32 | |
libv2001 | 3:39b24d902aa7 | 33 | CurrentState_t() : emergencyStop(false), |
libv2001 | 5:8142f455454b | 34 | inclinationState(SPEED_STATE_IDLE), |
libv2001 | 5:8142f455454b | 35 | directionState(DIRECTION_STATE_FORWARD), |
libv2001 | 5:8142f455454b | 36 | orientationState(ANGLE_STATE_STRAIGHT), |
libv2001 | 3:39b24d902aa7 | 37 | forwardOrientation(0) {} |
libv2001 | 3:39b24d902aa7 | 38 | }; |
libv2001 | 3:39b24d902aa7 | 39 | |
libv2001 | 3:39b24d902aa7 | 40 | CurrentState_t currentState; |
libv2001 | 3:39b24d902aa7 | 41 | |
libv2001 | 3:39b24d902aa7 | 42 | bool calibrateOrientationOnNextLoop = false; |
libv2001 | 4:b6d8445792cc | 43 | bool logOnNextLoop = false; |
libv2001 | 3:39b24d902aa7 | 44 | |
libv2001 | 3:39b24d902aa7 | 45 | void CalibrateOrientationInterrupt(){ |
libv2001 | 3:39b24d902aa7 | 46 | calibrateOrientationOnNextLoop = true; |
libv2001 | 3:39b24d902aa7 | 47 | } |
libv2001 | 3:39b24d902aa7 | 48 | |
libv2001 | 4:b6d8445792cc | 49 | void ExitEmergencyStopInterrupt(){ |
libv2001 | 4:b6d8445792cc | 50 | currentState.emergencyStop = false; |
libv2001 | 4:b6d8445792cc | 51 | } |
libv2001 | 4:b6d8445792cc | 52 | |
libv2001 | 4:b6d8445792cc | 53 | void LoggerTick(){ |
libv2001 | 4:b6d8445792cc | 54 | logOnNextLoop = true; |
libv2001 | 4:b6d8445792cc | 55 | } |
libv2001 | 4:b6d8445792cc | 56 | |
libv2001 | 5:8142f455454b | 57 | uint8_t GetNextInclinationState(int16_t inclination){ |
libv2001 | 3:39b24d902aa7 | 58 | uint16_t absInc = ABS(inclination); |
libv2001 | 3:39b24d902aa7 | 59 | |
libv2001 | 3:39b24d902aa7 | 60 | if (absInc > 45){ |
libv2001 | 3:39b24d902aa7 | 61 | currentState.emergencyStop = true; |
libv2001 | 3:39b24d902aa7 | 62 | } |
libv2001 | 3:39b24d902aa7 | 63 | |
libv2001 | 3:39b24d902aa7 | 64 | if (currentState.emergencyStop){ |
libv2001 | 5:8142f455454b | 65 | return SPEED_STATE_IDLE; |
libv2001 | 3:39b24d902aa7 | 66 | } |
libv2001 | 3:39b24d902aa7 | 67 | |
libv2001 | 3:39b24d902aa7 | 68 | switch (currentState.inclinationState){ |
libv2001 | 5:8142f455454b | 69 | case SPEED_STATE_IDLE: |
libv2001 | 3:39b24d902aa7 | 70 | if (absInc > 30){ |
libv2001 | 5:8142f455454b | 71 | return SPEED_STATE_FAST; |
libv2001 | 3:39b24d902aa7 | 72 | } else if (absInc > 22){ |
libv2001 | 5:8142f455454b | 73 | return SPEED_STATE_SLOW; |
libv2001 | 3:39b24d902aa7 | 74 | } |
libv2001 | 3:39b24d902aa7 | 75 | break; |
libv2001 | 5:8142f455454b | 76 | case SPEED_STATE_SLOW: |
libv2001 | 3:39b24d902aa7 | 77 | if (absInc > 30){ |
libv2001 | 5:8142f455454b | 78 | return SPEED_STATE_FAST; |
libv2001 | 3:39b24d902aa7 | 79 | } else if (absInc < 18){ |
libv2001 | 5:8142f455454b | 80 | return SPEED_STATE_IDLE; |
libv2001 | 3:39b24d902aa7 | 81 | } |
libv2001 | 3:39b24d902aa7 | 82 | break; |
libv2001 | 5:8142f455454b | 83 | case SPEED_STATE_FAST: |
libv2001 | 3:39b24d902aa7 | 84 | if (absInc < 18){ |
libv2001 | 5:8142f455454b | 85 | return SPEED_STATE_IDLE; |
libv2001 | 3:39b24d902aa7 | 86 | } else if (absInc < 25){ |
libv2001 | 5:8142f455454b | 87 | return SPEED_STATE_SLOW; |
libv2001 | 3:39b24d902aa7 | 88 | } |
libv2001 | 3:39b24d902aa7 | 89 | break; |
libv2001 | 3:39b24d902aa7 | 90 | } |
libv2001 | 3:39b24d902aa7 | 91 | return currentState.inclinationState; |
libv2001 | 3:39b24d902aa7 | 92 | } |
libv2001 | 3:39b24d902aa7 | 93 | |
libv2001 | 5:8142f455454b | 94 | uint8_t GetNextDirectionState(int16_t inclination){ |
libv2001 | 5:8142f455454b | 95 | return (inclination > 0 ? DIRECTION_STATE_FORWARD : DIRECTION_STATE_BACKWARD); |
libv2001 | 3:39b24d902aa7 | 96 | } |
libv2001 | 3:39b24d902aa7 | 97 | |
libv2001 | 5:8142f455454b | 98 | uint8_t GetNextOrientationState(int16_t orientation){ |
libv2001 | 3:39b24d902aa7 | 99 | // Bring the world orientation to a local reference |
libv2001 | 3:39b24d902aa7 | 100 | int16_t localOrientation = orientation - currentState.forwardOrientation; |
libv2001 | 3:39b24d902aa7 | 101 | // Be sure to have a value from 0 to 360 |
libv2001 | 3:39b24d902aa7 | 102 | localOrientation += localOrientation < 0 ? 360 : 0; |
libv2001 | 3:39b24d902aa7 | 103 | // Devide the range from 0 to 180 for the right and from -180 to 0 for the left |
libv2001 | 3:39b24d902aa7 | 104 | localOrientation -= localOrientation > 180 ? 360 : 0; |
libv2001 | 3:39b24d902aa7 | 105 | |
libv2001 | 3:39b24d902aa7 | 106 | if (ABS(localOrientation) > 90){ |
libv2001 | 3:39b24d902aa7 | 107 | currentState.emergencyStop = true; |
libv2001 | 3:39b24d902aa7 | 108 | } |
libv2001 | 3:39b24d902aa7 | 109 | |
libv2001 | 3:39b24d902aa7 | 110 | if (currentState.emergencyStop){ |
libv2001 | 5:8142f455454b | 111 | return ANGLE_STATE_STRAIGHT; |
libv2001 | 3:39b24d902aa7 | 112 | } |
libv2001 | 3:39b24d902aa7 | 113 | |
libv2001 | 3:39b24d902aa7 | 114 | switch(currentState.orientationState){ |
libv2001 | 5:8142f455454b | 115 | case ANGLE_STATE_STRAIGHT: |
libv2001 | 3:39b24d902aa7 | 116 | if (localOrientation < -20){ |
libv2001 | 5:8142f455454b | 117 | return ANGLE_STATE_LEFT; |
libv2001 | 3:39b24d902aa7 | 118 | } else if (localOrientation > 20){ |
libv2001 | 5:8142f455454b | 119 | return ANGLE_STATE_RIGHT; |
libv2001 | 3:39b24d902aa7 | 120 | } |
libv2001 | 3:39b24d902aa7 | 121 | break; |
libv2001 | 5:8142f455454b | 122 | case ANGLE_STATE_LEFT: |
libv2001 | 3:39b24d902aa7 | 123 | if (localOrientation > 20){ |
libv2001 | 5:8142f455454b | 124 | return ANGLE_STATE_RIGHT; |
libv2001 | 3:39b24d902aa7 | 125 | } else if (localOrientation > -15){ |
libv2001 | 5:8142f455454b | 126 | return ANGLE_STATE_STRAIGHT; |
libv2001 | 3:39b24d902aa7 | 127 | } |
libv2001 | 3:39b24d902aa7 | 128 | break; |
libv2001 | 5:8142f455454b | 129 | case ANGLE_STATE_RIGHT: |
libv2001 | 3:39b24d902aa7 | 130 | if (localOrientation < -20){ |
libv2001 | 5:8142f455454b | 131 | return ANGLE_STATE_LEFT; |
libv2001 | 3:39b24d902aa7 | 132 | } else if (localOrientation < 15){ |
libv2001 | 5:8142f455454b | 133 | return ANGLE_STATE_STRAIGHT; |
libv2001 | 3:39b24d902aa7 | 134 | } |
libv2001 | 3:39b24d902aa7 | 135 | break; |
libv2001 | 3:39b24d902aa7 | 136 | } |
libv2001 | 3:39b24d902aa7 | 137 | return currentState.orientationState; |
libv2001 | 3:39b24d902aa7 | 138 | } |
libv2001 | 3:39b24d902aa7 | 139 | |
libv2001 | 4:b6d8445792cc | 140 | void XbeeCallback(char* message, int length){ |
libv2001 | 4:b6d8445792cc | 141 | if (message[0] == STOP_COMMAND){ |
libv2001 | 4:b6d8445792cc | 142 | currentState.emergencyStop = true; |
libv2001 | 4:b6d8445792cc | 143 | pc.printf("Remote Emergency Stop received\r\n"); |
libv2001 | 0:28d5622d1a3e | 144 | } |
libv2001 | 4:b6d8445792cc | 145 | } |
libv2001 | 4:b6d8445792cc | 146 | |
libv2001 | 4:b6d8445792cc | 147 | void MainLoop(){ |
libv2001 | 4:b6d8445792cc | 148 | InitXbee(false, XbeeCallback, &xbeeTransmitter); |
libv2001 | 4:b6d8445792cc | 149 | |
libv2001 | 0:28d5622d1a3e | 150 | while(true){ |
libv2001 | 0:28d5622d1a3e | 151 | int16_t heading = magneto.GetHeadingXY(); |
libv2001 | 0:28d5622d1a3e | 152 | int16_t inclination = acc.GetInclinationYZ(); |
libv2001 | 3:39b24d902aa7 | 153 | |
libv2001 | 3:39b24d902aa7 | 154 | if (calibrateOrientationOnNextLoop){ |
libv2001 | 3:39b24d902aa7 | 155 | currentState.forwardOrientation = heading; |
libv2001 | 3:39b24d902aa7 | 156 | calibrateOrientationOnNextLoop = false; |
libv2001 | 3:39b24d902aa7 | 157 | } |
libv2001 | 3:39b24d902aa7 | 158 | |
libv2001 | 5:8142f455454b | 159 | uint8_t nextInc = GetNextInclinationState(inclination); |
libv2001 | 5:8142f455454b | 160 | uint8_t nextDir = GetNextDirectionState(inclination); |
libv2001 | 5:8142f455454b | 161 | uint8_t nextOr = GetNextOrientationState(heading); |
libv2001 | 3:39b24d902aa7 | 162 | |
libv2001 | 3:39b24d902aa7 | 163 | if (nextInc != currentState.inclinationState){ |
libv2001 | 5:8142f455454b | 164 | pc.printf("Changing Inclination from %02X to %02X\r\n", currentState.inclinationState, nextInc); |
libv2001 | 3:39b24d902aa7 | 165 | currentState.inclinationState = nextInc; |
libv2001 | 3:39b24d902aa7 | 166 | } |
libv2001 | 3:39b24d902aa7 | 167 | |
libv2001 | 3:39b24d902aa7 | 168 | if (nextDir != currentState.directionState){ |
libv2001 | 5:8142f455454b | 169 | pc.printf("Changing Direction from %02X to %02X\r\n", currentState.directionState, nextDir); |
libv2001 | 3:39b24d902aa7 | 170 | currentState.directionState = nextDir; |
libv2001 | 3:39b24d902aa7 | 171 | } |
libv2001 | 3:39b24d902aa7 | 172 | |
libv2001 | 5:8142f455454b | 173 | if (nextOr != currentState.orientationState){ |
libv2001 | 5:8142f455454b | 174 | pc.printf("Changing Orientation from %02X to %02X\r\n", currentState.orientationState, nextOr); |
libv2001 | 5:8142f455454b | 175 | currentState.orientationState = nextOr; |
libv2001 | 5:8142f455454b | 176 | } |
libv2001 | 5:8142f455454b | 177 | |
libv2001 | 5:8142f455454b | 178 | //pc.printf("Heading : %d, Inclination : %d\r\n", heading, inclination); |
libv2001 | 4:b6d8445792cc | 179 | |
libv2001 | 4:b6d8445792cc | 180 | if (logOnNextLoop){ |
libv2001 | 4:b6d8445792cc | 181 | pc.printf("Logging data\r\n"); |
libv2001 | 4:b6d8445792cc | 182 | char data[4] = {LOG_COMMAND, |
libv2001 | 4:b6d8445792cc | 183 | currentState.emergencyStop ? SPEED_STATE_STOP : currentState.inclinationState, |
libv2001 | 4:b6d8445792cc | 184 | currentState.orientationState, |
libv2001 | 4:b6d8445792cc | 185 | currentState.directionState}; |
libv2001 | 4:b6d8445792cc | 186 | XbeeSendData(data, 4); |
libv2001 | 4:b6d8445792cc | 187 | logOnNextLoop = false; |
libv2001 | 4:b6d8445792cc | 188 | } |
libv2001 | 4:b6d8445792cc | 189 | |
libv2001 | 0:28d5622d1a3e | 190 | wait(0.1); |
libv2001 | 0:28d5622d1a3e | 191 | } |
libv2001 | 0:28d5622d1a3e | 192 | } |
libv2001 | 4:b6d8445792cc | 193 | |
libv2001 | 4:b6d8445792cc | 194 | int main() { |
libv2001 | 5:8142f455454b | 195 | |
libv2001 | 5:8142f455454b | 196 | currentState = CurrentState_t(); |
libv2001 | 5:8142f455454b | 197 | |
libv2001 | 4:b6d8445792cc | 198 | if(!magneto.TestDeviceConnection() || !acc.TestDeviceConnection()){ |
libv2001 | 4:b6d8445792cc | 199 | pc.printf("SCRUB!!\r\n"); |
libv2001 | 4:b6d8445792cc | 200 | return -1; |
libv2001 | 4:b6d8445792cc | 201 | } |
libv2001 | 4:b6d8445792cc | 202 | |
libv2001 | 4:b6d8445792cc | 203 | magneto.ActivateDevice(); |
libv2001 | 4:b6d8445792cc | 204 | acc.ActivateDevice(); |
libv2001 | 4:b6d8445792cc | 205 | |
libv2001 | 4:b6d8445792cc | 206 | calibrateOrientation.rise(CalibrateOrientationInterrupt); |
libv2001 | 4:b6d8445792cc | 207 | exitEmergencyStop.rise(ExitEmergencyStopInterrupt); |
libv2001 | 4:b6d8445792cc | 208 | |
libv2001 | 4:b6d8445792cc | 209 | logger.attach(LoggerTick, 2); |
libv2001 | 4:b6d8445792cc | 210 | |
libv2001 | 4:b6d8445792cc | 211 | xbeeTransmitter.start(callback(MainLoop)); |
libv2001 | 4:b6d8445792cc | 212 | |
libv2001 | 4:b6d8445792cc | 213 | while (1){ |
libv2001 | 4:b6d8445792cc | 214 | myled = !myled; |
libv2001 | 4:b6d8445792cc | 215 | wait(0.5); |
libv2001 | 4:b6d8445792cc | 216 | } |
libv2001 | 4:b6d8445792cc | 217 | } |