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.h@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 * AcousticController.h
juansal12 0:c3a329a5b05d 3 * Author: Joseph DelPreto
juansal12 0:c3a329a5b05d 4 */
juansal12 0:c3a329a5b05d 5
juansal12 0:c3a329a5b05d 6 #ifndef FISH_CONTROLLER_H
juansal12 0:c3a329a5b05d 7 #define FISH_CONTROLLER_H
juansal12 0:c3a329a5b05d 8
juansal12 0:c3a329a5b05d 9 // comment out if no debug wanted
juansal12 0:c3a329a5b05d 10 #define debugFishState
juansal12 0:c3a329a5b05d 11
juansal12 0:c3a329a5b05d 12 // Fish version (only define one of them)
juansal12 0:c3a329a5b05d 13 #define FISH6
juansal12 0:c3a329a5b05d 14 //#define FISH4
juansal12 0:c3a329a5b05d 15
juansal12 0:c3a329a5b05d 16 #include "mbed.h"
juansal12 0:c3a329a5b05d 17 #include "string"
juansal12 0:c3a329a5b05d 18 #include "ButtonBoard.h"
juansal12 0:c3a329a5b05d 19 #ifdef FISH4
juansal12 0:c3a329a5b05d 20 #include "Servo.h"
juansal12 0:c3a329a5b05d 21 #include "esc.h" // brushless motor controller
juansal12 0:c3a329a5b05d 22 #endif
juansal12 0:c3a329a5b05d 23 #ifdef FISH6
juansal12 0:c3a329a5b05d 24 #include "Servo.h"
juansal12 0:c3a329a5b05d 25 #include "PumpWithValve/PumpWithValve.h"
juansal12 0:c3a329a5b05d 26 #include "BuoyancyControlUnit/BuoyancyControlUnit.h"
juansal12 0:c3a329a5b05d 27 #endif
juansal12 0:c3a329a5b05d 28
juansal12 0:c3a329a5b05d 29
juansal12 0:c3a329a5b05d 30 // Control
juansal12 0:c3a329a5b05d 31 #define fishControllerTickerInterval 1000 // how often to call the control ticker, in microseconds
juansal12 0:c3a329a5b05d 32
juansal12 0:c3a329a5b05d 33 // Constants
juansal12 0:c3a329a5b05d 34 #define PI2 6.2831853 // PI is not included with math.h for some reason
juansal12 0:c3a329a5b05d 35 // Values to use for resetting the fish to neutral
juansal12 0:c3a329a5b05d 36 #define resetSelectButtonValue 0
juansal12 0:c3a329a5b05d 37 #define resetPitchValue 0.5
juansal12 0:c3a329a5b05d 38 #define resetYawValue 0
juansal12 0:c3a329a5b05d 39 #define resetThrustValue 0
juansal12 0:c3a329a5b05d 40 #define resetFrequencyValue 0.0000012 // cycles/us
juansal12 0:c3a329a5b05d 41 #define resetPeriodHalfValue 416666 // 1/(2*frequency) -> us
juansal12 0:c3a329a5b05d 42
juansal12 0:c3a329a5b05d 43 // Value ranges
juansal12 0:c3a329a5b05d 44 #ifdef FISH4
juansal12 0:c3a329a5b05d 45 #define fishMinPitch ((float)(0.2)) // will want to redefine for fish 6 based on depth instead
juansal12 0:c3a329a5b05d 46 #define fishMaxPitch ((float)(0.8))
juansal12 0:c3a329a5b05d 47 #endif
juansal12 0:c3a329a5b05d 48
juansal12 0:c3a329a5b05d 49 #ifdef FISH6
juansal12 0:c3a329a5b05d 50 #define fishMinPitch ((float)(0.2)) // will want to redefine for fish 6 based on depth instead
juansal12 0:c3a329a5b05d 51 #define fishMaxPitch ((float)(0.8))
juansal12 0:c3a329a5b05d 52 //#define fishMinPitch ((float)(0.0)) // seems appropriate for BCUs
juansal12 0:c3a329a5b05d 53 //#define fishMaxPitch ((float)(30.0))
juansal12 0:c3a329a5b05d 54 #endif
juansal12 0:c3a329a5b05d 55
juansal12 0:c3a329a5b05d 56 #define fishMinYaw ((float)(-1.0))
juansal12 0:c3a329a5b05d 57 #define fishMaxYaw ((float)(1.0))
juansal12 0:c3a329a5b05d 58
juansal12 0:c3a329a5b05d 59 #define fishMinThrust ((float)(0.0))
juansal12 0:c3a329a5b05d 60 #ifdef FISH4
juansal12 0:c3a329a5b05d 61 #define fishMaxThrust ((float)(0.75))
juansal12 0:c3a329a5b05d 62 #endif
juansal12 0:c3a329a5b05d 63 #ifdef FISH6
juansal12 0:c3a329a5b05d 64 #define fishMaxThrust ((float)(1.0))
juansal12 0:c3a329a5b05d 65 #endif
juansal12 0:c3a329a5b05d 66
juansal12 0:c3a329a5b05d 67 #define fishMinFrequency ((float)(0.0000009))
juansal12 0:c3a329a5b05d 68 #define fishMaxFrequency ((float)(0.0000016))
juansal12 0:c3a329a5b05d 69
juansal12 0:c3a329a5b05d 70 // Preset states for auto mode definition
juansal12 0:c3a329a5b05d 71 // Each one is pitch, yaw, thrust, frequency
juansal12 0:c3a329a5b05d 72 #define FISH_STRAIGHT {resetPitchValue, resetYawValue , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
juansal12 0:c3a329a5b05d 73 #define FISH_UP {fishMaxPitch , resetYawValue , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
juansal12 0:c3a329a5b05d 74 #define FISH_DOWN {fishMinPitch , resetYawValue , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
juansal12 0:c3a329a5b05d 75 #define FISH_LEFT {resetPitchValue, fishMaxYaw , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
juansal12 0:c3a329a5b05d 76 #define FISH_RIGHT {resetPitchValue, fishMinYaw , (fishMaxThrust + fishMinThrust)/2.0 , (fishMaxFrequency + fishMinFrequency)/2.0}
juansal12 0:c3a329a5b05d 77 #define FISH_STOP {resetPitchValue, resetYawValue , resetThrustValue , resetFrequencyValue}
juansal12 0:c3a329a5b05d 78
juansal12 0:c3a329a5b05d 79 // Pins
juansal12 0:c3a329a5b05d 80 #define lowBatteryVoltagePin p16
juansal12 0:c3a329a5b05d 81
juansal12 0:c3a329a5b05d 82 #ifdef FISH4
juansal12 0:c3a329a5b05d 83 #define motorPWMPin p23
juansal12 0:c3a329a5b05d 84 #define motorOutAPin p11
juansal12 0:c3a329a5b05d 85 #define motorOutBPin p12
juansal12 0:c3a329a5b05d 86 #define servoLeftPin p21
juansal12 0:c3a329a5b05d 87 #define servoRightPin p26 //p24
juansal12 0:c3a329a5b05d 88 #endif
juansal12 0:c3a329a5b05d 89
juansal12 0:c3a329a5b05d 90 #ifdef FISH6
juansal12 0:c3a329a5b05d 91 // NOTE: FISH6 pins are defined in BCU and Valve classes
juansal12 0:c3a329a5b05d 92 #define pressureSensorPinSDA p28
juansal12 0:c3a329a5b05d 93 #define pressureSensorPinSCL p27
juansal12 0:c3a329a5b05d 94 #define imuSensorPinSDA p28
juansal12 0:c3a329a5b05d 95 #define imuSensorPinSCL p27
juansal12 0:c3a329a5b05d 96 #define servoLeftPin p21
juansal12 0:c3a329a5b05d 97 #define servoRightPin p26
juansal12 0:c3a329a5b05d 98 #endif
juansal12 0:c3a329a5b05d 99
juansal12 0:c3a329a5b05d 100
juansal12 0:c3a329a5b05d 101 #define buttonBoardSDAPin p9
juansal12 0:c3a329a5b05d 102 #define buttonBoardSCLPin p10
juansal12 0:c3a329a5b05d 103 #define buttonBoardInt1Pin p29
juansal12 0:c3a329a5b05d 104 #define buttonBoardInt2Pin p30
juansal12 0:c3a329a5b05d 105
juansal12 0:c3a329a5b05d 106 /* Button board commands
juansal12 0:c3a329a5b05d 107 Commented indexes go from top left (0) to bottom right (5) as follows:
juansal12 0:c3a329a5b05d 108 /=========================|
juansal12 0:c3a329a5b05d 109 / ______________________ |
juansal12 0:c3a329a5b05d 110 / | (0:8) (1:16) (2:32) | |
juansal12 0:c3a329a5b05d 111 fish nose | |(3:1) (4:2) (5:4) | | fish tail
juansal12 0:c3a329a5b05d 112 \ ----------------------| |
juansal12 0:c3a329a5b05d 113 \ |
juansal12 0:c3a329a5b05d 114 \=========================|
juansal12 0:c3a329a5b05d 115 The numbers after the colons are the values to use for that button
juansal12 0:c3a329a5b05d 116 */
juansal12 0:c3a329a5b05d 117 #define BTTN_FASTER 1 // 3
juansal12 0:c3a329a5b05d 118 #define BTTN_SLOWER 2 // 4
juansal12 0:c3a329a5b05d 119 #define BTTN_YAW_LEFT 4 // 5
juansal12 0:c3a329a5b05d 120 #define BTTN_YAW_RIGHT 8 // 0
juansal12 0:c3a329a5b05d 121 #define BTTN_PITCH_UP 16 // 1 // swims down
juansal12 0:c3a329a5b05d 122 #define BTTN_PITCH_DOWN 32 // 2 // swims up
juansal12 0:c3a329a5b05d 123 #define BTTN_RESET_MBED 36 // 2 and 5
juansal12 0:c3a329a5b05d 124 #define BTTN_SHUTDOWN_PI 9 // 0 and 3
juansal12 0:c3a329a5b05d 125 #define BTTN_AUTO_MODE 33 // 2 and 3
juansal12 0:c3a329a5b05d 126 #define BTTN_BTTN_MODE 12 // 0 and 5
juansal12 0:c3a329a5b05d 127
juansal12 0:c3a329a5b05d 128
juansal12 0:c3a329a5b05d 129 class FishController
juansal12 0:c3a329a5b05d 130 {
juansal12 0:c3a329a5b05d 131 public:
juansal12 0:c3a329a5b05d 132 // Initialization
juansal12 0:c3a329a5b05d 133 FishController();
juansal12 0:c3a329a5b05d 134 void start();
juansal12 0:c3a329a5b05d 135 void stop();
juansal12 0:c3a329a5b05d 136 // Processing
juansal12 0:c3a329a5b05d 137 void tickerCallback();
juansal12 0:c3a329a5b05d 138 // Debug / Logging
juansal12 0:c3a329a5b05d 139 volatile uint8_t streamFishStateEventController; // will indicate the last button board event - up to the caller to reset it if desired
juansal12 0:c3a329a5b05d 140 #ifdef debugFishState
juansal12 0:c3a329a5b05d 141 void printDebugState();
juansal12 0:c3a329a5b05d 142 #endif
juansal12 0:c3a329a5b05d 143 // LEDs
juansal12 0:c3a329a5b05d 144 void setLEDs(char mask, bool turnOn);
juansal12 0:c3a329a5b05d 145 // Set New State (which will take affect at next appropriate point in control cycle)
juansal12 0:c3a329a5b05d 146 void setSelectButton(bool newSelectButtonValue, bool master= false);
juansal12 0:c3a329a5b05d 147 void setPitch(float newPitchValue, bool master = false);
juansal12 0:c3a329a5b05d 148 void setYaw(float newYawValue, bool master = false);
juansal12 0:c3a329a5b05d 149 void setThrust(float newThrustValue, bool master = false);
juansal12 0:c3a329a5b05d 150 void setFrequency(float newFrequencyValue, float newPeriodHalfValue = -1, bool master = false);
juansal12 0:c3a329a5b05d 151 // Get (possible pending) State
juansal12 0:c3a329a5b05d 152 bool getSelectButton();
juansal12 0:c3a329a5b05d 153 float getPitch();
juansal12 0:c3a329a5b05d 154 float getYaw();
juansal12 0:c3a329a5b05d 155 float getThrust();
juansal12 0:c3a329a5b05d 156 float getFrequency();
juansal12 0:c3a329a5b05d 157 float getPeriodHalf();
juansal12 0:c3a329a5b05d 158 // Auto mode
juansal12 0:c3a329a5b05d 159 volatile bool autoMode;
juansal12 0:c3a329a5b05d 160 void startAutoMode();
juansal12 0:c3a329a5b05d 161 void stopAutoMode();
juansal12 0:c3a329a5b05d 162 void autoModeCallback();
juansal12 0:c3a329a5b05d 163
juansal12 0:c3a329a5b05d 164 void setIgnoreExternalCommands(bool ignore);
juansal12 0:c3a329a5b05d 165 bool getIgnoreExternalCommands();
juansal12 0:c3a329a5b05d 166
juansal12 0:c3a329a5b05d 167 // BCU Helper Functions
juansal12 0:c3a329a5b05d 168 float getBCUVset();
juansal12 0:c3a329a5b05d 169 float getBCUSetDepth();
juansal12 0:c3a329a5b05d 170 float getBCUCurDepth();
juansal12 0:c3a329a5b05d 171 float getBCUSetPos();
juansal12 0:c3a329a5b05d 172 float getBCUCurPos();
juansal12 0:c3a329a5b05d 173 float getreadPressure();
juansal12 0:c3a329a5b05d 174
juansal12 0:c3a329a5b05d 175 private:
juansal12 0:c3a329a5b05d 176 // Misc State
juansal12 0:c3a329a5b05d 177 volatile bool ignoreExternalCommands;
juansal12 0:c3a329a5b05d 178 // Ticker for controlling tail
juansal12 0:c3a329a5b05d 179 Ticker ticker;
juansal12 0:c3a329a5b05d 180 const uint16_t tickerInterval;
juansal12 0:c3a329a5b05d 181 volatile bool inTickerCallback;
juansal12 0:c3a329a5b05d 182
juansal12 0:c3a329a5b05d 183 // State which will be applied at the next appropriate time in the control cycle
juansal12 0:c3a329a5b05d 184 volatile bool newSelectButton;
juansal12 0:c3a329a5b05d 185 volatile float newPitch;
juansal12 0:c3a329a5b05d 186 volatile float newYaw;
juansal12 0:c3a329a5b05d 187 volatile float newThrust;
juansal12 0:c3a329a5b05d 188 volatile float newFrequency;
juansal12 0:c3a329a5b05d 189 volatile float newPeriodHalf;
juansal12 0:c3a329a5b05d 190
juansal12 0:c3a329a5b05d 191 // State currently executing on fish
juansal12 0:c3a329a5b05d 192 volatile bool selectButton;
juansal12 0:c3a329a5b05d 193 volatile float pitch;
juansal12 0:c3a329a5b05d 194 volatile float yaw;
juansal12 0:c3a329a5b05d 195 volatile float thrust;
juansal12 0:c3a329a5b05d 196 volatile float frequency;
juansal12 0:c3a329a5b05d 197
juansal12 0:c3a329a5b05d 198 // Servos (Fish 6)
juansal12 0:c3a329a5b05d 199 Servo servoLeft;
juansal12 0:c3a329a5b05d 200 Servo servoRight;
juansal12 0:c3a329a5b05d 201
juansal12 0:c3a329a5b05d 202
juansal12 0:c3a329a5b05d 203 #ifdef FISH4
juansal12 0:c3a329a5b05d 204 volatile float thrustCommand;
juansal12 0:c3a329a5b05d 205 volatile float periodHalf;
juansal12 0:c3a329a5b05d 206 volatile float dutyCycle;
juansal12 0:c3a329a5b05d 207 volatile bool brushlessOff;
juansal12 0:c3a329a5b05d 208 volatile uint32_t curTime;
juansal12 0:c3a329a5b05d 209 volatile bool fullCycle;
juansal12 0:c3a329a5b05d 210 const float raiser;
juansal12 0:c3a329a5b05d 211 // Outputs for motor and servos
juansal12 0:c3a329a5b05d 212 //PwmOut motorPWM;
juansal12 0:c3a329a5b05d 213 //DigitalOut motorOutA;
juansal12 0:c3a329a5b05d 214 //DigitalOut motorOutB;
juansal12 0:c3a329a5b05d 215 Servo servoLeft;
juansal12 0:c3a329a5b05d 216 Servo servoRight;
juansal12 0:c3a329a5b05d 217 //PwmOut brushlessMotor;
juansal12 0:c3a329a5b05d 218 const uint32_t brushlessOffTime;
juansal12 0:c3a329a5b05d 219 #endif
juansal12 0:c3a329a5b05d 220
juansal12 0:c3a329a5b05d 221 // Button control
juansal12 0:c3a329a5b05d 222 ButtonBoard buttonBoard;
juansal12 0:c3a329a5b05d 223 static void buttonCallback(char button, bool pressed, char state);
juansal12 0:c3a329a5b05d 224
juansal12 0:c3a329a5b05d 225 // Auto mode
juansal12 0:c3a329a5b05d 226 Ticker autoModeTicker;
juansal12 0:c3a329a5b05d 227 uint32_t autoModeCount;
juansal12 0:c3a329a5b05d 228 uint16_t autoModeIndex;
juansal12 0:c3a329a5b05d 229 bool ignoreExternalCommandsPreAutoMode;
juansal12 0:c3a329a5b05d 230 };
juansal12 0:c3a329a5b05d 231
juansal12 0:c3a329a5b05d 232 // Create a static instance of FishController to be used by anyone doing detection
juansal12 0:c3a329a5b05d 233 extern FishController fishController;
juansal12 0:c3a329a5b05d 234 extern volatile uint8_t streamFishStateEvent;
juansal12 0:c3a329a5b05d 235 extern volatile uint16_t streamCurFishState;
juansal12 0:c3a329a5b05d 236
juansal12 0:c3a329a5b05d 237 #endif // ifndef FISH_CONTROLLER_H