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@2:892b58e56712, 2017-10-06 (annotated)
- Committer:
- tzyoung
- Date:
- Fri Oct 06 13:25:26 2017 +0000
- Revision:
- 2:892b58e56712
- Parent:
- 1:133216180125
- Child:
- 3:7824127c5cfd
cleaned up and re-factored code to make some classes more general and extendable
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 | 2:892b58e56712 | 5 | #define BCE_P 0.1 |
tzyoung | 2:892b58e56712 | 6 | #define BCE_I 0.0 |
tzyoung | 2:892b58e56712 | 7 | #define BCE_D 0.0 |
tzyoung | 2:892b58e56712 | 8 | |
tzyoung | 2:892b58e56712 | 9 | #define BCE_ZERO 400 //counts |
tzyoung | 2:892b58e56712 | 10 | #define BCE_LIMIT 498.729 //mm |
tzyoung | 2:892b58e56712 | 11 | #define POT_SLOPE .121760 // mm/counts |
tzyoung | 2:892b58e56712 | 12 | |
tzyoung | 2:892b58e56712 | 13 | #define BATT_P 0.1 |
tzyoung | 2:892b58e56712 | 14 | #define BATT_I 0.0 |
tzyoung | 2:892b58e56712 | 15 | #define BATT_D 0.0 |
tzyoung | 2:892b58e56712 | 16 | |
tzyoung | 2:892b58e56712 | 17 | #define BATT_ZERO 100 //counts |
tzyoung | 2:892b58e56712 | 18 | #define BATT_LIMIT 50.0 //mm |
tzyoung | 2:892b58e56712 | 19 | |
tzyoung | 2:892b58e56712 | 20 | #include <fstream> |
tzyoung | 2:892b58e56712 | 21 | #include <iostream> |
tzyoung | 0:ea293bbf9717 | 22 | |
tzyoung | 0:ea293bbf9717 | 23 | int main() |
tzyoung | 0:ea293bbf9717 | 24 | { |
tzyoung | 2:892b58e56712 | 25 | //initialize linear actuator objects (doesn't work) |
tzyoung | 2:892b58e56712 | 26 | load_BCE_config(); |
tzyoung | 2:892b58e56712 | 27 | |
tzyoung | 2:892b58e56712 | 28 | //Front load the desired parameters into the linear acuator objects. |
tzyoung | 2:892b58e56712 | 29 | //This could be done using ConfigFile, if it worked |
tzyoung | 2:892b58e56712 | 30 | |
tzyoung | 2:892b58e56712 | 31 | //I need to also check up in whether the limits are being passed to the linear |
tzyoung | 2:892b58e56712 | 32 | //actuator's PID objects. I noticed I have defaults that are appropriate for only |
tzyoung | 2:892b58e56712 | 33 | //the bouyancy engine |
tzyoung | 2:892b58e56712 | 34 | |
tzyoung | 2:892b58e56712 | 35 | bce().setControllerP(BCE_P); |
tzyoung | 2:892b58e56712 | 36 | bce().setControllerI(BCE_I); |
tzyoung | 2:892b58e56712 | 37 | bce().setControllerD(BCE_D); |
tzyoung | 2:892b58e56712 | 38 | bce().setZeroCounts(BCE_ZERO); |
tzyoung | 2:892b58e56712 | 39 | bce().setTravelLimit(BCE_LIMIT); |
tzyoung | 2:892b58e56712 | 40 | bce().setPotSlope(POT_SLOPE); |
tzyoung | 2:892b58e56712 | 41 | |
tzyoung | 2:892b58e56712 | 42 | batt().setControllerP(BATT_P); |
tzyoung | 2:892b58e56712 | 43 | batt().setControllerI(BATT_I); |
tzyoung | 2:892b58e56712 | 44 | batt().setControllerD(BATT_D); |
tzyoung | 2:892b58e56712 | 45 | batt().setZeroCounts(BATT_ZERO); |
tzyoung | 2:892b58e56712 | 46 | batt().setTravelLimit(BATT_LIMIT); |
tzyoung | 2:892b58e56712 | 47 | batt().setPotSlope(POT_SLOPE); |
tzyoung | 2:892b58e56712 | 48 | |
tzyoung | 2:892b58e56712 | 49 | //start up the system timer |
tzyoung | 0:ea293bbf9717 | 50 | systemTime().start(); |
tzyoung | 0:ea293bbf9717 | 51 | |
tzyoung | 0:ea293bbf9717 | 52 | //setup and start the adc. This runs on a fixed interval and is interrupt driven |
tzyoung | 0:ea293bbf9717 | 53 | adc().initialize(); |
tzyoung | 0:ea293bbf9717 | 54 | adc().start(); |
tzyoung | 0:ea293bbf9717 | 55 | |
tzyoung | 2:892b58e56712 | 56 | //start the bce and batt control loops. ADC has to be running first. |
tzyoung | 2:892b58e56712 | 57 | //The motors will not activate until their respective position filters have |
tzyoung | 2:892b58e56712 | 58 | //converged |
tzyoung | 2:892b58e56712 | 59 | bce().start(); |
tzyoung | 2:892b58e56712 | 60 | batt().start(); |
tzyoung | 2:892b58e56712 | 61 | |
tzyoung | 2:892b58e56712 | 62 | //read in a script file describing the dive cycle |
tzyoung | 2:892b58e56712 | 63 | //I envision entries such as |
tzyoung | 2:892b58e56712 | 64 | // target pitch angle target depth target depth rate |
tzyoung | 2:892b58e56712 | 65 | // 10 degrees 5 ft 0.05 ft/s example dive request |
tzyoung | 2:892b58e56712 | 66 | // -10 degrees 0 ft -0.05 ft/s example surface request |
tzyoung | 2:892b58e56712 | 67 | |
tzyoung | 2:892b58e56712 | 68 | //this implies two pid controllers |
tzyoung | 2:892b58e56712 | 69 | // one that manages the batt mover for pitch |
tzyoung | 2:892b58e56712 | 70 | // the other manages the buoyance engine for dive speed |
tzyoung | 2:892b58e56712 | 71 | |
tzyoung | 2:892b58e56712 | 72 | // then some logic is needed to check the box when the desired condition is reached |
tzyoung | 2:892b58e56712 | 73 | // like a waypoint threshold. This allows you to get away from worrying as much about |
tzyoung | 2:892b58e56712 | 74 | // keeping time |
tzyoung | 0:ea293bbf9717 | 75 | |
tzyoung | 0:ea293bbf9717 | 76 | |
tzyoung | 0:ea293bbf9717 | 77 | |
tzyoung | 0:ea293bbf9717 | 78 | while(1) { |
tzyoung | 2:892b58e56712 | 79 | |
tzyoung | 2:892b58e56712 | 80 | //psuedo code to outline what we want to do |
tzyoung | 2:892b58e56712 | 81 | /* |
tzyoung | 2:892b58e56712 | 82 | check for and parse IMU data |
tzyoung | 2:892b58e56712 | 83 | |
tzyoung | 2:892b58e56712 | 84 | poll for depth adc reading (eventually this will come from the external adc) |
tzyoung | 2:892b58e56712 | 85 | |
tzyoung | 2:892b58e56712 | 86 | run depth data through position velocity filter |
tzyoung | 2:892b58e56712 | 87 | |
tzyoung | 2:892b58e56712 | 88 | update the PID controllers for Pitch and depth rate |
tzyoung | 2:892b58e56712 | 89 | |
tzyoung | 2:892b58e56712 | 90 | check whether depth has been triggered |
tzyoung | 2:892b58e56712 | 91 | if so, move on to the next line of the script |
tzyoung | 2:892b58e56712 | 92 | if done , surface or repeat |
tzyoung | 2:892b58e56712 | 93 | |
tzyoung | 2:892b58e56712 | 94 | */ |
tzyoung | 0:ea293bbf9717 | 95 | |
tzyoung | 2:892b58e56712 | 96 | /* |
tzyoung | 2:892b58e56712 | 97 | This can be ignored for now this was the old serial comms stuff for when I |
tzyoung | 2:892b58e56712 | 98 | was prototyping the BCE controls |
tzyoung | 2:892b58e56712 | 99 | |
tzyoung | 0:ea293bbf9717 | 100 | if (pc().readable()) { |
tzyoung | 0:ea293bbf9717 | 101 | // get the key |
tzyoung | 0:ea293bbf9717 | 102 | userInput = pc().getc(); |
tzyoung | 0:ea293bbf9717 | 103 | |
tzyoung | 0:ea293bbf9717 | 104 | //check command against desired control buttons |
tzyoung | 0:ea293bbf9717 | 105 | if (userInput == '=') { |
tzyoung | 0:ea293bbf9717 | 106 | //increment the duty cycle |
tzyoung | 0:ea293bbf9717 | 107 | positionCmd += 5.0 ; |
tzyoung | 0:ea293bbf9717 | 108 | } else if (userInput == '-') { |
tzyoung | 0:ea293bbf9717 | 109 | //decrement the duty cycle |
tzyoung | 0:ea293bbf9717 | 110 | positionCmd -= 5.0 ; |
tzyoung | 0:ea293bbf9717 | 111 | } |
tzyoung | 0:ea293bbf9717 | 112 | |
tzyoung | 0:ea293bbf9717 | 113 | if (userInput == 'w') { |
tzyoung | 0:ea293bbf9717 | 114 | //increment the P gain |
tzyoung | 0:ea293bbf9717 | 115 | P += 0.01; |
tzyoung | 0:ea293bbf9717 | 116 | } |
tzyoung | 0:ea293bbf9717 | 117 | if (userInput == 's') { |
tzyoung | 0:ea293bbf9717 | 118 | //decrement the P gain |
tzyoung | 0:ea293bbf9717 | 119 | P -= 0.01; |
tzyoung | 0:ea293bbf9717 | 120 | } |
tzyoung | 0:ea293bbf9717 | 121 | if (userInput == 'i') { |
tzyoung | 0:ea293bbf9717 | 122 | //increment the D gain |
tzyoung | 0:ea293bbf9717 | 123 | D += 0.001; |
tzyoung | 0:ea293bbf9717 | 124 | } |
tzyoung | 0:ea293bbf9717 | 125 | if (userInput == 'k') { |
tzyoung | 0:ea293bbf9717 | 126 | //decrement the D gain |
tzyoung | 0:ea293bbf9717 | 127 | D -= 0.001; |
tzyoung | 0:ea293bbf9717 | 128 | } |
tzyoung | 0:ea293bbf9717 | 129 | if (userInput == 't') { |
tzyoung | 0:ea293bbf9717 | 130 | //increment the I gain |
tzyoung | 0:ea293bbf9717 | 131 | I += 0.001; |
tzyoung | 0:ea293bbf9717 | 132 | } |
tzyoung | 0:ea293bbf9717 | 133 | if (userInput == 'g') { |
tzyoung | 0:ea293bbf9717 | 134 | //decrement the I gain |
tzyoung | 0:ea293bbf9717 | 135 | I -= 0.001; |
tzyoung | 0:ea293bbf9717 | 136 | } |
tzyoung | 2:892b58e56712 | 137 | if (userInput == ' ') { |
tzyoung | 0:ea293bbf9717 | 138 | //reset the h-bridge |
tzyoung | 2:892b58e56712 | 139 | //hBridge().reset(); |
tzyoung | 0:ea293bbf9717 | 140 | } |
tzyoung | 0:ea293bbf9717 | 141 | |
tzyoung | 0:ea293bbf9717 | 142 | //if (userInput == ' ') { |
tzyoung | 0:ea293bbf9717 | 143 | //stop the ride |
tzyoung | 0:ea293bbf9717 | 144 | // motor_cmd = 0.0 ; |
tzyoung | 0:ea293bbf9717 | 145 | //} |
tzyoung | 0:ea293bbf9717 | 146 | |
tzyoung | 0:ea293bbf9717 | 147 | //clip pwm if over range |
tzyoung | 0:ea293bbf9717 | 148 | //if (motor_cmd > 1.0) { |
tzyoung | 0:ea293bbf9717 | 149 | // motor_cmd = 1.0; |
tzyoung | 0:ea293bbf9717 | 150 | //} |
tzyoung | 0:ea293bbf9717 | 151 | //if (motor_cmd < -1.0) { |
tzyoung | 0:ea293bbf9717 | 152 | // motor_cmd = -1.0; |
tzyoung | 0:ea293bbf9717 | 153 | //} |
tzyoung | 0:ea293bbf9717 | 154 | // assign the shiny new commands to the pins |
tzyoung | 1:133216180125 | 155 | //hBridge().run(motor_cmd); |
tzyoung | 0:ea293bbf9717 | 156 | if (userInput == '\r') { |
tzyoung | 2:892b58e56712 | 157 | bce().setPosition_mm(positionCmd); |
tzyoung | 2:892b58e56712 | 158 | // posCon().setPgain(P); |
tzyoung | 2:892b58e56712 | 159 | // posCon().setIgain(I); |
tzyoung | 2:892b58e56712 | 160 | // posCon().setDgain(D); |
tzyoung | 2:892b58e56712 | 161 | }*/ |
tzyoung | 0:ea293bbf9717 | 162 | |
tzyoung | 2:892b58e56712 | 163 | // } |
tzyoung | 0:ea293bbf9717 | 164 | |
tzyoung | 2:892b58e56712 | 165 | //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 | 2:892b58e56712 | 166 | //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); |
tzyoung | 0:ea293bbf9717 | 167 | |
tzyoung | 0:ea293bbf9717 | 168 | |
tzyoung | 0:ea293bbf9717 | 169 | } |
tzyoung | 0:ea293bbf9717 | 170 | } |