Juan Salazar / robotic_fish_6

test2

Dependencies:   MODDMA mbed sofi7

robotic_fish_6/FishController.cpp@3:16cf1cb2ebfd, 2021-12-05 (annotated)

Committer:
arizonat
Date:
Sun Dec 05 18:19:53 2021 +0000
Revision:
3:16cf1cb2ebfd
Parent:
2:7ca59d64e460 
Added period to define statements for easy change and some bug hack fixes also moved hall effect to different pin from pumpvalve

Who changed what in which revision?

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