most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
main.cpp
- Committer:
- mkelly10
- Date:
- 2017-10-20
- Revision:
- 5:15bd96205bb2
- Parent:
- 4:66f13fbb035d
- Child:
- 10:085ab7328054
File content as of revision 5:15bd96205bb2:
#include "mbed.h" #include "StaticDefs.hpp" #include "config_functions.h" int main() { float positionCmd = 10.0; char userInput; local(); //this makes sure the local file system is constructed //Read in and load the BCE parameters from the text file "bce.txt" load_BCE_config(); // should say "bce.load_config(bce.txt); bce().init(); ////Read in and load the battery mover parameters from the text file "batt.txt" load_BATT_config(); batt().init(); // do not leave this in. It is just there to check that things are working pc().printf("\n\rbce P: %3.2f I: %3.2f D %3.2f zero: %3i limit %3.2f slope %3.2f \n\r", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope()); pc().printf("\n\rbatt P: %3.2f I: %3.2f D %3.2f zero: %3i limit %3.2f slope %3.2f \n\r", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope()); //Front load the desired parameters into the linear acuator objects. //This could be done using ConfigFile, if it worked //I need to also check up in whether the limits are being passed to the linear //actuator's PID objects. I noticed I have defaults that are appropriate for only //the bouyancy engine //start up the system timer systemTime().start(); //setup and start the adc. This runs on a fixed interval and is interrupt driven adc().initialize(); adc().start(); //start the bce and batt control loops. ADC has to be running first. //The motors will not activate until their respective position filters have //converged, but just to be sure we pause them until we want them to really run bce().start(); bce().pause(); bce().setPosition_mm(positionCmd); bce().unpause(); batt().start(); batt().pause(); batt().setPosition_mm(positionCmd); batt().unpause(); while(1) { if (pc().readable()) { // get the key userInput = pc().getc(); //check command against desired control buttons if (userInput == '=') { //increment the duty cycle positionCmd += 5.0 ; } else if (userInput == '-') { //decrement the duty cycle positionCmd -= 5.0 ; } else if (userInput == '\r') { batt().setPosition_mm(positionCmd); bce().setPosition_mm(positionCmd); } } //pc().printf("set point %3.1f \r", positionCmd); //pc().printf("pos: %3.2f mm pos: %3.2f counts vel: % 3.2f mm/s Set Point %3.1f \r", bce().getPosition_mm(), bce().getPosition_counts(), bce().getVelocity_mms(), positionCmd); //pc().printf("pos: %3.2f mm pos: %3.2f counts vel: % 3.2f mm/s Set Point %3.1f \r", batt().getPosition_mm(), batt().getPosition_counts(), batt().getVelocity_mms(), positionCmd); pc().printf("pos bce: %3.2f mm, pos batt: %3.2f mm, Set Point %3.1f \r", bce().getPosition_mm(), batt().getPosition_mm(), positionCmd); } } //read in a script file describing the dive cycle //I envision entries such as // target pitch angle target depth target depth rate // 10 degrees 5 ft 0.05 ft/s example dive request // -10 degrees 0 ft -0.05 ft/s example surface request //this implies two pid controllers // one that manages the batt mover for pitch // the other manages the buoyance engine for dive speed // then some logic is needed to check the box when the desired condition is reached // like a waypoint threshold. This allows you to get away from worrying as much about // keeping time //psuedo code to outline what we want to do /* check for and parse IMU data poll for depth adc reading (eventually this will come from the external adc) run depth data through position velocity filter update the PID controllers for Pitch and depth rate check whether depth has been triggered if so, move on to the next line of the script if done , surface or repeat */ //This can be ignored for now this was the old serial comms stuff for when I //was prototyping the BCE controls