FishController.h

00001 /*
00002  * AcousticController.h
00003  * Author: Joseph DelPreto
00004  */
00005 
00006 #ifndef FISH_CONTROLLER_H
00007 #define FISH_CONTROLLER_H
00008 
00009 // comment out if no debug wanted
00010 #define debugFishState
00011 
00012 // Fish version (only define one of them)
00013 #define FISH6
00014 //#define FISH4
00015 
00016 #include "mbed.h"
00017 #include "string"
00018 #include "ButtonBoard.h"
00019 #ifdef FISH4
00020 #include "Servo.h"
00021 #include "esc.h" // brushless motor controller
00022 #endif
00023 #ifdef FISH6
00024 #include "Servo.h"
00025 #include "PumpWithValve/PumpWithValve.h"
00026 #include "BuoyancyControlUnit/BuoyancyControlUnit.h"
00027 #endif
00028 
00029 
00030 // Control
00031 #define fishControllerTickerInterval 1000 // how often to call the control ticker, in microseconds
00032 
00033 // Constants
00034 #define PI2 6.2831853  // PI is not included with math.h for some reason
00035 // Values to use for resetting the fish to neutral
00036 #define resetSelectButtonValue 0
00037 #define resetPitchValue        0.5
00038 #define resetYawValue          0
00039 #define resetThrustValue       0
00040 #define resetFrequencyValue    0.0000012 // cycles/us
00041 #define resetPeriodHalfValue   416666    // 1/(2*frequency) -> us
00042 
00043 // Value ranges
00044 #ifdef FISH4
00045 #define fishMinPitch     ((float)(0.2)) // will want to redefine for fish 6 based on depth instead
00046 #define fishMaxPitch     ((float)(0.8))
00047 #endif
00048 
00049 #ifdef FISH6
00050 #define fishMinPitch     ((float)(0.2)) // will want to redefine for fish 6 based on depth instead
00051 #define fishMaxPitch     ((float)(0.8))
00052 //#define fishMinPitch     ((float)(0.0)) // seems appropriate for BCUs
00053 //#define fishMaxPitch     ((float)(30.0))
00054 #endif
00055 
00056 #define fishMinYaw       ((float)(-1.0))
00057 #define fishMaxYaw       ((float)(1.0))
00058 
00059 #define fishMinThrust    ((float)(0.0))
00060 #ifdef FISH4
00061 #define fishMaxThrust    ((float)(0.75))
00062 #endif
00063 #ifdef FISH6
00064 #define fishMaxThrust    ((float)(1.0))
00065 #endif
00066 
00067 #define fishMinFrequency ((float)(0.0000009))
00068 #define fishMaxFrequency ((float)(0.0000016))
00069 
00070 // Preset states for auto mode definition
00071 // Each one is pitch, yaw, thrust, frequency
00072 #define FISH_STRAIGHT   {resetPitchValue, resetYawValue , (fishMaxThrust + fishMinThrust)/2.0   , (fishMaxFrequency + fishMinFrequency)/2.0}
00073 #define FISH_UP         {fishMaxPitch   , resetYawValue , (fishMaxThrust + fishMinThrust)/2.0   , (fishMaxFrequency + fishMinFrequency)/2.0}
00074 #define FISH_DOWN       {fishMinPitch   , resetYawValue , (fishMaxThrust + fishMinThrust)/2.0   , (fishMaxFrequency + fishMinFrequency)/2.0}
00075 #define FISH_LEFT       {resetPitchValue, fishMaxYaw    , (fishMaxThrust + fishMinThrust)/2.0   , (fishMaxFrequency + fishMinFrequency)/2.0}
00076 #define FISH_RIGHT      {resetPitchValue, fishMinYaw    , (fishMaxThrust + fishMinThrust)/2.0   , (fishMaxFrequency + fishMinFrequency)/2.0}
00077 #define FISH_STOP       {resetPitchValue, resetYawValue , resetThrustValue                      , resetFrequencyValue}
00078 
00079 // Pins
00080 #define lowBatteryVoltagePin p16
00081 
00082 #ifdef FISH4
00083 #define motorPWMPin   p23
00084 #define motorOutAPin  p11
00085 #define motorOutBPin  p12
00086 #define servoLeftPin  p21
00087 #define servoRightPin p26 //p24
00088 #endif
00089 
00090 #ifdef FISH6
00091 // NOTE: FISH6 pins are defined in BCU and Valve classes
00092 #define pressureSensorPinSDA p28
00093 #define pressureSensorPinSCL p27
00094 #define imuSensorPinSDA p28
00095 #define imuSensorPinSCL p27
00096 #define servoLeftPin  p21
00097 #define servoRightPin p26
00098 #endif
00099 
00100 
00101 #define buttonBoardSDAPin  p9
00102 #define buttonBoardSCLPin  p10
00103 #define buttonBoardInt1Pin p29
00104 #define buttonBoardInt2Pin p30
00105 
00106 /* Button board commands
00107   Commented indexes go from top left (0) to bottom right (5) as follows:
00108                  /=========================|
00109                /   ______________________  |
00110              /    |  (0:8) (1:16) (2:32) | |
00111   fish nose |     |(3:1) (4:2) (5:4)     | |  fish tail
00112              \     ----------------------| |
00113                \                           |
00114                  \=========================|
00115   The numbers after the colons are the values to use for that button
00116 */
00117 #define BTTN_FASTER      1  // 3
00118 #define BTTN_SLOWER      2  // 4
00119 #define BTTN_YAW_LEFT    4  // 5
00120 #define BTTN_YAW_RIGHT   8  // 0
00121 #define BTTN_PITCH_UP    16 // 1 // swims down
00122 #define BTTN_PITCH_DOWN  32 // 2 // swims up
00123 #define BTTN_RESET_MBED  36 // 2 and 5
00124 #define BTTN_SHUTDOWN_PI 9  // 0 and 3
00125 #define BTTN_AUTO_MODE   33 // 2 and 3
00126 #define BTTN_BTTN_MODE   12 // 0 and 5
00127 
00128 
00129 class FishController
00130 {
00131     public:
00132         // Initialization
00133         FishController();
00134         void start();
00135         void stop();
00136         // Processing
00137         void tickerCallback();
00138         // Debug / Logging
00139         volatile uint8_t streamFishStateEventController; // will indicate the last button board event - up to the caller to reset it if desired
00140         #ifdef debugFishState
00141         void printDebugState();
00142         #endif
00143         // LEDs
00144         void setLEDs(char mask, bool turnOn);
00145         // Set New State (which will take affect at next appropriate point in control cycle)
00146         void setSelectButton(bool newSelectButtonValue, bool master= false);
00147         void setPitch(float newPitchValue, bool master = false);
00148         void setYaw(float newYawValue, bool master = false);
00149         void setThrust(float newThrustValue, bool master = false);
00150         void setFrequency(float newFrequencyValue, float newPeriodHalfValue = -1, bool master = false);
00151         // Get (possible pending) State
00152         bool getSelectButton();
00153         float getPitch();
00154         float getYaw();
00155         float getThrust();
00156         float getFrequency();
00157         float getPeriodHalf();
00158         // Auto mode
00159         volatile bool autoMode;
00160         void startAutoMode();
00161         void stopAutoMode();
00162         void autoModeCallback();
00163 
00164         void setIgnoreExternalCommands(bool ignore);
00165         bool getIgnoreExternalCommands();
00166         
00167         // BCU Helper Functions
00168         float getBCUVset();
00169         float getBCUSetDepth();
00170         float getBCUCurDepth();
00171         float getBCUSetPos();
00172         float getBCUCurPos();
00173         float getreadPressure(); 
00174         
00175     private:
00176         // Misc State
00177         volatile bool ignoreExternalCommands;
00178         // Ticker for controlling tail
00179         Ticker ticker;
00180         const uint16_t tickerInterval;
00181         volatile bool inTickerCallback;
00182 
00183         // State which will be applied at the next appropriate time in the control cycle
00184         volatile bool newSelectButton;
00185         volatile float newPitch;
00186         volatile float newYaw;
00187         volatile float newThrust;
00188         volatile float newFrequency;
00189         volatile float newPeriodHalf;
00190 
00191         // State currently executing on fish
00192         volatile bool selectButton;
00193         volatile float pitch;
00194         volatile float yaw;
00195         volatile float thrust;
00196         volatile float frequency;
00197         
00198         // Servos (Fish 6)
00199         Servo servoLeft;
00200         Servo servoRight;
00201         
00202 
00203 #ifdef FISH4
00204         volatile float thrustCommand;
00205         volatile float periodHalf;
00206         volatile float dutyCycle;
00207         volatile bool brushlessOff;
00208         volatile uint32_t curTime;
00209         volatile bool fullCycle;
00210         const float raiser;
00211         // Outputs for motor and servos
00212         //PwmOut motorPWM;
00213         //DigitalOut motorOutA;
00214         //DigitalOut motorOutB;
00215         Servo servoLeft;
00216         Servo servoRight;
00217         //PwmOut brushlessMotor;
00218         const uint32_t brushlessOffTime;
00219 #endif
00220 
00221         // Button control
00222         ButtonBoard buttonBoard;
00223         static void buttonCallback(char button, bool pressed, char state);
00224 
00225         // Auto mode
00226         Ticker autoModeTicker;
00227         uint32_t autoModeCount;
00228         uint16_t autoModeIndex;
00229         bool ignoreExternalCommandsPreAutoMode;
00230 };
00231 
00232 // Create a static instance of FishController to be used by anyone doing detection
00233 extern FishController fishController;
00234 extern volatile uint8_t streamFishStateEvent;
00235 extern volatile uint16_t streamCurFishState;
00236 
00237 #endif // ifndef FISH_CONTROLLER_H