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