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

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