update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
Diff: main.cpp
- Revision:
- 4:66f13fbb035d
- Parent:
- 3:7824127c5cfd
- Child:
- 5:15bd96205bb2
--- a/main.cpp Fri Oct 06 21:16:55 2017 +0000 +++ b/main.cpp Tue Oct 10 20:22:57 2017 +0000 @@ -2,46 +2,28 @@ #include "StaticDefs.hpp" #include "config_functions.h" -#define BCE_P 0.1 -#define BCE_I 0.0 -#define BCE_D 0.0 - -#define BCE_ZERO 400 //counts -#define BCE_LIMIT 498.729 //mm -#define POT_SLOPE .121760 // mm/counts +int main() +{ + float positionCmd = 10.0; + char userInput; -#define BATT_P 0.1 -#define BATT_I 0.0 -#define BATT_D 0.0 - -#define BATT_ZERO 100 //counts -#define BATT_LIMIT 50.0 //mm - -int main() -{ local(); //this makes sure the local file system is constructed - + //pc().baud(115200); //Read in and load the BCE parameters from the text file "bce.txt" load_BCE_config(); - + bce().init(); + ////Read in and load the battery mover parameters from the text file "batt.txt" - + // do not leave this in. It is just there to check that things are working - pc().printf("\n\rP: %3.2f I: %3.2f D %3.2f zero: %3i limit %3.2f slope %3.2f", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope()); - - //Front load the desired parameters into the linear acuator objects. + pc().printf("\n\rP: %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()); + + //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 - - batt().setControllerP(BATT_P); - batt().setControllerI(BATT_I); - batt().setControllerD(BATT_D); - batt().setZeroCounts(BATT_ZERO); - batt().setTravelLimit(BATT_LIMIT); - batt().setPotSlope(POT_SLOPE); //start up the system timer systemTime().start(); @@ -52,47 +34,53 @@ //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 + //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(); + //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 - + // 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 + // keeping time while(1) { - //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 + + //This can be ignored for now this was the old serial comms stuff for when I + //was prototyping the BCE controls if (pc().readable()) { // get the key @@ -105,63 +93,16 @@ } else if (userInput == '-') { //decrement the duty cycle positionCmd -= 5.0 ; - } - - if (userInput == 'w') { - //increment the P gain - P += 0.01; - } - if (userInput == 's') { - //decrement the P gain - P -= 0.01; - } - if (userInput == 'i') { - //increment the D gain - D += 0.001; - } - if (userInput == 'k') { - //decrement the D gain - D -= 0.001; - } - if (userInput == 't') { - //increment the I gain - I += 0.001; - } - if (userInput == 'g') { - //decrement the I gain - I -= 0.001; + } else if (userInput == '\r') { + bce().setPosition_mm(positionCmd); } - if (userInput == ' ') { - //reset the h-bridge - //hBridge().reset(); - } - - //if (userInput == ' ') { - //stop the ride - // motor_cmd = 0.0 ; - //} + } - //clip pwm if over range - //if (motor_cmd > 1.0) { - // motor_cmd = 1.0; - //} - //if (motor_cmd < -1.0) { - // motor_cmd = -1.0; - //} - // assign the shiny new commands to the pins - //hBridge().run(motor_cmd); - if (userInput == '\r') { - bce().setPosition_mm(positionCmd); - // posCon().setPgain(P); - // posCon().setIgain(I); - // posCon().setDgain(D); - }*/ + //pc().printf("set point %3.1f \r", positionCmd); - // } //pc().printf("Position: %10.1f mm Velocity: % 2.2f mm/s Output: % 1.3f switch: %d \r", bce().getPosition_mm(), bce().getVelocity_mms(), bce().getOutput(), bce().getSwitch()); - //pc().printf("pos: %3.0f mm vel: % 2.2f mm/s Set Point %3.0f controller output: % 1.3f P: %1.3f I: %1.4f D: %1.4f\r", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput(), P, I, D); - + pc().printf("pos: %3.2f mm vel: % 3.2f mm/s Set Point %3.1f \r", bce().getPosition_mm(), bce().getVelocity_mms(), positionCmd); } }