update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
Diff: main.cpp
- Revision:
- 2:892b58e56712
- Parent:
- 1:133216180125
- Child:
- 3:7824127c5cfd
--- a/main.cpp Thu Apr 27 16:47:29 2017 +0000 +++ b/main.cpp Fri Oct 06 13:25:26 2017 +0000 @@ -1,46 +1,102 @@ #include "mbed.h" #include "StaticDefs.hpp" -#include "ltc1298.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 + +#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 + +#include <fstream> +#include <iostream> int main() { - //start up the system timer so the position velocity estimator has a time signal + //initialize linear actuator objects (doesn't work) + load_BCE_config(); + + //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 + + bce().setControllerP(BCE_P); + bce().setControllerI(BCE_I); + bce().setControllerD(BCE_D); + bce().setZeroCounts(BCE_ZERO); + bce().setTravelLimit(BCE_LIMIT); + bce().setPotSlope(POT_SLOPE); + + 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(); //setup and start the adc. This runs on a fixed interval and is interrupt driven adc().initialize(); adc().start(); - //Initialize the position velocity filter. This will consume a couple of seconds for - //the filter to converge - pvf().init(); - + //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 + bce().start(); + batt().start(); + + //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 - float motor_cmd = 0.0; - float positionCmd = 250.0; - float P = 0.10; - float I = 0.00; - float D = 0.00; - char userInput; - - hBridge().run(motor_cmd); - - //set the intial gains for the position controller - posCon().setPgain(P); - posCon().setIgain(I); - posCon().setDgain(D); - posCon().writeSetPoint(positionCmd); while(1) { - - // update the position velocity filter - pvf().update(); + + //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 + +*/ - //update the controller with the current numbers in the position guesser - posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ; - hBridge().run(posCon().getOutput()); - +/* + 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 userInput = pc().getc(); @@ -78,9 +134,9 @@ //decrement the I gain I -= 0.001; } - if (userInput == ' '){ + if (userInput == ' ') { //reset the h-bridge - hBridge().reset(); + //hBridge().reset(); } //if (userInput == ' ') { @@ -98,15 +154,16 @@ // assign the shiny new commands to the pins //hBridge().run(motor_cmd); if (userInput == '\r') { - posCon().writeSetPoint(positionCmd); - posCon().setPgain(P); - posCon().setIgain(I); - posCon().setDgain(D); - } + bce().setPosition_mm(positionCmd); + // posCon().setPgain(P); + // posCon().setIgain(I); + // posCon().setDgain(D); + }*/ - } + // } - 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("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); }