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@6:2605aa78ef9f, 2017-04-06 (annotated)

Committer:
libv2001
Date:
Thu Apr 06 11:27:04 2017 +0000
Revision:
6:2605aa78ef9f
Parent:
5:8142f455454b 
Underscores!!!!

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 6:2605aa78ef9f 8 DigitalOut _myled(LED1);
libv2001 6:2605aa78ef9f 9 DigitalOut _stopLed(LED2);
libv2001 0:28d5622d1a3e 10
libv2001 6:2605aa78ef9f 11 I2C _i2c(p28, p27);
libv2001 0:28d5622d1a3e 12
libv2001 6:2605aa78ef9f 13 Magneto _magneto(_i2c);
libv2001 6:2605aa78ef9f 14 Acc _acc(_i2c);
libv2001 0:28d5622d1a3e 15
libv2001 6:2605aa78ef9f 16 InterruptIn _calibrateOrientation(p13);
libv2001 6:2605aa78ef9f 17 InterruptIn _exitEmergencyStop(p14);
libv2001 4:b6d8445792cc 18
libv2001 6:2605aa78ef9f 19 Ticker _logger;
libv2001 3:39b24d902aa7 20
libv2001 6:2605aa78ef9f 21 Thread _xbeeTransmitter;
libv2001 0:28d5622d1a3e 22
libv2001 6:2605aa78ef9f 23 #define _ABS(a) ((a)<0 ? -(a) : (a))
libv2001 3:39b24d902aa7 24
libv2001 3:39b24d902aa7 25 struct CurrentState_t {
libv2001 3:39b24d902aa7 26 bool emergencyStop;
libv2001 5:8142f455454b 27 uint8_t inclinationState;
libv2001 5:8142f455454b 28 uint8_t directionState;
libv2001 5:8142f455454b 29 uint8_t orientationState;
libv2001 3:39b24d902aa7 30 uint16_t forwardOrientation;
libv2001 3:39b24d902aa7 31
libv2001 3:39b24d902aa7 32 CurrentState_t() : emergencyStop(false),
libv2001 5:8142f455454b 33 inclinationState(SPEED_STATE_IDLE),
libv2001 5:8142f455454b 34 directionState(DIRECTION_STATE_FORWARD),
libv2001 5:8142f455454b 35 orientationState(ANGLE_STATE_STRAIGHT),
libv2001 3:39b24d902aa7 36 forwardOrientation(0) {}
libv2001 3:39b24d902aa7 37 };
libv2001 3:39b24d902aa7 38
libv2001 6:2605aa78ef9f 39 CurrentState_t _currentState;
libv2001 3:39b24d902aa7 40
libv2001 6:2605aa78ef9f 41 bool _calibrateOrientationOnNextLoop = false;
libv2001 6:2605aa78ef9f 42 bool _logOnNextLoop = false;
libv2001 3:39b24d902aa7 43
libv2001 6:2605aa78ef9f 44 void _CalibrateOrientationInterrupt(){
libv2001 6:2605aa78ef9f 45 _calibrateOrientationOnNextLoop = true;
libv2001 3:39b24d902aa7 46 }
libv2001 3:39b24d902aa7 47
libv2001 6:2605aa78ef9f 48 void _ExitEmergencyStopInterrupt(){
libv2001 6:2605aa78ef9f 49 _currentState.emergencyStop = false;
libv2001 4:b6d8445792cc 50 }
libv2001 4:b6d8445792cc 51
libv2001 6:2605aa78ef9f 52 void _LoggerTick(){
libv2001 6:2605aa78ef9f 53 _logOnNextLoop = true;
libv2001 4:b6d8445792cc 54 }
libv2001 4:b6d8445792cc 55
libv2001 6:2605aa78ef9f 56 uint8_t _GetNextInclinationState(int16_t inclination){
libv2001 6:2605aa78ef9f 57 uint16_t absInc = _ABS(inclination);
libv2001 3:39b24d902aa7 58
libv2001 6:2605aa78ef9f 59 if (absInc > 45 && !_currentState.emergencyStop){
libv2001 6:2605aa78ef9f 60 _currentState.emergencyStop = true;
libv2001 6:2605aa78ef9f 61 printf("Inclinaison Max!!!!!\r\n");
libv2001 3:39b24d902aa7 62 }
libv2001 3:39b24d902aa7 63
libv2001 6:2605aa78ef9f 64 if (_currentState.emergencyStop){
libv2001 5:8142f455454b 65 return SPEED_STATE_IDLE;
libv2001 3:39b24d902aa7 66 }
libv2001 3:39b24d902aa7 67
libv2001 6:2605aa78ef9f 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 6:2605aa78ef9f 91 return _currentState.inclinationState;
libv2001 3:39b24d902aa7 92 }
libv2001 3:39b24d902aa7 93
libv2001 6:2605aa78ef9f 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 6:2605aa78ef9f 98 uint8_t _GetNextOrientationState(int16_t orientation){
libv2001 3:39b24d902aa7 99 // Bring the world orientation to a local reference
libv2001 6:2605aa78ef9f 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 6:2605aa78ef9f 106 if (_ABS(localOrientation) > 90 && !_currentState.emergencyStop){
libv2001 6:2605aa78ef9f 107 _currentState.emergencyStop = true;
libv2001 6:2605aa78ef9f 108 printf("Orientation Max!!!!!\r\n");
libv2001 3:39b24d902aa7 109 }
libv2001 3:39b24d902aa7 110
libv2001 6:2605aa78ef9f 111 switch(_currentState.orientationState){
libv2001 5:8142f455454b 112 case ANGLE_STATE_STRAIGHT:
libv2001 3:39b24d902aa7 113 if (localOrientation < -20){
libv2001 5:8142f455454b 114 return ANGLE_STATE_LEFT;
libv2001 3:39b24d902aa7 115 } else if (localOrientation > 20){
libv2001 5:8142f455454b 116 return ANGLE_STATE_RIGHT;
libv2001 3:39b24d902aa7 117 }
libv2001 3:39b24d902aa7 118 break;
libv2001 5:8142f455454b 119 case ANGLE_STATE_LEFT:
libv2001 3:39b24d902aa7 120 if (localOrientation > 20){
libv2001 5:8142f455454b 121 return ANGLE_STATE_RIGHT;
libv2001 3:39b24d902aa7 122 } else if (localOrientation > -15){
libv2001 5:8142f455454b 123 return ANGLE_STATE_STRAIGHT;
libv2001 3:39b24d902aa7 124 }
libv2001 3:39b24d902aa7 125 break;
libv2001 5:8142f455454b 126 case ANGLE_STATE_RIGHT:
libv2001 3:39b24d902aa7 127 if (localOrientation < -20){
libv2001 5:8142f455454b 128 return ANGLE_STATE_LEFT;
libv2001 3:39b24d902aa7 129 } else if (localOrientation < 15){
libv2001 5:8142f455454b 130 return ANGLE_STATE_STRAIGHT;
libv2001 3:39b24d902aa7 131 }
libv2001 3:39b24d902aa7 132 break;
libv2001 3:39b24d902aa7 133 }
libv2001 6:2605aa78ef9f 134 return _currentState.orientationState;
libv2001 3:39b24d902aa7 135 }
libv2001 3:39b24d902aa7 136
libv2001 6:2605aa78ef9f 137 void _XbeeCallback(char* message, int length){
libv2001 4:b6d8445792cc 138 if (message[0] == STOP_COMMAND){
libv2001 6:2605aa78ef9f 139 _currentState.emergencyStop = true;
libv2001 6:2605aa78ef9f 140 _stopLed = !_stopLed;
libv2001 0:28d5622d1a3e 141 }
libv2001 4:b6d8445792cc 142 }
libv2001 4:b6d8445792cc 143
libv2001 6:2605aa78ef9f 144 void _MainLoop(){
libv2001 6:2605aa78ef9f 145 InitXbee(false, _XbeeCallback, &_xbeeTransmitter);
libv2001 4:b6d8445792cc 146
libv2001 0:28d5622d1a3e 147 while(true){
libv2001 6:2605aa78ef9f 148 int16_t heading = _magneto.GetHeadingXY();
libv2001 6:2605aa78ef9f 149 int16_t inclination = _acc.GetInclinationYZ();
libv2001 3:39b24d902aa7 150
libv2001 6:2605aa78ef9f 151 if (_calibrateOrientationOnNextLoop){
libv2001 6:2605aa78ef9f 152 _currentState.forwardOrientation = heading;
libv2001 6:2605aa78ef9f 153 _calibrateOrientationOnNextLoop = false;
libv2001 3:39b24d902aa7 154 }
libv2001 3:39b24d902aa7 155
libv2001 6:2605aa78ef9f 156 uint8_t nextInc = _GetNextInclinationState(inclination);
libv2001 6:2605aa78ef9f 157 uint8_t nextDir = _GetNextDirectionState(inclination);
libv2001 6:2605aa78ef9f 158 uint8_t nextOr = _GetNextOrientationState(heading);
libv2001 6:2605aa78ef9f 159
libv2001 6:2605aa78ef9f 160 if (nextInc != _currentState.inclinationState){
libv2001 6:2605aa78ef9f 161 _currentState.inclinationState = nextInc;
libv2001 3:39b24d902aa7 162 }
libv2001 3:39b24d902aa7 163
libv2001 6:2605aa78ef9f 164 if (nextDir != _currentState.directionState){
libv2001 6:2605aa78ef9f 165 _currentState.directionState = nextDir;
libv2001 5:8142f455454b 166 }
libv2001 5:8142f455454b 167
libv2001 6:2605aa78ef9f 168 if (nextOr != _currentState.orientationState){
libv2001 6:2605aa78ef9f 169 _currentState.orientationState = nextOr;
libv2001 6:2605aa78ef9f 170 }
libv2001 4:b6d8445792cc 171
libv2001 6:2605aa78ef9f 172 if (_logOnNextLoop){
libv2001 4:b6d8445792cc 173 char data[4] = {LOG_COMMAND,
libv2001 6:2605aa78ef9f 174 _currentState.emergencyStop ? SPEED_STATE_STOP : _currentState.inclinationState,
libv2001 6:2605aa78ef9f 175 _currentState.orientationState,
libv2001 6:2605aa78ef9f 176 _currentState.directionState};
libv2001 4:b6d8445792cc 177 XbeeSendData(data, 4);
libv2001 6:2605aa78ef9f 178 _logOnNextLoop = false;
libv2001 4:b6d8445792cc 179 }
libv2001 4:b6d8445792cc 180
libv2001 0:28d5622d1a3e 181 wait(0.1);
libv2001 0:28d5622d1a3e 182 }
libv2001 0:28d5622d1a3e 183 }
libv2001 4:b6d8445792cc 184
libv2001 4:b6d8445792cc 185 int main() {
libv2001 5:8142f455454b 186
libv2001 6:2605aa78ef9f 187 _currentState = CurrentState_t();
libv2001 5:8142f455454b 188
libv2001 6:2605aa78ef9f 189 if(!_magneto.TestDeviceConnection() || !_acc.TestDeviceConnection()){
libv2001 6:2605aa78ef9f 190 //pc.printf("SCRUB!!\r\n");
libv2001 4:b6d8445792cc 191 return -1;
libv2001 4:b6d8445792cc 192 }
libv2001 4:b6d8445792cc 193
libv2001 6:2605aa78ef9f 194 _magneto.ActivateDevice();
libv2001 6:2605aa78ef9f 195 _acc.ActivateDevice();
libv2001 4:b6d8445792cc 196
libv2001 6:2605aa78ef9f 197 _calibrateOrientation.rise(_CalibrateOrientationInterrupt);
libv2001 6:2605aa78ef9f 198 _exitEmergencyStop.rise(_ExitEmergencyStopInterrupt);
libv2001 4:b6d8445792cc 199
libv2001 6:2605aa78ef9f 200 _logger.attach(_LoggerTick, 2);
libv2001 4:b6d8445792cc 201
libv2001 6:2605aa78ef9f 202 _xbeeTransmitter.start(callback(_MainLoop));
libv2001 4:b6d8445792cc 203
libv2001 4:b6d8445792cc 204 while (1){
libv2001 6:2605aa78ef9f 205 _myled = !_myled;
libv2001 4:b6d8445792cc 206 wait(0.5);
libv2001 4:b6d8445792cc 207 }
libv2001 4:b6d8445792cc 208 }