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