Diff: main.cpp

Revision:

10:085ab7328054

Parent:

5:15bd96205bb2

Child:

11:3b241ecb75ed

--- a/main.cpp	Fri Oct 20 11:41:22 2017 +0000
+++ b/main.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -1,120 +1,155 @@
+/*
+    Starting from Trent's Linear Actuator code from 2017-10-19, these modifications
+    by Dan add an outer loop controller for depth and pitch to command the inner
+    linear actuator loops.
+    Modified 2017-10-20 revA by Dan
+        - added outer loop controller, but it is hanging the mbed. (turns out it was the imu update)
+    Modified 2017-10-22 revA by Dan
+        - outer loop now works with a call to outerloop.update() in main loop(), not with an attached ticker
+    Modified 2017-10-22 revB by Dan
+        - enabled both depth and pitch outer loop controllers
+        - added ability to keyboard reset
+    Modified 2017-10-22 revC by Dan
+        - major update to the IMU library processing to make a state machine that doesn't hang
+        - added lat/lon/alt and GNSS fix information to the IMU library
+        - brought out the pin names into the constructors of IMU, omega, SpiADC
+    Modified 2017-10-22 revD by Dan
+        - everything seems to be working, shy of re-checking on the hardware
+        - Depth sensor call done inside the OuterLoop, but should somehow be set as a callback instead
+        - IMU sensor call done inside the OuterLoop, but should somehow be set as a callback instead
+    Modified 2017-10-23 revA by Dan/Matt
+        - linear actuator hardware works great, limit switches, sensing, etc.
+        - outer loops run, but move the BCE in the wrong direction.
+        - new IMU code doesn't read from the sensor correctly, but doesn't hang up either.
+        - depth sensor worked fine, just needs zero bias adjustment.
+*/
+
 #include "mbed.h"
 #include "StaticDefs.hpp"
 #include "config_functions.h"
 
-int main()
-{
-    float positionCmd = 10.0;
-    char userInput;
-
-    local();  //this makes sure the local file system is constructed
-
-    //Read in and load the BCE parameters from the text file "bce.txt"
-    load_BCE_config(); // should say "bce.load_config(bce.txt);
-    bce().init();
+extern "C" void mbed_reset();           // utilized to reset the mbed
 
-    ////Read in and load the battery mover parameters from the text file "batt.txt"
-    load_BATT_config();
-    batt().init();
+void setup() {
+    pc().printf("\n\n\r\rFSG 2017-10-22 revD\n\r");
 
-    // do not leave this in. It is just there to check that things are working
-    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());
-    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());
-    //Front load the desired parameters into the linear acuator objects.
-    //This could be done using ConfigFile, if it worked
-
-    //I need to also check up in whether the limits are being passed to the linear
-    //actuator's PID objects. I noticed I have defaults that are appropriate for only
-    //the bouyancy engine
-
-    //start up the system timer
+    // start up the system timer
     systemTime().start();
 
-    //setup and start the adc. This runs on a fixed interval and is interrupt driven
+    // set up and start the adc. This runs on a fixed interval and is interrupt driven
     adc().initialize();
     adc().start();
+    
+    // set up and start the imu. This polls in the background
+    imu().initialize();
+    imu().start();
+    
+    // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
+    depth().initialize();
+    
+    // construct a local file system
+    local();
 
-    //start the bce and batt control loops.  ADC has to be running first.
-    //The motors will not activate until their respective position filters have
-    //converged, but just to be sure we pause them until we want them to really run
-    bce().start();
-    bce().pause();
-
-    bce().setPosition_mm(positionCmd);
+    // load config data from files
+    load_BCE_config();      // load the buoyancy engine parameters from the file "bce.txt"
+    load_BATT_config();     // load the battery mass mover parameters from the file "batt.txt"
+    load_DEPTH_config();    // load the depth control loop parameters from the file "depth.txt"
+    load_PITCH_config();    // load the depth control loop parameters from the file "pitch.txt"
 
-    bce().unpause();
+    // set up the linear actuators.  adc has to be running first.
+    bce().init();
+    bce().start();
+    bce().setPosition_mm(bce().getPosition_mm()); // start by not moving
 
+    batt().init();
     batt().start();
-    batt().pause();
-    
-    batt().setPosition_mm(positionCmd);
-    
-    batt().unpause();
+    batt().setPosition_mm(batt().getPosition_mm()); // start by not moving
+
+    // set up the depth and pitch outer loop controllers
+    depthLoop().init();
+    depthLoop().setCommand(0.0);
 
+    pitchLoop().init();
+    pitchLoop().setCommand(0.0);
+
+    // do not leave this in. Check that PID gains are loading
+    pc().printf("bce    P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f  \n\r", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
+    pc().printf("batt   P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f  \n\r", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
+    pc().printf("depth  P: %3.2f, I: %3.2f, D %3.2f, \n\r", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD());
+    pc().printf("pitch  P: %3.2f, I: %3.2f, D %3.2f, \n\r", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD());
+    pc().printf("\n\r");
+}
 
-
-
-    while(1) {
-
-        
-        if (pc().readable()) {
-            // get the key
-            userInput = pc().getc();
+void keyboard() {
+    static float setpoint = 50.0;
+    static bool isOuterLoopActive = false; // allow user to select if the outer loop is active
+    char userInput;
+    
+    // check keyboard and make settings changes as requested
+    if (pc().readable()) {
+        // get the key
+        userInput = pc().getc();
 
-            //check command against desired control buttons
-            if (userInput == '=') {
-                //increment the duty cycle
-                positionCmd += 5.0 ;
-            } else if (userInput == '-') {
-                //decrement the duty cycle
-                positionCmd -= 5.0 ;
-            } else if (userInput == '\r') {
-                batt().setPosition_mm(positionCmd);
-                bce().setPosition_mm(positionCmd);
+        // check command against desired control buttons
+        if (userInput == '+' or userInput == '=') {
+            setpoint += 1.0; //increment the command
+        }
+        else if (userInput == '-' or userInput == '_') {
+            setpoint -= 1.0; //decrement the command
+        }
+        else if (userInput == 't' or userInput == 'T') {
+            isOuterLoopActive = !isOuterLoopActive; // toggle the outer loop
+            
+            if (isOuterLoopActive)
+                pc().printf("Outer loops are now active                                \n\r");
+            else
+                pc().printf("Outer loops are now inactive                                \n\r");
+        }
+        else if (userInput == '\r') {
+            if (isOuterLoopActive) {
+                pc().printf("setting outer loop: %3.0f                                \n\r", setpoint);
+                
+                depthLoop().setCommand(setpoint);
+                bce().setPosition_mm(depthLoop().getOutput());  // connect outer to inner loop
+                
+                pitchLoop().setCommand(setpoint);
+                batt().setPosition_mm(pitchLoop().getOutput()); // connect outer to inner loop
+            }
+            else {
+                pc().printf("setting inner loop: %3.0f                                \n\r", setpoint);
+                
+                batt().setPosition_mm(setpoint);
+                bce().setPosition_mm(setpoint);
             }
         }
-        //pc().printf("set point %3.1f  \r", positionCmd);
-        //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);
-        //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);
-        pc().printf("pos bce: %3.2f mm, pos batt: %3.2f mm, Set Point %3.1f       \r", bce().getPosition_mm(), batt().getPosition_mm(), positionCmd);
+        else if (userInput == '?') {
+            pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
+            mbed_reset();
+        }
     }
+    
+    if (isOuterLoopActive)
+        pc().printf("setpoint %3.1f, depth: %3.1f ft, bce: %3.1f mm       \r", setpoint, depth().getDepth(), bce().getPosition_mm());
+        //pc().printf("setpoint %3.1f, pitch: %3.1f deg, batt: %3.1f mm       \r", setpoint, imu().getPitch(), batt().getPosition_mm());
+    else
+        pc().printf("setpoint %3.1f, bce:   %3.1f mm       \r", setpoint, bce().getPosition_mm());
+        //pc().printf("setpoint %3.1f, batt:  %3.1f mm       \r", setpoint, batt().getPosition_mm());
 }
 
-
-
-    //read in a script file describing the dive cycle
-    //I envision entries such as
-    // target pitch angle  target depth  target depth rate
-    // 10 degrees          5 ft          0.05 ft/s         example dive request
-    // -10 degrees         0 ft          -0.05 ft/s        example surface request
-
-    //this implies two pid controllers
-    // one that manages the batt mover for pitch
-    // the other manages the buoyance engine for dive speed
-
-    // then some logic is needed to check the box when the desired condition is reached
-    // like a waypoint threshold. This allows you to get away from worrying as much about
-    // keeping time 
+void loop() {
+    led1() = !led1(); // blink
+    
+    keyboard();
     
-    
-    
+    // sequence controllers...
+    // check whether depth has been triggered
+    //    if so, move on to the next line of the script
+    //    if done, surface or repeat
+}
 
-        //psuedo code to outline what we want to do
-        /*
-        check for and parse IMU data
-
-        poll for depth adc reading (eventually this will come from the external adc)
-
-        run depth data through position velocity filter
-
-        update the PID controllers for Pitch and depth rate
-
-        check whether depth has been triggered
-            if so, move on to the next line of the script
-            if done , surface or repeat
-
-        */
-
-
-        //This can be ignored for now this was the old serial comms stuff for when I
-        //was prototyping the BCE controls
\ No newline at end of file
+int main() {
+    setup();
+    while(1) {
+        loop();
+    }
+}
\ No newline at end of file