Diff: main.cpp

Revision:

2:892b58e56712

Parent:

1:133216180125

Child:

3:7824127c5cfd

--- a/main.cpp	Thu Apr 27 16:47:29 2017 +0000
+++ b/main.cpp	Fri Oct 06 13:25:26 2017 +0000
@@ -1,46 +1,102 @@
 #include "mbed.h"
 #include "StaticDefs.hpp"
-#include "ltc1298.hpp"
+#include "config_functions.h"
+
+#define BCE_P 0.1
+#define BCE_I 0.0
+#define BCE_D 0.0
+
+#define BCE_ZERO 400    //counts
+#define BCE_LIMIT 498.729   //mm
+#define POT_SLOPE .121760   // mm/counts
+
+#define BATT_P 0.1
+#define BATT_I 0.0
+#define BATT_D 0.0
+
+#define BATT_ZERO 100    //counts
+#define BATT_LIMIT 50.0 //mm
+
+#include <fstream>
+#include <iostream>
 
 int main()
 {
-    //start up the system timer so the position velocity estimator has a time signal
+    //initialize linear actuator objects (doesn't work) 
+    load_BCE_config();
+    
+    //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
+    
+    bce().setControllerP(BCE_P);
+    bce().setControllerI(BCE_I);
+    bce().setControllerD(BCE_D);
+    bce().setZeroCounts(BCE_ZERO);
+    bce().setTravelLimit(BCE_LIMIT);
+    bce().setPotSlope(POT_SLOPE);
+    
+    batt().setControllerP(BATT_P);
+    batt().setControllerI(BATT_I);
+    batt().setControllerD(BATT_D);
+    batt().setZeroCounts(BATT_ZERO);
+    batt().setTravelLimit(BATT_LIMIT);
+    batt().setPotSlope(POT_SLOPE);
+
+    //start up the system timer
     systemTime().start();
 
     //setup and start the adc. This runs on a fixed interval and is interrupt driven
     adc().initialize();
     adc().start();
 
-    //Initialize the position velocity filter. This will consume a couple of seconds for
-    //the filter to converge
-    pvf().init();
-
+    //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
+    bce().start();
+    batt().start();
+    
+    //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            
 
-    float motor_cmd = 0.0;
-    float positionCmd = 250.0;
-    float P = 0.10;
-    float I = 0.00;
-    float D = 0.00;
-    char userInput;
-
-    hBridge().run(motor_cmd);
-
-    //set the intial gains for the position controller
-    posCon().setPgain(P);
-    posCon().setIgain(I);
-    posCon().setDgain(D);
-    posCon().writeSetPoint(positionCmd);
 
 
     while(1) {
-
-        // update the position velocity filter
-        pvf().update();
+        
+        //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
+        
+*/
 
-        //update the controller with the current numbers in the position guesser
-        posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ;
-        hBridge().run(posCon().getOutput());
-
+/*
+        This can be ignored for now this was the old serial comms stuff for when I
+        was prototyping the BCE controls
+        
         if (pc().readable()) {
             // get the key
             userInput = pc().getc();
@@ -78,9 +134,9 @@
                 //decrement the I gain
                 I -= 0.001;
             }
-            if (userInput == ' '){
+            if (userInput == ' ') {
                 //reset the h-bridge
-                hBridge().reset();    
+                //hBridge().reset();
             }
 
             //if (userInput == ' ') {
@@ -98,15 +154,16 @@
             // assign the shiny new commands to the pins
             //hBridge().run(motor_cmd);
             if (userInput == '\r') {
-                posCon().writeSetPoint(positionCmd);
-                posCon().setPgain(P);
-                posCon().setIgain(I);
-                posCon().setDgain(D);
-            }
+                bce().setPosition_mm(positionCmd);
+                //    posCon().setPgain(P);
+                //    posCon().setIgain(I);
+                //    posCon().setDgain(D);
+            }*/
 
-        }
+     //   }  
 
-        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);
+        //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());
+        //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);
 
 
     }