Juan Salazar / robotic_fish_6

test2

Dependencies:   MODDMA mbed sofi7

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