Juan Salazar / robotic_fish_7

All mbed code for control over dive planes, pump motor, valve motor, BCUs, UART interface, etc.

Dependencies:   mbed ESC mbed MODDMA

robotic_fish_6/FishController.cpp@0:c3a329a5b05d, 2020-01-14 (annotated)

Committer:
juansal12
Date:
Tue Jan 14 19:17:05 2020 +0000
Revision:
0:c3a329a5b05d
Sofi7 mbed code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
juansal12 0:c3a329a5b05d 1
juansal12 0:c3a329a5b05d 2 #include "FishController.h"
juansal12 0:c3a329a5b05d 3
juansal12 0:c3a329a5b05d 4 // The static instance
juansal12 0:c3a329a5b05d 5 FishController fishController;
juansal12 0:c3a329a5b05d 6
juansal12 0:c3a329a5b05d 7 // Function to reset mbed
juansal12 0:c3a329a5b05d 8 extern "C" void mbed_reset();
juansal12 0:c3a329a5b05d 9
juansal12 0:c3a329a5b05d 10 // Auto mode
juansal12 0:c3a329a5b05d 11 float autoModeCommands[][4] = {FISH_STRAIGHT, FISH_UP, FISH_STRAIGHT, FISH_DOWN};
juansal12 0:c3a329a5b05d 12 uint32_t autoModeDurations[] = {4000, 2000, 2000, 2000}; // durations in milliseconds (is it really? -> check if statement that compares autoModeDurations elements to 10*indexcount
juansal12 0:c3a329a5b05d 13 const uint8_t autoModeLength = sizeof(autoModeDurations)/sizeof(autoModeDurations[0]);
juansal12 0:c3a329a5b05d 14
juansal12 0:c3a329a5b05d 15 //============================================
juansal12 0:c3a329a5b05d 16 // Initialization
juansal12 0:c3a329a5b05d 17 //============================================
juansal12 0:c3a329a5b05d 18
juansal12 0:c3a329a5b05d 19 // Constructor
juansal12 0:c3a329a5b05d 20 FishController::FishController():
juansal12 0:c3a329a5b05d 21 // Initialize variables
juansal12 0:c3a329a5b05d 22 autoMode(false),
juansal12 0:c3a329a5b05d 23 ignoreExternalCommands(false),
juansal12 0:c3a329a5b05d 24 tickerInterval(fishControllerTickerInterval),
juansal12 0:c3a329a5b05d 25 inTickerCallback(false),
juansal12 0:c3a329a5b05d 26 servoLeft(servoLeftPin),
juansal12 0:c3a329a5b05d 27 servoRight(servoRightPin),
juansal12 0:c3a329a5b05d 28 #ifdef FISH4
juansal12 0:c3a329a5b05d 29 curTime(0),
juansal12 0:c3a329a5b05d 30 fullCycle(true),
juansal12 0:c3a329a5b05d 31 raiser(3.5),
juansal12 0:c3a329a5b05d 32 // Outputs for motor and servos
juansal12 0:c3a329a5b05d 33 //motorPWM(motorPWMPin),
juansal12 0:c3a329a5b05d 34 //motorOutA(motorOutAPin),
juansal12 0:c3a329a5b05d 35 //motorOutB(motorOutBPin),
juansal12 0:c3a329a5b05d 36 servoLeft(servoLeftPin),
juansal12 0:c3a329a5b05d 37 servoRight(servoRightPin),
juansal12 0:c3a329a5b05d 38 //brushlessMotor(p25),
juansal12 0:c3a329a5b05d 39 brushlessOffTime(30000),
juansal12 0:c3a329a5b05d 40 #endif
juansal12 0:c3a329a5b05d 41 /* #ifdef FISH6 // these are declared in BCU class
juansal12 0:c3a329a5b05d 42 pressureSensor(pressureSensorPinSDA, pressureSensorPinSCL, ms5837_addr_no_CS),
juansal12 0:c3a329a5b05d 43 imuSensor(imuSensorPinSDA, imuSensorPinSCL)
juansal12 0:c3a329a5b05d 44 #endif*/
juansal12 0:c3a329a5b05d 45 // Button board
juansal12 0:c3a329a5b05d 46 buttonBoard(buttonBoardSDAPin, buttonBoardSCLPin, buttonBoardInt1Pin, buttonBoardInt2Pin) // sda, scl, int1, int2
juansal12 0:c3a329a5b05d 47
juansal12 0:c3a329a5b05d 48 {
juansal12 0:c3a329a5b05d 49 streamFishStateEventController = 0;
juansal12 0:c3a329a5b05d 50
juansal12 0:c3a329a5b05d 51 newSelectButton = resetSelectButtonValue;
juansal12 0:c3a329a5b05d 52 newPitch = resetPitchValue;
juansal12 0:c3a329a5b05d 53 newYaw = resetYawValue;
juansal12 0:c3a329a5b05d 54 newThrust = resetThrustValue;
juansal12 0:c3a329a5b05d 55 newFrequency = resetFrequencyValue;
juansal12 0:c3a329a5b05d 56 newPeriodHalf = resetPeriodHalfValue;
juansal12 0:c3a329a5b05d 57
juansal12 0:c3a329a5b05d 58 selectButton = newSelectButton;
juansal12 0:c3a329a5b05d 59 pitch = newPitch;
juansal12 0:c3a329a5b05d 60 yaw = newYaw;
juansal12 0:c3a329a5b05d 61 thrust = newThrust;
juansal12 0:c3a329a5b05d 62 frequency = newFrequency;
juansal12 0:c3a329a5b05d 63 #ifdef FISH4
juansal12 0:c3a329a5b05d 64 periodHalf = newPeriodHalf;
juansal12 0:c3a329a5b05d 65 thrustCommand = 0;
juansal12 0:c3a329a5b05d 66 dutyCycle = 0;
juansal12 0:c3a329a5b05d 67 brushlessOff = false;
juansal12 0:c3a329a5b05d 68 #endif
juansal12 0:c3a329a5b05d 69
juansal12 0:c3a329a5b05d 70 buttonBoard.registerCallback(&FishController::buttonCallback);
juansal12 0:c3a329a5b05d 71 buttonBoard.setLEDs(255, false);
juansal12 0:c3a329a5b05d 72
juansal12 0:c3a329a5b05d 73 autoModeIndex = 0;
juansal12 0:c3a329a5b05d 74 autoModeCount = 0;
juansal12 0:c3a329a5b05d 75
juansal12 0:c3a329a5b05d 76 }
juansal12 0:c3a329a5b05d 77
juansal12 0:c3a329a5b05d 78 // Set the desired state
juansal12 0:c3a329a5b05d 79 // They will take affect at the next appropriate time in the control cycle
juansal12 0:c3a329a5b05d 80 void FishController::setSelectButton(bool newSelectButtonValue, bool master /* = false*/)
juansal12 0:c3a329a5b05d 81 {
juansal12 0:c3a329a5b05d 82 if(!ignoreExternalCommands || master)
juansal12 0:c3a329a5b05d 83 newSelectButton = newSelectButtonValue;
juansal12 0:c3a329a5b05d 84 }
juansal12 0:c3a329a5b05d 85 void FishController::setPitch(float newPitchValue, bool master /* = false*/)
juansal12 0:c3a329a5b05d 86 {
juansal12 0:c3a329a5b05d 87 if(!ignoreExternalCommands || master)
juansal12 0:c3a329a5b05d 88 {
juansal12 0:c3a329a5b05d 89 newPitch = newPitchValue;
juansal12 0:c3a329a5b05d 90 setLEDs(BTTN_PITCH_UP, (newPitch-fishMinPitch) > (fishMaxPitch - newPitch));
juansal12 0:c3a329a5b05d 91 setLEDs(BTTN_PITCH_DOWN, (newPitch-fishMinPitch) < (fishMaxPitch - newPitch));
juansal12 0:c3a329a5b05d 92 }
juansal12 0:c3a329a5b05d 93 }
juansal12 0:c3a329a5b05d 94 void FishController::setYaw(float newYawValue, bool master /* = false*/)
juansal12 0:c3a329a5b05d 95 {
juansal12 0:c3a329a5b05d 96 if(!ignoreExternalCommands || master)
juansal12 0:c3a329a5b05d 97 {
juansal12 0:c3a329a5b05d 98 newYaw = newYawValue;
juansal12 0:c3a329a5b05d 99 setLEDs(BTTN_YAW_LEFT, (newYaw-fishMinYaw) < (fishMaxYaw - newYaw));
juansal12 0:c3a329a5b05d 100 setLEDs(BTTN_YAW_RIGHT, (newYaw-fishMinYaw) > (fishMaxYaw - newYaw));
juansal12 0:c3a329a5b05d 101 }
juansal12 0:c3a329a5b05d 102 }
juansal12 0:c3a329a5b05d 103 void FishController::setThrust(float newThrustValue, bool master /* = false*/)
juansal12 0:c3a329a5b05d 104 {
juansal12 0:c3a329a5b05d 105 if(!ignoreExternalCommands || master)
juansal12 0:c3a329a5b05d 106 {
juansal12 0:c3a329a5b05d 107 newThrust = newThrustValue;
juansal12 0:c3a329a5b05d 108 setLEDs(BTTN_FASTER, newThrust>fishMinThrust);
juansal12 0:c3a329a5b05d 109 // If we're in button-control mode, keep the no-thrust light on as an indicator
juansal12 0:c3a329a5b05d 110 if(!ignoreExternalCommands)
juansal12 0:c3a329a5b05d 111 setLEDs(BTTN_SLOWER, newThrust==fishMinThrust);
juansal12 0:c3a329a5b05d 112 else
juansal12 0:c3a329a5b05d 113 setLEDs(BTTN_SLOWER, true);
juansal12 0:c3a329a5b05d 114 }
juansal12 0:c3a329a5b05d 115 }
juansal12 0:c3a329a5b05d 116 void FishController::setFrequency(float newFrequencyValue, float newPeriodHalfValue /* = -1 */, bool master /* = false*/)
juansal12 0:c3a329a5b05d 117 {
juansal12 0:c3a329a5b05d 118 if(!ignoreExternalCommands || master)
juansal12 0:c3a329a5b05d 119 {
juansal12 0:c3a329a5b05d 120 newFrequency = newFrequencyValue;
juansal12 0:c3a329a5b05d 121 newPeriodHalf = newPeriodHalfValue > -1 ? newPeriodHalfValue : (1.0/(2.0*newFrequency));
juansal12 0:c3a329a5b05d 122 }
juansal12 0:c3a329a5b05d 123 }
juansal12 0:c3a329a5b05d 124 // Get the (possible pending) state
juansal12 0:c3a329a5b05d 125 bool FishController::getSelectButton() {return newSelectButton;}
juansal12 0:c3a329a5b05d 126 float FishController::getPitch() {return newPitch;}
juansal12 0:c3a329a5b05d 127 float FishController::getYaw() {return newYaw;}
juansal12 0:c3a329a5b05d 128 float FishController::getThrust() {return newThrust;}
juansal12 0:c3a329a5b05d 129 float FishController::getFrequency() {return newFrequency;}
juansal12 0:c3a329a5b05d 130 float FishController::getPeriodHalf() {return newPeriodHalf;}
juansal12 0:c3a329a5b05d 131
juansal12 0:c3a329a5b05d 132 void FishController::start()
juansal12 0:c3a329a5b05d 133 {
juansal12 0:c3a329a5b05d 134
juansal12 0:c3a329a5b05d 135 // Blink button board LEDs to indicate startup
juansal12 0:c3a329a5b05d 136 for(uint8_t i = 0; i < 3; i++)
juansal12 0:c3a329a5b05d 137 {
juansal12 0:c3a329a5b05d 138 buttonBoard.setLEDs(255, true);
juansal12 0:c3a329a5b05d 139 wait_ms(500);
juansal12 0:c3a329a5b05d 140 buttonBoard.setLEDs(255, false);
juansal12 0:c3a329a5b05d 141 wait_ms(500);
juansal12 0:c3a329a5b05d 142 }
juansal12 0:c3a329a5b05d 143
juansal12 0:c3a329a5b05d 144 #ifdef FISH6
juansal12 0:c3a329a5b05d 145 buoyancyControlUnit.start();
juansal12 0:c3a329a5b05d 146 pumpWithValve.start();
juansal12 0:c3a329a5b05d 147 #endif
juansal12 0:c3a329a5b05d 148
juansal12 0:c3a329a5b05d 149 // Start control ticker callback
juansal12 0:c3a329a5b05d 150 ticker.attach_us(&fishController, &FishController::tickerCallback, tickerInterval);
juansal12 0:c3a329a5b05d 151 #ifdef debugFishState
juansal12 0:c3a329a5b05d 152 printf("Starting...\n");
juansal12 0:c3a329a5b05d 153 #endif
juansal12 0:c3a329a5b05d 154
juansal12 0:c3a329a5b05d 155
juansal12 0:c3a329a5b05d 156 }
juansal12 0:c3a329a5b05d 157
juansal12 0:c3a329a5b05d 158 void FishController::stop()
juansal12 0:c3a329a5b05d 159 {
juansal12 0:c3a329a5b05d 160 // Stop updating the fish
juansal12 0:c3a329a5b05d 161 while(inTickerCallback); // wait for commands to settle
juansal12 0:c3a329a5b05d 162 ticker.detach(); // stop updating commands
juansal12 0:c3a329a5b05d 163 wait_ms(5); // wait a bit to make sure it stops
juansal12 0:c3a329a5b05d 164
juansal12 0:c3a329a5b05d 165 // Reset fish state to neutral
juansal12 0:c3a329a5b05d 166 newSelectButton = resetSelectButtonValue;
juansal12 0:c3a329a5b05d 167 newPitch = resetPitchValue;
juansal12 0:c3a329a5b05d 168 newYaw = resetYawValue;
juansal12 0:c3a329a5b05d 169 newThrust = resetThrustValue;
juansal12 0:c3a329a5b05d 170 newFrequency = resetFrequencyValue;
juansal12 0:c3a329a5b05d 171 newPeriodHalf = resetPeriodHalfValue;
juansal12 0:c3a329a5b05d 172 // Send commands to fish (multiple times to make sure we get in the right part of the cycle to actually update it)
juansal12 0:c3a329a5b05d 173 for(int i = 0; i < 200; i++)
juansal12 0:c3a329a5b05d 174 {
juansal12 0:c3a329a5b05d 175 tickerCallback();
juansal12 0:c3a329a5b05d 176 wait_ms(10);
juansal12 0:c3a329a5b05d 177 }
juansal12 0:c3a329a5b05d 178 // Make sure commands are sent to motors and applied
juansal12 0:c3a329a5b05d 179 wait(1);
juansal12 0:c3a329a5b05d 180
juansal12 0:c3a329a5b05d 181 #ifdef FISH4
juansal12 0:c3a329a5b05d 182 // Put dive planes in a weird position to indicate stopped
juansal12 0:c3a329a5b05d 183 servoLeft = 0.3;
juansal12 0:c3a329a5b05d 184 servoRight = 0.3;
juansal12 0:c3a329a5b05d 185 #endif
juansal12 0:c3a329a5b05d 186
juansal12 0:c3a329a5b05d 187 #ifdef FISH6
juansal12 0:c3a329a5b05d 188 pumpWithValve.stop();
juansal12 0:c3a329a5b05d 189 buoyancyControlUnit.stop();
juansal12 0:c3a329a5b05d 190 #endif // FISH6
juansal12 0:c3a329a5b05d 191
juansal12 0:c3a329a5b05d 192
juansal12 0:c3a329a5b05d 193 // Light the LEDs to indicate termination
juansal12 0:c3a329a5b05d 194 buttonBoard.setLEDs(255, true);
juansal12 0:c3a329a5b05d 195 }
juansal12 0:c3a329a5b05d 196
juansal12 0:c3a329a5b05d 197 //============================================
juansal12 0:c3a329a5b05d 198 // Processing
juansal12 0:c3a329a5b05d 199 //============================================
juansal12 0:c3a329a5b05d 200 #ifdef FISH4
juansal12 0:c3a329a5b05d 201 void FishController::tickerCallback()
juansal12 0:c3a329a5b05d 202 {
juansal12 0:c3a329a5b05d 203 inTickerCallback = true; // so we don't asynchronously stop the controller in a bad point of the cycle
juansal12 0:c3a329a5b05d 204
juansal12 0:c3a329a5b05d 205 // get the current elapsed time since last reset (us)
juansal12 0:c3a329a5b05d 206 curTime += tickerInterval;
juansal12 0:c3a329a5b05d 207
juansal12 0:c3a329a5b05d 208 // see if brushless should be shut down
juansal12 0:c3a329a5b05d 209 brushlessOff = curTime > (periodHalf-brushlessOffTime);
juansal12 0:c3a329a5b05d 210
juansal12 0:c3a329a5b05d 211 // update every half cycle
juansal12 0:c3a329a5b05d 212 if(curTime > periodHalf)
juansal12 0:c3a329a5b05d 213 {
juansal12 0:c3a329a5b05d 214 // read new yaw value every half cycle
juansal12 0:c3a329a5b05d 215 yaw = newYaw; // a value from -1 to 1
juansal12 0:c3a329a5b05d 216
juansal12 0:c3a329a5b05d 217 // Read frequency only every full cycle
juansal12 0:c3a329a5b05d 218 if(fullCycle)
juansal12 0:c3a329a5b05d 219 {
juansal12 0:c3a329a5b05d 220 // Read other new inputs
juansal12 0:c3a329a5b05d 221 thrust = newThrust; // a value from 0 to 1
juansal12 0:c3a329a5b05d 222 frequency = newFrequency;
juansal12 0:c3a329a5b05d 223 periodHalf = newPeriodHalf;
juansal12 0:c3a329a5b05d 224 // Adjust thrust if needed
juansal12 0:c3a329a5b05d 225 if(yaw < 0.0)
juansal12 0:c3a329a5b05d 226 thrustCommand = (1.0 + 0.75*yaw)*thrust; // 0.7 can be adjusted to a power of 2 if needed
juansal12 0:c3a329a5b05d 227 else
juansal12 0:c3a329a5b05d 228 thrustCommand = thrust;
juansal12 0:c3a329a5b05d 229 fullCycle = false;
juansal12 0:c3a329a5b05d 230 }
juansal12 0:c3a329a5b05d 231 else
juansal12 0:c3a329a5b05d 232 {
juansal12 0:c3a329a5b05d 233 // Reverse for the downward slope
juansal12 0:c3a329a5b05d 234 if(yaw > 0.0)
juansal12 0:c3a329a5b05d 235 thrustCommand = -(1.0 - 0.75*yaw)*thrust;
juansal12 0:c3a329a5b05d 236 else
juansal12 0:c3a329a5b05d 237 thrustCommand = -thrust;
juansal12 0:c3a329a5b05d 238 fullCycle = true;
juansal12 0:c3a329a5b05d 239 }
juansal12 0:c3a329a5b05d 240
juansal12 0:c3a329a5b05d 241 // Reset time
juansal12 0:c3a329a5b05d 242 curTime = 0;
juansal12 0:c3a329a5b05d 243 }
juansal12 0:c3a329a5b05d 244
juansal12 0:c3a329a5b05d 245 // Update the servos
juansal12 0:c3a329a5b05d 246
juansal12 0:c3a329a5b05d 247 pitch = newPitch;
juansal12 0:c3a329a5b05d 248 servoLeft = pitch - 0.05; // The 0.03 calibrates the angles of the servo
juansal12 0:c3a329a5b05d 249 servoRight = (1.0 - pitch) < 0.03 ? 0.03 : (1.0 - pitch);
juansal12 0:c3a329a5b05d 250
juansal12 0:c3a329a5b05d 251 // Testing whether fishController is running
juansal12 0:c3a329a5b05d 252 // DigitalOut test(LED1);
juansal12 0:c3a329a5b05d 253 // test = 1;
juansal12 0:c3a329a5b05d 254
juansal12 0:c3a329a5b05d 255 // Update the duty cycle
juansal12 0:c3a329a5b05d 256 /*dutyCycle = raiser * sin(PI2 * frequency * curTime); // add factor 4.0 to get a cut off sinus
juansal12 0:c3a329a5b05d 257 if(dutyCycle > 1)
juansal12 0:c3a329a5b05d 258 dutyCycle = 1;
juansal12 0:c3a329a5b05d 259 if(dutyCycle < -1)
juansal12 0:c3a329a5b05d 260 dutyCycle = -1;
juansal12 0:c3a329a5b05d 261 dutyCycle *= thrustCommand;
juansal12 0:c3a329a5b05d 262 if(dutyCycle >= 0 && dutyCycle < 0.01)
juansal12 0:c3a329a5b05d 263 dutyCycle = 0;
juansal12 0:c3a329a5b05d 264 if(dutyCycle < 0 && dutyCycle > -0.01)
juansal12 0:c3a329a5b05d 265 dutyCycle = 0;
juansal12 0:c3a329a5b05d 266 // Update the brushed motor
juansal12 0:c3a329a5b05d 267 if(dutyCycle >= 0)
juansal12 0:c3a329a5b05d 268 {
juansal12 0:c3a329a5b05d 269 motorOutA.write(0);
juansal12 0:c3a329a5b05d 270 motorOutB.write(1);
juansal12 0:c3a329a5b05d 271 motorPWM.write(dutyCycle);
juansal12 0:c3a329a5b05d 272 }
juansal12 0:c3a329a5b05d 273 else
juansal12 0:c3a329a5b05d 274 {
juansal12 0:c3a329a5b05d 275 motorOutA.write(1);
juansal12 0:c3a329a5b05d 276 motorOutB.write(0);
juansal12 0:c3a329a5b05d 277 motorPWM.write(-1 * dutyCycle);
juansal12 0:c3a329a5b05d 278 }*/
juansal12 0:c3a329a5b05d 279 // Update the brushless motor
juansal12 0:c3a329a5b05d 280 //brushlessMotor = dutyCycle * !brushlessOff;
juansal12 0:c3a329a5b05d 281 //brushlessMotor.pulsewidth_us(dutyCycle*500+1500);
juansal12 0:c3a329a5b05d 282 //brushlessMotor();
juansal12 0:c3a329a5b05d 283
juansal12 0:c3a329a5b05d 284
juansal12 0:c3a329a5b05d 285 #ifdef debugFishState
juansal12 0:c3a329a5b05d 286 printDebugState();
juansal12 0:c3a329a5b05d 287 #endif
juansal12 0:c3a329a5b05d 288 //printf("%f\n", dutyCycle);
juansal12 0:c3a329a5b05d 289 //printf("%f %f\r\n", pitch, servoLeft.read());
juansal12 0:c3a329a5b05d 290 inTickerCallback = false;
juansal12 0:c3a329a5b05d 291 }
juansal12 0:c3a329a5b05d 292 #endif
juansal12 0:c3a329a5b05d 293
juansal12 0:c3a329a5b05d 294 #ifdef FISH6
juansal12 0:c3a329a5b05d 295 void FishController::tickerCallback()
juansal12 0:c3a329a5b05d 296 {
juansal12 0:c3a329a5b05d 297 inTickerCallback = true; // so we don't asynchronously stop the controller in a bad point of the cycle
juansal12 0:c3a329a5b05d 298
juansal12 0:c3a329a5b05d 299 //set current state to newly commanded value
juansal12 0:c3a329a5b05d 300 frequency = newFrequency;
juansal12 0:c3a329a5b05d 301 yaw = newYaw;
juansal12 0:c3a329a5b05d 302 thrust = newThrust;
juansal12 0:c3a329a5b05d 303 pitch = newPitch;
juansal12 0:c3a329a5b05d 304
juansal12 0:c3a329a5b05d 305 // Update dive planes
juansal12 0:c3a329a5b05d 306 servoLeft = pitch - 0.05; // The 0.03 calibrates the angles of the servo
juansal12 0:c3a329a5b05d 307 servoRight = (1.0 - pitch) < 0.03 ? 0.03 : (1.0 - pitch);
juansal12 0:c3a329a5b05d 308
juansal12 0:c3a329a5b05d 309 pumpWithValve.set(frequency, yaw, thrust);
juansal12 0:c3a329a5b05d 310
juansal12 0:c3a329a5b05d 311 /* TURNING OFF BCU FOR FIRST OPEN WORLD TEST - AUGUST 21, 2019*/
juansal12 0:c3a329a5b05d 312 //buoyancyControlUnit.set(pitch); //1100 - 1180 seems to follow well
juansal12 0:c3a329a5b05d 313
juansal12 0:c3a329a5b05d 314 //Testing whether fishController.tickerCallback() is running
juansal12 0:c3a329a5b05d 315 //DigitalOut test(LED1);
juansal12 0:c3a329a5b05d 316 //test = 1;
juansal12 0:c3a329a5b05d 317
juansal12 0:c3a329a5b05d 318
juansal12 0:c3a329a5b05d 319 #ifdef debugFishState
juansal12 0:c3a329a5b05d 320 printDebugState();
juansal12 0:c3a329a5b05d 321 #endif
juansal12 0:c3a329a5b05d 322
juansal12 0:c3a329a5b05d 323 inTickerCallback = false;
juansal12 0:c3a329a5b05d 324 }
juansal12 0:c3a329a5b05d 325 #endif
juansal12 0:c3a329a5b05d 326
juansal12 0:c3a329a5b05d 327
juansal12 0:c3a329a5b05d 328 // button will be mask indicating which button triggered this interrupt
juansal12 0:c3a329a5b05d 329 // pressed will indicate whether that button was pressed or released
juansal12 0:c3a329a5b05d 330 // buttonState will be a mask that indicates which buttons are currently pressed
juansal12 0:c3a329a5b05d 331 void FishController::buttonCallback(char button, bool pressed, char state) // static
juansal12 0:c3a329a5b05d 332 {
juansal12 0:c3a329a5b05d 333 //printf("button %d\t pressed: %d\t state: %d\n", button, pressed, state);
juansal12 0:c3a329a5b05d 334 //fishController.buttonBoard.setLEDs(button, !fishController.buttonBoard.getLEDs(button));
juansal12 0:c3a329a5b05d 335 // Only act on button presses (not releases)
juansal12 0:c3a329a5b05d 336 if(!pressed)
juansal12 0:c3a329a5b05d 337 return;
juansal12 0:c3a329a5b05d 338
juansal12 0:c3a329a5b05d 339 DigitalOut* simBatteryLow;
juansal12 0:c3a329a5b05d 340 float newYaw, newThrust, newPitch;
juansal12 0:c3a329a5b05d 341 switch(state)
juansal12 0:c3a329a5b05d 342 {
juansal12 0:c3a329a5b05d 343 case BTTN_YAW_LEFT:
juansal12 0:c3a329a5b05d 344 newYaw = fishController.newYaw;
juansal12 0:c3a329a5b05d 345 newYaw -= (fishMaxYaw - fishMinYaw)/4.0;
juansal12 0:c3a329a5b05d 346 newYaw = newYaw < fishMinYaw ? fishMinYaw : newYaw;
juansal12 0:c3a329a5b05d 347 fishController.setYaw(newYaw, true);
juansal12 0:c3a329a5b05d 348 fishController.streamFishStateEventController = 6;
juansal12 0:c3a329a5b05d 349 break;
juansal12 0:c3a329a5b05d 350 case BTTN_YAW_RIGHT:
juansal12 0:c3a329a5b05d 351 newYaw = fishController.newYaw;
juansal12 0:c3a329a5b05d 352 newYaw += (fishMaxYaw - fishMinYaw)/4.0;
juansal12 0:c3a329a5b05d 353 newYaw = newYaw > fishMaxYaw ? fishMaxYaw : newYaw;
juansal12 0:c3a329a5b05d 354 fishController.setYaw(newYaw, true);
juansal12 0:c3a329a5b05d 355 fishController.streamFishStateEventController = 7;
juansal12 0:c3a329a5b05d 356 break;
juansal12 0:c3a329a5b05d 357 case BTTN_FASTER:
juansal12 0:c3a329a5b05d 358 newThrust = fishController.newThrust;
juansal12 0:c3a329a5b05d 359 newThrust += (fishMaxThrust - fishMinThrust)/4.0;
juansal12 0:c3a329a5b05d 360 newThrust = newThrust > fishMaxThrust ? fishMaxThrust : newThrust;
juansal12 0:c3a329a5b05d 361 fishController.setThrust(newThrust, true);
juansal12 0:c3a329a5b05d 362 fishController.streamFishStateEventController = 8;
juansal12 0:c3a329a5b05d 363 break;
juansal12 0:c3a329a5b05d 364 case BTTN_SLOWER:
juansal12 0:c3a329a5b05d 365 newThrust = fishController.newThrust;
juansal12 0:c3a329a5b05d 366 newThrust -= (fishMaxThrust - fishMinThrust)/4.0;
juansal12 0:c3a329a5b05d 367 newThrust = newThrust < fishMinThrust ? fishMinThrust : newThrust;
juansal12 0:c3a329a5b05d 368 fishController.setThrust(newThrust, true);
juansal12 0:c3a329a5b05d 369 fishController.streamFishStateEventController = 9;
juansal12 0:c3a329a5b05d 370 break;
juansal12 0:c3a329a5b05d 371 case BTTN_PITCH_UP:
juansal12 0:c3a329a5b05d 372 newPitch = fishController.newPitch;
juansal12 0:c3a329a5b05d 373 newPitch += (fishMaxPitch - fishMinPitch)/4.0;
juansal12 0:c3a329a5b05d 374 newPitch = newPitch > fishMaxPitch ? fishMaxPitch : newPitch;
juansal12 0:c3a329a5b05d 375 fishController.setPitch(newPitch, true);
juansal12 0:c3a329a5b05d 376 fishController.streamFishStateEventController = 10;
juansal12 0:c3a329a5b05d 377 break;
juansal12 0:c3a329a5b05d 378 case BTTN_PITCH_DOWN:
juansal12 0:c3a329a5b05d 379 newPitch = fishController.newPitch;
juansal12 0:c3a329a5b05d 380 newPitch -= (fishMaxPitch - fishMinPitch)/4.0;
juansal12 0:c3a329a5b05d 381 newPitch = newPitch < fishMinPitch ? fishMinPitch : newPitch;
juansal12 0:c3a329a5b05d 382 fishController.setPitch(newPitch, true);
juansal12 0:c3a329a5b05d 383 fishController.streamFishStateEventController = 11;
juansal12 0:c3a329a5b05d 384 break;
juansal12 0:c3a329a5b05d 385 case BTTN_SHUTDOWN_PI: // signal a low battery signal to trigger the pi to shutdown
juansal12 0:c3a329a5b05d 386 fishController.streamFishStateEventController = 12;
juansal12 0:c3a329a5b05d 387 simBatteryLow = new DigitalOut(lowBatteryVoltagePin);
juansal12 0:c3a329a5b05d 388 simBatteryLow->write(0);
juansal12 0:c3a329a5b05d 389 break;
juansal12 0:c3a329a5b05d 390 case BTTN_RESET_MBED:
juansal12 0:c3a329a5b05d 391 fishController.streamFishStateEventController = 13; // ... if you see this, it didn't happen :)
juansal12 0:c3a329a5b05d 392 mbed_reset();
juansal12 0:c3a329a5b05d 393 break;
juansal12 0:c3a329a5b05d 394 case BTTN_AUTO_MODE:
juansal12 0:c3a329a5b05d 395 fishController.streamFishStateEventController = 14;
juansal12 0:c3a329a5b05d 396 if(fishController.autoMode)
juansal12 0:c3a329a5b05d 397 fishController.stopAutoMode();
juansal12 0:c3a329a5b05d 398 else
juansal12 0:c3a329a5b05d 399 fishController.startAutoMode();
juansal12 0:c3a329a5b05d 400 break;
juansal12 0:c3a329a5b05d 401 case BTTN_BTTN_MODE:
juansal12 0:c3a329a5b05d 402 fishController.setIgnoreExternalCommands(!fishController.getIgnoreExternalCommands());
juansal12 0:c3a329a5b05d 403 break;
juansal12 0:c3a329a5b05d 404 default:
juansal12 0:c3a329a5b05d 405 fishController.streamFishStateEventController = 15;
juansal12 0:c3a329a5b05d 406 break;
juansal12 0:c3a329a5b05d 407 }
juansal12 0:c3a329a5b05d 408 }
juansal12 0:c3a329a5b05d 409
juansal12 0:c3a329a5b05d 410 void FishController::setIgnoreExternalCommands(bool ignore)
juansal12 0:c3a329a5b05d 411 {
juansal12 0:c3a329a5b05d 412 ignoreExternalCommands = ignore;
juansal12 0:c3a329a5b05d 413 }
juansal12 0:c3a329a5b05d 414
juansal12 0:c3a329a5b05d 415 bool FishController::getIgnoreExternalCommands()
juansal12 0:c3a329a5b05d 416 {
juansal12 0:c3a329a5b05d 417 return ignoreExternalCommands;
juansal12 0:c3a329a5b05d 418 }
juansal12 0:c3a329a5b05d 419
juansal12 0:c3a329a5b05d 420 void FishController::startAutoMode()
juansal12 0:c3a329a5b05d 421 {
juansal12 0:c3a329a5b05d 422 // Start ignoring external commands so as not to interfere with auto mode
juansal12 0:c3a329a5b05d 423 // But remember what the previous setting was so we can restore it after auto mode
juansal12 0:c3a329a5b05d 424 ignoreExternalCommandsPreAutoMode = ignoreExternalCommands;
juansal12 0:c3a329a5b05d 425 setIgnoreExternalCommands(true);
juansal12 0:c3a329a5b05d 426 // Reset state
juansal12 0:c3a329a5b05d 427 autoModeCount = 0;
juansal12 0:c3a329a5b05d 428 autoModeIndex = 0;
juansal12 0:c3a329a5b05d 429 // Start executing the auto loop
juansal12 0:c3a329a5b05d 430 autoMode = true;
juansal12 0:c3a329a5b05d 431 autoModeTicker.attach_us(&fishController, &FishController::autoModeCallback, 10000);
juansal12 0:c3a329a5b05d 432 }
juansal12 0:c3a329a5b05d 433
juansal12 0:c3a329a5b05d 434 void FishController::stopAutoMode()
juansal12 0:c3a329a5b05d 435 {
juansal12 0:c3a329a5b05d 436 autoModeTicker.detach();
juansal12 0:c3a329a5b05d 437 // Auto mode was terminated - put fish into a neutral position
juansal12 0:c3a329a5b05d 438 setSelectButton(resetSelectButtonValue, true);
juansal12 0:c3a329a5b05d 439 setPitch(resetPitchValue, true);
juansal12 0:c3a329a5b05d 440 setYaw(resetYawValue, true);
juansal12 0:c3a329a5b05d 441 setThrust(resetThrustValue, true);
juansal12 0:c3a329a5b05d 442 setFrequency(resetFrequencyValue, resetPeriodHalfValue, true);
juansal12 0:c3a329a5b05d 443 // Restore external mode to what is was previously
juansal12 0:c3a329a5b05d 444 setIgnoreExternalCommands(ignoreExternalCommandsPreAutoMode);
juansal12 0:c3a329a5b05d 445 autoMode = false;
juansal12 0:c3a329a5b05d 446 }
juansal12 0:c3a329a5b05d 447
juansal12 0:c3a329a5b05d 448 void FishController::autoModeCallback()
juansal12 0:c3a329a5b05d 449 {
juansal12 0:c3a329a5b05d 450 // Assign the current state (stored as pitch, yaw, thrust, frequency)
juansal12 0:c3a329a5b05d 451 setPitch(autoModeCommands[autoModeIndex][0], true);
juansal12 0:c3a329a5b05d 452 setYaw(autoModeCommands[autoModeIndex][1], true);
juansal12 0:c3a329a5b05d 453 setThrust(autoModeCommands[autoModeIndex][2], true);
juansal12 0:c3a329a5b05d 454 setFrequency(autoModeCommands[autoModeIndex][3], 1.0/(2.0*autoModeCommands[autoModeIndex][3]), true);
juansal12 0:c3a329a5b05d 455 // See if we advance to the next command
juansal12 0:c3a329a5b05d 456 autoModeCount++;
juansal12 0:c3a329a5b05d 457 if(autoModeCount*10 > autoModeDurations[autoModeIndex])
juansal12 0:c3a329a5b05d 458 {
juansal12 0:c3a329a5b05d 459 autoModeCount = 0;
juansal12 0:c3a329a5b05d 460 autoModeIndex = (autoModeIndex+1) % autoModeLength; // loop continuously through commands
juansal12 0:c3a329a5b05d 461 }
juansal12 0:c3a329a5b05d 462 }
juansal12 0:c3a329a5b05d 463
juansal12 0:c3a329a5b05d 464 #ifdef debugFishState
juansal12 0:c3a329a5b05d 465 void FishController::printDebugState()
juansal12 0:c3a329a5b05d 466 {
juansal12 0:c3a329a5b05d 467 printf("pitch: %f yaw: %f thrust: %f frequency: %.8f\r\n",
juansal12 0:c3a329a5b05d 468 pitch, yaw, thrust, frequency);
juansal12 0:c3a329a5b05d 469 }
juansal12 0:c3a329a5b05d 470
juansal12 0:c3a329a5b05d 471 #endif
juansal12 0:c3a329a5b05d 472
juansal12 0:c3a329a5b05d 473 void FishController::setLEDs(char mask, bool turnOn)
juansal12 0:c3a329a5b05d 474 {
juansal12 0:c3a329a5b05d 475 buttonBoard.setLEDs(mask, turnOn);
juansal12 0:c3a329a5b05d 476 }
juansal12 0:c3a329a5b05d 477
juansal12 0:c3a329a5b05d 478 #ifdef debugBCU
juansal12 0:c3a329a5b05d 479 /* BCU + Pressure Sensor Helper Functions */
juansal12 0:c3a329a5b05d 480
juansal12 0:c3a329a5b05d 481 float FishController::getBCUVset(){ return buoyancyControlUnit.getVset(); }
juansal12 0:c3a329a5b05d 482 float FishController::getBCUSetDepth(){ return buoyancyControlUnit.getSetDepth(); }
juansal12 0:c3a329a5b05d 483 float FishController::getBCUCurDepth(){ return buoyancyControlUnit.getCurDepth(); }
juansal12 0:c3a329a5b05d 484 float FishController::getBCUSetPos(){ return buoyancyControlUnit.getSetPos(); }
juansal12 0:c3a329a5b05d 485 float FishController::getBCUCurPos(){ return buoyancyControlUnit.getCurPos(); }
juansal12 0:c3a329a5b05d 486 float FishController::getreadPressure(){ return buoyancyControlUnit.readPressure(); }
juansal12 0:c3a329a5b05d 487
juansal12 0:c3a329a5b05d 488 #endif