Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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@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?

UserRevisionLine numberNew 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 }