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@4:b6d8445792cc, 2017-04-03 (annotated)

Committer:
libv2001
Date:
Mon Apr 03 12:17:55 2017 +0000
Revision:
4:b6d8445792cc
Parent:
3:39b24d902aa7
Child:
5:8142f455454b
Xbee int?gr?

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 4:b6d8445792cc 26 enum INCLINATION_STATE {IDLE_INC = SPEED_STATE_IDLE, SLOW = SPEED_STATE_SLOW, FAST = SPEED_STATE_FAST};
libv2001 4:b6d8445792cc 27 enum DIRECTION_STATE {FORWARD = DIRECTION_STATE_FORWARD, BACKWARD = DIRECTION_STATE_BACKWARD};
libv2001 4:b6d8445792cc 28 enum ORIENTATION_STATE {IDLE_OR = ANGLE_STATE_STRAIGHT, LEFT = ANGLE_STATE_LEFT, RIGHT = ANGLE_STATE_RIGHT};
libv2001 3:39b24d902aa7 29
libv2001 3:39b24d902aa7 30 struct CurrentState_t {
libv2001 3:39b24d902aa7 31 bool emergencyStop;
libv2001 3:39b24d902aa7 32 INCLINATION_STATE inclinationState;
libv2001 3:39b24d902aa7 33 DIRECTION_STATE directionState;
libv2001 3:39b24d902aa7 34 ORIENTATION_STATE orientationState;
libv2001 3:39b24d902aa7 35 uint16_t forwardOrientation;
libv2001 3:39b24d902aa7 36
libv2001 3:39b24d902aa7 37 CurrentState_t() : emergencyStop(false),
libv2001 3:39b24d902aa7 38 inclinationState(IDLE_INC),
libv2001 3:39b24d902aa7 39 directionState(FORWARD),
libv2001 3:39b24d902aa7 40 orientationState(IDLE_OR),
libv2001 3:39b24d902aa7 41 forwardOrientation(0) {}
libv2001 3:39b24d902aa7 42 };
libv2001 3:39b24d902aa7 43
libv2001 3:39b24d902aa7 44 CurrentState_t currentState;
libv2001 3:39b24d902aa7 45
libv2001 3:39b24d902aa7 46 bool calibrateOrientationOnNextLoop = false;
libv2001 4:b6d8445792cc 47 bool logOnNextLoop = false;
libv2001 3:39b24d902aa7 48
libv2001 3:39b24d902aa7 49 void CalibrateOrientationInterrupt(){
libv2001 3:39b24d902aa7 50 calibrateOrientationOnNextLoop = true;
libv2001 3:39b24d902aa7 51 }
libv2001 3:39b24d902aa7 52
libv2001 4:b6d8445792cc 53 void ExitEmergencyStopInterrupt(){
libv2001 4:b6d8445792cc 54 currentState.emergencyStop = false;
libv2001 4:b6d8445792cc 55 }
libv2001 4:b6d8445792cc 56
libv2001 4:b6d8445792cc 57 void LoggerTick(){
libv2001 4:b6d8445792cc 58 logOnNextLoop = true;
libv2001 4:b6d8445792cc 59 }
libv2001 4:b6d8445792cc 60
libv2001 3:39b24d902aa7 61 INCLINATION_STATE GetNextInclinationState(int16_t inclination){
libv2001 3:39b24d902aa7 62 uint16_t absInc = ABS(inclination);
libv2001 3:39b24d902aa7 63
libv2001 3:39b24d902aa7 64 if (absInc > 45){
libv2001 3:39b24d902aa7 65 currentState.emergencyStop = true;
libv2001 3:39b24d902aa7 66 }
libv2001 3:39b24d902aa7 67
libv2001 3:39b24d902aa7 68 if (currentState.emergencyStop){
libv2001 3:39b24d902aa7 69 return IDLE_INC;
libv2001 3:39b24d902aa7 70 }
libv2001 3:39b24d902aa7 71
libv2001 3:39b24d902aa7 72 switch (currentState.inclinationState){
libv2001 3:39b24d902aa7 73 case IDLE_INC:
libv2001 3:39b24d902aa7 74 if (absInc > 30){
libv2001 3:39b24d902aa7 75 return FAST;
libv2001 3:39b24d902aa7 76 } else if (absInc > 22){
libv2001 3:39b24d902aa7 77 return SLOW;
libv2001 3:39b24d902aa7 78 }
libv2001 3:39b24d902aa7 79 break;
libv2001 3:39b24d902aa7 80 case SLOW:
libv2001 3:39b24d902aa7 81 if (absInc > 30){
libv2001 3:39b24d902aa7 82 return FAST;
libv2001 3:39b24d902aa7 83 } else if (absInc < 18){
libv2001 3:39b24d902aa7 84 return IDLE_INC;
libv2001 3:39b24d902aa7 85 }
libv2001 3:39b24d902aa7 86 break;
libv2001 3:39b24d902aa7 87 case FAST:
libv2001 3:39b24d902aa7 88 if (absInc < 18){
libv2001 3:39b24d902aa7 89 return IDLE_INC;
libv2001 3:39b24d902aa7 90 } else if (absInc < 25){
libv2001 3:39b24d902aa7 91 return SLOW;
libv2001 3:39b24d902aa7 92 }
libv2001 3:39b24d902aa7 93 break;
libv2001 3:39b24d902aa7 94 }
libv2001 3:39b24d902aa7 95 return currentState.inclinationState;
libv2001 3:39b24d902aa7 96 }
libv2001 3:39b24d902aa7 97
libv2001 3:39b24d902aa7 98 DIRECTION_STATE GetNextDirectionState(int16_t inclination){
libv2001 3:39b24d902aa7 99 return (inclination > 0 ? FORWARD : BACKWARD);
libv2001 3:39b24d902aa7 100 }
libv2001 3:39b24d902aa7 101
libv2001 3:39b24d902aa7 102 ORIENTATION_STATE GetNextOrientationState(int16_t orientation){
libv2001 3:39b24d902aa7 103 // Bring the world orientation to a local reference
libv2001 3:39b24d902aa7 104 int16_t localOrientation = orientation - currentState.forwardOrientation;
libv2001 3:39b24d902aa7 105 // Be sure to have a value from 0 to 360
libv2001 3:39b24d902aa7 106 localOrientation += localOrientation < 0 ? 360 : 0;
libv2001 3:39b24d902aa7 107 // Devide the range from 0 to 180 for the right and from -180 to 0 for the left
libv2001 3:39b24d902aa7 108 localOrientation -= localOrientation > 180 ? 360 : 0;
libv2001 3:39b24d902aa7 109
libv2001 3:39b24d902aa7 110 if (ABS(localOrientation) > 90){
libv2001 3:39b24d902aa7 111 currentState.emergencyStop = true;
libv2001 3:39b24d902aa7 112 }
libv2001 3:39b24d902aa7 113
libv2001 3:39b24d902aa7 114 if (currentState.emergencyStop){
libv2001 3:39b24d902aa7 115 return IDLE_OR;
libv2001 3:39b24d902aa7 116 }
libv2001 3:39b24d902aa7 117
libv2001 3:39b24d902aa7 118 switch(currentState.orientationState){
libv2001 3:39b24d902aa7 119 case IDLE_OR:
libv2001 3:39b24d902aa7 120 if (localOrientation < -20){
libv2001 3:39b24d902aa7 121 return LEFT;
libv2001 3:39b24d902aa7 122 } else if (localOrientation > 20){
libv2001 3:39b24d902aa7 123 return RIGHT;
libv2001 3:39b24d902aa7 124 }
libv2001 3:39b24d902aa7 125 break;
libv2001 3:39b24d902aa7 126 case LEFT:
libv2001 3:39b24d902aa7 127 if (localOrientation > 20){
libv2001 3:39b24d902aa7 128 return RIGHT;
libv2001 3:39b24d902aa7 129 } else if (localOrientation > -15){
libv2001 3:39b24d902aa7 130 return IDLE_OR;
libv2001 3:39b24d902aa7 131 }
libv2001 3:39b24d902aa7 132 break;
libv2001 3:39b24d902aa7 133 case RIGHT:
libv2001 3:39b24d902aa7 134 if (localOrientation < -20){
libv2001 3:39b24d902aa7 135 return LEFT;
libv2001 3:39b24d902aa7 136 } else if (localOrientation < 15){
libv2001 3:39b24d902aa7 137 return IDLE_OR;
libv2001 3:39b24d902aa7 138 }
libv2001 3:39b24d902aa7 139 break;
libv2001 3:39b24d902aa7 140 }
libv2001 3:39b24d902aa7 141 return currentState.orientationState;
libv2001 3:39b24d902aa7 142 }
libv2001 3:39b24d902aa7 143
libv2001 4:b6d8445792cc 144 void XbeeCallback(char* message, int length){
libv2001 4:b6d8445792cc 145 if (message[0] == STOP_COMMAND){
libv2001 4:b6d8445792cc 146 currentState.emergencyStop = true;
libv2001 4:b6d8445792cc 147 pc.printf("Remote Emergency Stop received\r\n");
libv2001 0:28d5622d1a3e 148 }
libv2001 4:b6d8445792cc 149 }
libv2001 4:b6d8445792cc 150
libv2001 4:b6d8445792cc 151 void MainLoop(){
libv2001 4:b6d8445792cc 152 InitXbee(false, XbeeCallback, &xbeeTransmitter);
libv2001 4:b6d8445792cc 153
libv2001 0:28d5622d1a3e 154 while(true){
libv2001 0:28d5622d1a3e 155 int16_t heading = magneto.GetHeadingXY();
libv2001 0:28d5622d1a3e 156 int16_t inclination = acc.GetInclinationYZ();
libv2001 3:39b24d902aa7 157
libv2001 3:39b24d902aa7 158 if (calibrateOrientationOnNextLoop){
libv2001 3:39b24d902aa7 159 currentState.forwardOrientation = heading;
libv2001 3:39b24d902aa7 160 calibrateOrientationOnNextLoop = false;
libv2001 3:39b24d902aa7 161 }
libv2001 3:39b24d902aa7 162
libv2001 3:39b24d902aa7 163 INCLINATION_STATE nextInc = GetNextInclinationState(inclination);
libv2001 3:39b24d902aa7 164 DIRECTION_STATE nextDir = GetNextDirectionState(inclination);
libv2001 3:39b24d902aa7 165
libv2001 3:39b24d902aa7 166 if (nextInc != currentState.inclinationState){
libv2001 3:39b24d902aa7 167 pc.printf("Changing Inclination from %d to %d\r\n", currentState.inclinationState, nextInc);
libv2001 3:39b24d902aa7 168 currentState.inclinationState = nextInc;
libv2001 3:39b24d902aa7 169 }
libv2001 3:39b24d902aa7 170
libv2001 3:39b24d902aa7 171 if (nextDir != currentState.directionState){
libv2001 3:39b24d902aa7 172 pc.printf("Changing Direction from %d to %d\r\n", currentState.directionState, nextDir);
libv2001 3:39b24d902aa7 173 currentState.directionState = nextDir;
libv2001 3:39b24d902aa7 174 }
libv2001 3:39b24d902aa7 175
libv2001 0:28d5622d1a3e 176 pc.printf("Heading : %d, Inclination : %d\r\n", heading, inclination);
libv2001 4:b6d8445792cc 177
libv2001 4:b6d8445792cc 178 if (logOnNextLoop){
libv2001 4:b6d8445792cc 179 pc.printf("Logging data\r\n");
libv2001 4:b6d8445792cc 180 char data[4] = {LOG_COMMAND,
libv2001 4:b6d8445792cc 181 currentState.emergencyStop ? SPEED_STATE_STOP : currentState.inclinationState,
libv2001 4:b6d8445792cc 182 currentState.orientationState,
libv2001 4:b6d8445792cc 183 currentState.directionState};
libv2001 4:b6d8445792cc 184 XbeeSendData(data, 4);
libv2001 4:b6d8445792cc 185 logOnNextLoop = false;
libv2001 4:b6d8445792cc 186 }
libv2001 4:b6d8445792cc 187
libv2001 0:28d5622d1a3e 188 wait(0.1);
libv2001 0:28d5622d1a3e 189 }
libv2001 0:28d5622d1a3e 190 }
libv2001 4:b6d8445792cc 191
libv2001 4:b6d8445792cc 192 int main() {
libv2001 4:b6d8445792cc 193 if(!magneto.TestDeviceConnection() || !acc.TestDeviceConnection()){
libv2001 4:b6d8445792cc 194 pc.printf("SCRUB!!\r\n");
libv2001 4:b6d8445792cc 195 return -1;
libv2001 4:b6d8445792cc 196 }
libv2001 4:b6d8445792cc 197
libv2001 4:b6d8445792cc 198 magneto.ActivateDevice();
libv2001 4:b6d8445792cc 199 acc.ActivateDevice();
libv2001 4:b6d8445792cc 200
libv2001 4:b6d8445792cc 201 calibrateOrientation.rise(CalibrateOrientationInterrupt);
libv2001 4:b6d8445792cc 202 exitEmergencyStop.rise(ExitEmergencyStopInterrupt);
libv2001 4:b6d8445792cc 203
libv2001 4:b6d8445792cc 204 logger.attach(LoggerTick, 2);
libv2001 4:b6d8445792cc 205
libv2001 4:b6d8445792cc 206 xbeeTransmitter.start(callback(MainLoop));
libv2001 4:b6d8445792cc 207
libv2001 4:b6d8445792cc 208 while (1){
libv2001 4:b6d8445792cc 209 myled = !myled;
libv2001 4:b6d8445792cc 210 wait(0.5);
libv2001 4:b6d8445792cc 211 }
libv2001 4:b6d8445792cc 212 }