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@4:66f13fbb035d, 2017-10-10 (annotated)
- Committer:
- tzyoung
- Date:
- Tue Oct 10 20:22:57 2017 +0000
- Revision:
- 4:66f13fbb035d
- Parent:
- 3:7824127c5cfd
- Child:
- 5:15bd96205bb2
First full working build of the "black box" hardware driver paradigm. Successfully tested the homing function of the linear actuator on the BCE. Also tested the config reading and control loop of the BCE.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tzyoung | 0:ea293bbf9717 | 1 | #include "mbed.h" |
tzyoung | 0:ea293bbf9717 | 2 | #include "StaticDefs.hpp" |
tzyoung | 2:892b58e56712 | 3 | #include "config_functions.h" |
tzyoung | 2:892b58e56712 | 4 | |
tzyoung | 4:66f13fbb035d | 5 | int main() |
tzyoung | 4:66f13fbb035d | 6 | { |
tzyoung | 4:66f13fbb035d | 7 | float positionCmd = 10.0; |
tzyoung | 4:66f13fbb035d | 8 | char userInput; |
tzyoung | 2:892b58e56712 | 9 | |
tzyoung | 3:7824127c5cfd | 10 | local(); //this makes sure the local file system is constructed |
tzyoung | 4:66f13fbb035d | 11 | //pc().baud(115200); |
tzyoung | 3:7824127c5cfd | 12 | //Read in and load the BCE parameters from the text file "bce.txt" |
tzyoung | 2:892b58e56712 | 13 | load_BCE_config(); |
tzyoung | 4:66f13fbb035d | 14 | bce().init(); |
tzyoung | 4:66f13fbb035d | 15 | |
tzyoung | 3:7824127c5cfd | 16 | ////Read in and load the battery mover parameters from the text file "batt.txt" |
tzyoung | 4:66f13fbb035d | 17 | |
tzyoung | 3:7824127c5cfd | 18 | // do not leave this in. It is just there to check that things are working |
tzyoung | 4:66f13fbb035d | 19 | 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()); |
tzyoung | 4:66f13fbb035d | 20 | |
tzyoung | 4:66f13fbb035d | 21 | //Front load the desired parameters into the linear acuator objects. |
tzyoung | 2:892b58e56712 | 22 | //This could be done using ConfigFile, if it worked |
tzyoung | 4:66f13fbb035d | 23 | |
tzyoung | 2:892b58e56712 | 24 | //I need to also check up in whether the limits are being passed to the linear |
tzyoung | 2:892b58e56712 | 25 | //actuator's PID objects. I noticed I have defaults that are appropriate for only |
tzyoung | 2:892b58e56712 | 26 | //the bouyancy engine |
tzyoung | 2:892b58e56712 | 27 | |
tzyoung | 2:892b58e56712 | 28 | //start up the system timer |
tzyoung | 0:ea293bbf9717 | 29 | systemTime().start(); |
tzyoung | 0:ea293bbf9717 | 30 | |
tzyoung | 0:ea293bbf9717 | 31 | //setup and start the adc. This runs on a fixed interval and is interrupt driven |
tzyoung | 0:ea293bbf9717 | 32 | adc().initialize(); |
tzyoung | 0:ea293bbf9717 | 33 | adc().start(); |
tzyoung | 0:ea293bbf9717 | 34 | |
tzyoung | 2:892b58e56712 | 35 | //start the bce and batt control loops. ADC has to be running first. |
tzyoung | 2:892b58e56712 | 36 | //The motors will not activate until their respective position filters have |
tzyoung | 4:66f13fbb035d | 37 | //converged, but just to be sure we pause them until we want them to really run |
tzyoung | 2:892b58e56712 | 38 | bce().start(); |
tzyoung | 4:66f13fbb035d | 39 | bce().pause(); |
tzyoung | 4:66f13fbb035d | 40 | |
tzyoung | 4:66f13fbb035d | 41 | bce().setPosition_mm(positionCmd); |
tzyoung | 4:66f13fbb035d | 42 | |
tzyoung | 4:66f13fbb035d | 43 | bce().unpause(); |
tzyoung | 4:66f13fbb035d | 44 | |
tzyoung | 2:892b58e56712 | 45 | batt().start(); |
tzyoung | 4:66f13fbb035d | 46 | batt().pause(); |
tzyoung | 4:66f13fbb035d | 47 | |
tzyoung | 2:892b58e56712 | 48 | //read in a script file describing the dive cycle |
tzyoung | 2:892b58e56712 | 49 | //I envision entries such as |
tzyoung | 2:892b58e56712 | 50 | // target pitch angle target depth target depth rate |
tzyoung | 2:892b58e56712 | 51 | // 10 degrees 5 ft 0.05 ft/s example dive request |
tzyoung | 2:892b58e56712 | 52 | // -10 degrees 0 ft -0.05 ft/s example surface request |
tzyoung | 4:66f13fbb035d | 53 | |
tzyoung | 2:892b58e56712 | 54 | //this implies two pid controllers |
tzyoung | 2:892b58e56712 | 55 | // one that manages the batt mover for pitch |
tzyoung | 4:66f13fbb035d | 56 | // the other manages the buoyance engine for dive speed |
tzyoung | 4:66f13fbb035d | 57 | |
tzyoung | 2:892b58e56712 | 58 | // then some logic is needed to check the box when the desired condition is reached |
tzyoung | 2:892b58e56712 | 59 | // like a waypoint threshold. This allows you to get away from worrying as much about |
tzyoung | 4:66f13fbb035d | 60 | // keeping time |
tzyoung | 0:ea293bbf9717 | 61 | |
tzyoung | 0:ea293bbf9717 | 62 | |
tzyoung | 0:ea293bbf9717 | 63 | |
tzyoung | 0:ea293bbf9717 | 64 | while(1) { |
tzyoung | 2:892b58e56712 | 65 | //psuedo code to outline what we want to do |
tzyoung | 2:892b58e56712 | 66 | /* |
tzyoung | 2:892b58e56712 | 67 | check for and parse IMU data |
tzyoung | 4:66f13fbb035d | 68 | |
tzyoung | 2:892b58e56712 | 69 | poll for depth adc reading (eventually this will come from the external adc) |
tzyoung | 4:66f13fbb035d | 70 | |
tzyoung | 2:892b58e56712 | 71 | run depth data through position velocity filter |
tzyoung | 4:66f13fbb035d | 72 | |
tzyoung | 2:892b58e56712 | 73 | update the PID controllers for Pitch and depth rate |
tzyoung | 4:66f13fbb035d | 74 | |
tzyoung | 2:892b58e56712 | 75 | check whether depth has been triggered |
tzyoung | 2:892b58e56712 | 76 | if so, move on to the next line of the script |
tzyoung | 2:892b58e56712 | 77 | if done , surface or repeat |
tzyoung | 4:66f13fbb035d | 78 | |
tzyoung | 4:66f13fbb035d | 79 | */ |
tzyoung | 0:ea293bbf9717 | 80 | |
tzyoung | 4:66f13fbb035d | 81 | |
tzyoung | 4:66f13fbb035d | 82 | //This can be ignored for now this was the old serial comms stuff for when I |
tzyoung | 4:66f13fbb035d | 83 | //was prototyping the BCE controls |
tzyoung | 2:892b58e56712 | 84 | |
tzyoung | 0:ea293bbf9717 | 85 | if (pc().readable()) { |
tzyoung | 0:ea293bbf9717 | 86 | // get the key |
tzyoung | 0:ea293bbf9717 | 87 | userInput = pc().getc(); |
tzyoung | 0:ea293bbf9717 | 88 | |
tzyoung | 0:ea293bbf9717 | 89 | //check command against desired control buttons |
tzyoung | 0:ea293bbf9717 | 90 | if (userInput == '=') { |
tzyoung | 0:ea293bbf9717 | 91 | //increment the duty cycle |
tzyoung | 0:ea293bbf9717 | 92 | positionCmd += 5.0 ; |
tzyoung | 0:ea293bbf9717 | 93 | } else if (userInput == '-') { |
tzyoung | 0:ea293bbf9717 | 94 | //decrement the duty cycle |
tzyoung | 0:ea293bbf9717 | 95 | positionCmd -= 5.0 ; |
tzyoung | 4:66f13fbb035d | 96 | } else if (userInput == '\r') { |
tzyoung | 4:66f13fbb035d | 97 | bce().setPosition_mm(positionCmd); |
tzyoung | 0:ea293bbf9717 | 98 | } |
tzyoung | 4:66f13fbb035d | 99 | } |
tzyoung | 0:ea293bbf9717 | 100 | |
tzyoung | 4:66f13fbb035d | 101 | //pc().printf("set point %3.1f \r", positionCmd); |
tzyoung | 0:ea293bbf9717 | 102 | |
tzyoung | 0:ea293bbf9717 | 103 | |
tzyoung | 2:892b58e56712 | 104 | //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()); |
tzyoung | 4:66f13fbb035d | 105 | pc().printf("pos: %3.2f mm vel: % 3.2f mm/s Set Point %3.1f \r", bce().getPosition_mm(), bce().getVelocity_mms(), positionCmd); |
tzyoung | 0:ea293bbf9717 | 106 | |
tzyoung | 0:ea293bbf9717 | 107 | } |
tzyoung | 0:ea293bbf9717 | 108 | } |