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@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?

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 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 }