update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
main.cpp
- Committer:
- tzyoung
- Date:
- 2017-10-06
- Revision:
- 3:7824127c5cfd
- Parent:
- 2:892b58e56712
- Child:
- 4:66f13fbb035d
File content as of revision 3:7824127c5cfd:
#include "mbed.h" #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 #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 //Read in and load the BCE parameters from the text file "bce.txt" load_BCE_config(); ////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. //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(); //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 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 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 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 ; } 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; } 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("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); } }