FishController.cpp

00001 
00002 #include "FishController.h"
00003 
00004 // The static instance
00005 FishController fishController;
00006 
00007 // Function to reset mbed
00008 extern "C" void mbed_reset();
00009 
00010 // Auto mode
00011 float autoModeCommands[][4] = {FISH_STRAIGHT, FISH_UP, FISH_STRAIGHT, FISH_DOWN};
00012 uint32_t autoModeDurations[] = {4000, 2000, 2000, 2000}; // durations in milliseconds (is it really? -> check if statement that compares autoModeDurations elements to 10*indexcount
00013 const uint8_t autoModeLength = sizeof(autoModeDurations)/sizeof(autoModeDurations[0]);
00014 
00015 //============================================
00016 // Initialization
00017 //============================================
00018 
00019 // Constructor
00020 FishController::FishController():
00021     // Initialize variables
00022     autoMode(false),
00023     ignoreExternalCommands(false),
00024     tickerInterval(fishControllerTickerInterval),
00025     inTickerCallback(false),
00026     servoLeft(servoLeftPin),
00027     servoRight(servoRightPin),
00028     #ifdef FISH4
00029     curTime(0),
00030     fullCycle(true),
00031     raiser(3.5),
00032     // Outputs for motor and servos
00033     //motorPWM(motorPWMPin),
00034     //motorOutA(motorOutAPin),
00035     //motorOutB(motorOutBPin),
00036     servoLeft(servoLeftPin),
00037     servoRight(servoRightPin),
00038     //brushlessMotor(p25),
00039     brushlessOffTime(30000),
00040     #endif
00041 /*  #ifdef FISH6 // these are declared in BCU class
00042     pressureSensor(pressureSensorPinSDA, pressureSensorPinSCL, ms5837_addr_no_CS),
00043     imuSensor(imuSensorPinSDA, imuSensorPinSCL)
00044     #endif*/
00045     // Button board
00046     buttonBoard(buttonBoardSDAPin, buttonBoardSCLPin, buttonBoardInt1Pin, buttonBoardInt2Pin) // sda, scl, int1, int2
00047 
00048 {
00049     streamFishStateEventController = 0;
00050 
00051     newSelectButton = resetSelectButtonValue;
00052     newPitch = resetPitchValue;
00053     newYaw = resetYawValue;
00054     newThrust = resetThrustValue;
00055     newFrequency = resetFrequencyValue;
00056     newPeriodHalf = resetPeriodHalfValue;
00057 
00058     selectButton = newSelectButton;
00059     pitch = newPitch;
00060     yaw = newYaw;
00061     thrust = newThrust;
00062     frequency = newFrequency;
00063 #ifdef FISH4
00064     periodHalf = newPeriodHalf;
00065     thrustCommand = 0;
00066     dutyCycle = 0;
00067     brushlessOff = false;
00068 #endif
00069 
00070     buttonBoard.registerCallback(&FishController::buttonCallback);
00071     buttonBoard.setLEDs(255, false);
00072 
00073     autoModeIndex = 0;
00074     autoModeCount = 0;
00075 
00076 }
00077 
00078 // Set the desired state
00079 // They will take affect at the next appropriate time in the control cycle
00080 void FishController::setSelectButton(bool newSelectButtonValue, bool master /* = false*/)
00081 {
00082     if(!ignoreExternalCommands || master)
00083         newSelectButton = newSelectButtonValue;
00084 }
00085 void FishController::setPitch(float newPitchValue, bool master /* = false*/)
00086 {
00087     if(!ignoreExternalCommands || master)
00088     {
00089         newPitch = newPitchValue;
00090         setLEDs(BTTN_PITCH_UP,   (newPitch-fishMinPitch) > (fishMaxPitch - newPitch));
00091         setLEDs(BTTN_PITCH_DOWN, (newPitch-fishMinPitch) < (fishMaxPitch - newPitch));
00092     }
00093 }
00094 void FishController::setYaw(float newYawValue, bool master /* = false*/)
00095 {
00096     if(!ignoreExternalCommands || master)
00097     {
00098         newYaw = newYawValue;
00099         setLEDs(BTTN_YAW_LEFT,  (newYaw-fishMinYaw) < (fishMaxYaw - newYaw));
00100         setLEDs(BTTN_YAW_RIGHT, (newYaw-fishMinYaw) > (fishMaxYaw - newYaw));
00101     }
00102 }
00103 void FishController::setThrust(float newThrustValue, bool master /* = false*/)
00104 {
00105     if(!ignoreExternalCommands || master)
00106     {
00107         newThrust = newThrustValue;
00108         setLEDs(BTTN_FASTER, newThrust>fishMinThrust);
00109         // If we're in button-control mode, keep the no-thrust light on as an indicator
00110         if(!ignoreExternalCommands)
00111             setLEDs(BTTN_SLOWER, newThrust==fishMinThrust);
00112         else
00113             setLEDs(BTTN_SLOWER, true);
00114     }
00115 }
00116 void FishController::setFrequency(float newFrequencyValue, float newPeriodHalfValue /* = -1 */, bool master /* = false*/)
00117 {
00118     if(!ignoreExternalCommands || master)
00119     {
00120         newFrequency = newFrequencyValue;
00121         newPeriodHalf = newPeriodHalfValue > -1 ? newPeriodHalfValue : (1.0/(2.0*newFrequency));
00122     }
00123 }
00124 // Get the (possible pending) state
00125 bool FishController::getSelectButton() {return newSelectButton;}
00126 float FishController::getPitch() {return newPitch;}
00127 float FishController::getYaw() {return newYaw;}
00128 float FishController::getThrust() {return newThrust;}
00129 float FishController::getFrequency() {return newFrequency;}
00130 float FishController::getPeriodHalf() {return newPeriodHalf;}
00131 
00132 void FishController::start()
00133 {
00134 
00135     // Blink button board LEDs to indicate startup
00136     for(uint8_t i = 0; i < 3; i++)
00137     {
00138         buttonBoard.setLEDs(255, true);
00139         wait_ms(500);
00140         buttonBoard.setLEDs(255, false);
00141         wait_ms(500);
00142     }
00143     
00144 #ifdef FISH6
00145     buoyancyControlUnit.start();
00146     pumpWithValve.start();
00147 #endif
00148 
00149     // Start control ticker callback
00150     ticker.attach_us(&fishController, &FishController::tickerCallback, tickerInterval);
00151 #ifdef debugFishState
00152     printf("Starting...\n");
00153 #endif
00154 
00155 
00156 }
00157 
00158 void FishController::stop()
00159 {
00160     // Stop updating the fish
00161     while(inTickerCallback); // wait for commands to settle
00162     ticker.detach(); // stop updating commands
00163     wait_ms(5);      // wait a bit to make sure it stops
00164 
00165     // Reset fish state to neutral
00166     newSelectButton = resetSelectButtonValue;
00167     newPitch = resetPitchValue;
00168     newYaw = resetYawValue;
00169     newThrust = resetThrustValue;
00170     newFrequency = resetFrequencyValue;
00171     newPeriodHalf = resetPeriodHalfValue;
00172     // Send commands to fish (multiple times to make sure we get in the right part of the cycle to actually update it)
00173     for(int i = 0; i < 200; i++)
00174     {
00175         tickerCallback();
00176         wait_ms(10);
00177     }
00178     // Make sure commands are sent to motors and applied
00179     wait(1);
00180 
00181     #ifdef FISH4
00182     // Put dive planes in a weird position to indicate stopped
00183     servoLeft = 0.3;
00184     servoRight = 0.3;
00185     #endif
00186 
00187     #ifdef FISH6
00188     pumpWithValve.stop();
00189     buoyancyControlUnit.stop();
00190     #endif // FISH6
00191 
00192 
00193     // Light the LEDs to indicate termination
00194     buttonBoard.setLEDs(255, true);
00195 }
00196 
00197 //============================================
00198 // Processing
00199 //============================================
00200 #ifdef FISH4
00201 void FishController::tickerCallback()
00202 {
00203     inTickerCallback = true; // so we don't asynchronously stop the controller in a bad point of the cycle
00204 
00205     // get the current elapsed time since last reset (us)
00206     curTime += tickerInterval;
00207 
00208     // see if brushless should be shut down
00209     brushlessOff = curTime > (periodHalf-brushlessOffTime);
00210 
00211     // update every half cycle
00212     if(curTime > periodHalf)
00213     {
00214         // read new yaw value every half cycle
00215         yaw = newYaw; // a value from -1 to 1
00216 
00217         // Read frequency only every full cycle
00218         if(fullCycle)
00219         {
00220             // Read other new inputs
00221             thrust = newThrust; // a value from 0 to 1
00222             frequency = newFrequency;
00223             periodHalf = newPeriodHalf;
00224             // Adjust thrust if needed
00225             if(yaw < 0.0)
00226                 thrustCommand = (1.0 + 0.75*yaw)*thrust; // 0.7 can be adjusted to a power of 2 if needed
00227             else
00228                 thrustCommand = thrust;
00229             fullCycle = false;
00230         }
00231         else
00232         {
00233             // Reverse for the downward slope
00234             if(yaw > 0.0)
00235                 thrustCommand = -(1.0 - 0.75*yaw)*thrust;
00236             else
00237                 thrustCommand = -thrust;
00238             fullCycle = true;
00239         }
00240 
00241         // Reset time
00242         curTime = 0;
00243     }
00244 
00245     // Update the servos
00246     
00247     pitch = newPitch;
00248     servoLeft = pitch - 0.05; // The 0.03 calibrates the angles of the servo
00249     servoRight = (1.0 - pitch) < 0.03 ? 0.03 : (1.0 - pitch);
00250 
00251     //  Testing whether fishController is running
00252     //  DigitalOut test(LED1);
00253     //  test = 1;
00254 
00255     // Update the duty cycle
00256     /*dutyCycle = raiser * sin(PI2 * frequency * curTime); // add factor 4.0 to get a cut off sinus
00257     if(dutyCycle > 1)
00258         dutyCycle = 1;
00259     if(dutyCycle < -1)
00260         dutyCycle = -1;
00261     dutyCycle *= thrustCommand;
00262     if(dutyCycle >= 0 && dutyCycle < 0.01)
00263         dutyCycle = 0;
00264     if(dutyCycle < 0 && dutyCycle > -0.01)
00265         dutyCycle = 0;
00266     // Update the brushed motor
00267     if(dutyCycle >= 0)
00268     {
00269         motorOutA.write(0);
00270         motorOutB.write(1);
00271         motorPWM.write(dutyCycle);
00272     }
00273     else
00274     {
00275         motorOutA.write(1);
00276         motorOutB.write(0);
00277         motorPWM.write(-1 * dutyCycle);
00278     }*/
00279     // Update the brushless motor
00280     //brushlessMotor = dutyCycle * !brushlessOff;
00281     //brushlessMotor.pulsewidth_us(dutyCycle*500+1500);
00282     //brushlessMotor();
00283 
00284 
00285 #ifdef debugFishState
00286     printDebugState();
00287 #endif
00288     //printf("%f\n", dutyCycle);
00289     //printf("%f %f\r\n", pitch, servoLeft.read());
00290     inTickerCallback = false;
00291 }
00292 #endif
00293 
00294 #ifdef FISH6
00295 void FishController::tickerCallback()
00296 {
00297     inTickerCallback = true; // so we don't asynchronously stop the controller in a bad point of the cycle
00298 
00299     //set current state to newly commanded value
00300     frequency = newFrequency;
00301     yaw = newYaw;
00302     thrust = newThrust;
00303     pitch = newPitch;
00304     
00305     // Update dive planes
00306     servoLeft = pitch - 0.05; // The 0.03 calibrates the angles of the servo
00307     servoRight = (1.0 - pitch) < 0.03 ? 0.03 : (1.0 - pitch);
00308     
00309     pumpWithValve.set(frequency, yaw, thrust);
00310     
00311     /* TURNING OFF BCU FOR FIRST OPEN WORLD TEST - AUGUST 21, 2019*/
00312     //buoyancyControlUnit.set(pitch); //1100 - 1180 seems to follow well
00313     
00314     //Testing whether fishController.tickerCallback() is running
00315     //DigitalOut test(LED1);
00316     //test = 1;
00317     
00318 
00319 #ifdef debugFishState
00320     printDebugState();
00321 #endif
00322 
00323     inTickerCallback = false;
00324 }
00325 #endif
00326 
00327 
00328 // button will be mask indicating which button triggered this interrupt
00329 // pressed will indicate whether that button was pressed or released
00330 // buttonState will be a mask that indicates which buttons are currently pressed
00331 void FishController::buttonCallback(char button, bool pressed, char state) // static
00332 {
00333     //printf("button %d\t pressed: %d\t state: %d\n", button, pressed, state);
00334     //fishController.buttonBoard.setLEDs(button, !fishController.buttonBoard.getLEDs(button));
00335     // Only act on button presses (not releases)
00336     if(!pressed)
00337         return;
00338 
00339     DigitalOut* simBatteryLow;
00340     float newYaw, newThrust, newPitch;
00341     switch(state)
00342     {
00343         case BTTN_YAW_LEFT:
00344             newYaw = fishController.newYaw;
00345             newYaw -= (fishMaxYaw - fishMinYaw)/4.0;
00346             newYaw = newYaw < fishMinYaw ? fishMinYaw : newYaw;
00347             fishController.setYaw(newYaw, true);
00348             fishController.streamFishStateEventController = 6;
00349             break;
00350         case BTTN_YAW_RIGHT:
00351             newYaw = fishController.newYaw;
00352             newYaw += (fishMaxYaw - fishMinYaw)/4.0;
00353             newYaw = newYaw > fishMaxYaw ? fishMaxYaw : newYaw;
00354             fishController.setYaw(newYaw, true);
00355             fishController.streamFishStateEventController = 7;
00356             break;
00357         case BTTN_FASTER:
00358             newThrust = fishController.newThrust;
00359             newThrust += (fishMaxThrust - fishMinThrust)/4.0;
00360             newThrust = newThrust > fishMaxThrust ? fishMaxThrust : newThrust;
00361             fishController.setThrust(newThrust, true);
00362             fishController.streamFishStateEventController = 8;
00363             break;
00364         case BTTN_SLOWER:
00365             newThrust = fishController.newThrust;
00366             newThrust -= (fishMaxThrust - fishMinThrust)/4.0;
00367             newThrust = newThrust < fishMinThrust ? fishMinThrust : newThrust;
00368             fishController.setThrust(newThrust, true);
00369             fishController.streamFishStateEventController = 9;
00370             break;
00371         case BTTN_PITCH_UP:
00372             newPitch = fishController.newPitch;
00373             newPitch += (fishMaxPitch - fishMinPitch)/4.0;
00374             newPitch = newPitch > fishMaxPitch ? fishMaxPitch : newPitch;
00375             fishController.setPitch(newPitch, true);
00376             fishController.streamFishStateEventController = 10;
00377             break;
00378         case BTTN_PITCH_DOWN:
00379             newPitch = fishController.newPitch;
00380             newPitch -= (fishMaxPitch - fishMinPitch)/4.0;
00381             newPitch = newPitch < fishMinPitch ? fishMinPitch : newPitch;
00382             fishController.setPitch(newPitch, true);
00383             fishController.streamFishStateEventController = 11;
00384             break;
00385         case BTTN_SHUTDOWN_PI: // signal a low battery signal to trigger the pi to shutdown
00386             fishController.streamFishStateEventController = 12;
00387             simBatteryLow = new DigitalOut(lowBatteryVoltagePin);
00388             simBatteryLow->write(0);
00389             break;
00390         case BTTN_RESET_MBED:
00391             fishController.streamFishStateEventController = 13; // ... if you see this, it didn't happen :)
00392             mbed_reset();
00393             break;
00394         case BTTN_AUTO_MODE:
00395             fishController.streamFishStateEventController = 14;
00396             if(fishController.autoMode)
00397                 fishController.stopAutoMode();
00398             else
00399                 fishController.startAutoMode();
00400             break;
00401         case BTTN_BTTN_MODE:
00402             fishController.setIgnoreExternalCommands(!fishController.getIgnoreExternalCommands());
00403             break;
00404         default:
00405             fishController.streamFishStateEventController = 15;
00406             break;
00407     }
00408 }
00409 
00410 void FishController::setIgnoreExternalCommands(bool ignore)
00411 {
00412     ignoreExternalCommands = ignore;
00413 }
00414 
00415 bool FishController::getIgnoreExternalCommands()
00416 {
00417     return ignoreExternalCommands;
00418 }
00419 
00420 void FishController::startAutoMode()
00421 {
00422     // Start ignoring external commands so as not to interfere with auto mode
00423     // But remember what the previous setting was so we can restore it after auto mode
00424     ignoreExternalCommandsPreAutoMode = ignoreExternalCommands;
00425     setIgnoreExternalCommands(true);
00426     // Reset state
00427     autoModeCount = 0;
00428     autoModeIndex = 0;
00429     // Start executing the auto loop
00430     autoMode = true;
00431     autoModeTicker.attach_us(&fishController, &FishController::autoModeCallback, 10000);
00432 }
00433 
00434 void FishController::stopAutoMode()
00435 {
00436     autoModeTicker.detach();
00437     // Auto mode was terminated - put fish into a neutral position
00438     setSelectButton(resetSelectButtonValue, true);
00439     setPitch(resetPitchValue, true);
00440     setYaw(resetYawValue, true);
00441     setThrust(resetThrustValue, true);
00442     setFrequency(resetFrequencyValue, resetPeriodHalfValue, true);
00443     // Restore external mode to what is was previously
00444     setIgnoreExternalCommands(ignoreExternalCommandsPreAutoMode);
00445     autoMode = false;
00446 }
00447 
00448 void FishController::autoModeCallback()
00449 {
00450     // Assign the current state (stored as pitch, yaw, thrust, frequency)
00451     setPitch(autoModeCommands[autoModeIndex][0], true);
00452     setYaw(autoModeCommands[autoModeIndex][1], true);
00453     setThrust(autoModeCommands[autoModeIndex][2], true);
00454     setFrequency(autoModeCommands[autoModeIndex][3], 1.0/(2.0*autoModeCommands[autoModeIndex][3]), true);
00455     // See if we advance to the next command
00456     autoModeCount++;
00457     if(autoModeCount*10 > autoModeDurations[autoModeIndex])
00458     {
00459         autoModeCount = 0;
00460         autoModeIndex = (autoModeIndex+1) % autoModeLength; // loop continuously through commands
00461     }
00462 }
00463 
00464 #ifdef debugFishState
00465 void FishController::printDebugState()
00466 {
00467     printf("pitch: %f yaw: %f thrust: %f frequency: %.8f\r\n",
00468             pitch, yaw, thrust, frequency);
00469 }
00470 
00471 #endif
00472 
00473 void FishController::setLEDs(char mask, bool turnOn)
00474 {
00475     buttonBoard.setLEDs(mask, turnOn);
00476 }
00477 
00478 #ifdef debugBCU
00479 /* BCU + Pressure Sensor Helper Functions */
00480 
00481 float FishController::getBCUVset(){ return buoyancyControlUnit.getVset(); }
00482 float FishController::getBCUSetDepth(){ return buoyancyControlUnit.getSetDepth(); }
00483 float FishController::getBCUCurDepth(){ return buoyancyControlUnit.getCurDepth(); }
00484 float FishController::getBCUSetPos(){ return buoyancyControlUnit.getSetPos(); }
00485 float FishController::getBCUCurPos(){ return buoyancyControlUnit.getCurPos(); }
00486 float FishController::getreadPressure(){ return buoyancyControlUnit.readPressure(); }
00487 
00488 #endif