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

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