Diff: main.cpp

Revision:

65:2ac186553959

Parent:

63:6cb0405fc6e6

Child:

66:0f20870117b7

diff -r a8939bc127ab -r 2ac186553959 main.cpp
--- a/main.cpp	Mon Jun 18 21:04:09 2018 +0000
+++ b/main.cpp	Tue Jun 19 18:10:50 2018 +0000
@@ -37,60 +37,163 @@
     
     Modified FSG PCB V_1_4
         - adcLed = 0; in adc
+    
+    Modified FSG PCB V_1_5
+        - IMU update
 */
- 
+
+/* removed unused variables */
 #include "mbed.h"
 #include "StaticDefs.hpp"
+
+#ifndef lround
+#define lround(var)    (long)(var+0.5f)
+#endif
+
+////////////////////////////////////////////////////////////////// NEW TICKER
+Ticker systemTicker;
+bool setup_complete = false;
+volatile unsigned int bTick = 0;
+volatile unsigned int timer_counter = 0;
+
+char hex[9];
+
+char * conversion(float input_float) {
+    int integer_number = lround(100.0 * input_float);      //convert floating point input to integer to broadcast over serial (reduce precision)
+    
+    memset(hex, 0, sizeof(hex) );   //  void* memset( void* dest, int ch, size_t count );   // CLEAR IT  (need null at end of string)
+    sprintf(hex, "%8x", integer_number);                                 //generates spaces 0x20
+    return hex;
+}
+
+static unsigned int read_ticker(void) {      //Basically this makes sure you're reading the data at one instance (not while it's changing)
+    unsigned int val = bTick;
+    if( val )
+        bTick = 0;
+    return( val );
+}
+////////////////////////////////////////////////////////////////// NEW TICKER
  
 // loop rate used to determine how fast events trigger in the while loop
-Ticker loop_rate_ticker;
-Ticker log_loop_rate_ticker;
+//Ticker main_loop_rate_ticker;
+//Ticker log_loop_rate_ticker;
 
-volatile bool loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
+volatile bool fsm_loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
 volatile bool log_loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
 
-void loop_trigger() { loop = true;} // loop trigger (used in while loop)
+void loop_trigger() { fsm_loop = true;} // loop trigger (used in while loop)
 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
 
-/****************** for rudder servo    ******************/
-Ticker systemTicker;
+static int current_state = 0;     
+static bool file_opened = false;
+
+void FSM() {                    // FSM loop runs at 100 hz
+    if(fsm_loop) {
+        fsm_loop = false;       // wait until the loop rate timer fires again
+        current_state = stateMachine().runStateMachine();       //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
+    }
+}
+
+void log_function() {    
+    // log loop runs at 1 hz
+    if (log_loop) {
+        //when the state machine is not in SIT_IDLE state (or a random keyboard press)
+
+        if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {                
+            ;   //pass
+        }
+        
+        else if(current_state != 0) {
+            if (!file_opened) {                                 //if the log file is not open, open it
+                mbedLogger().appendLogFile(current_state, 0);   //open MBED file once
+                //sdLogger().appendLogFile(current_state, 0);     //open SD file once
+                           
+                file_opened = true;                             //stops it from continuing to open it
 
-volatile unsigned int timer_counter = 0;
+                pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
+            }
+            
+            //record to Mbed file system   
+            led4() = !led4();
+            
+            mbedLogger().appendLogFile(current_state, 1);    //writing data
+            //sdLogger().appendLogFile(current_state, 1);    //writing data
+        }
+        
+        //when the current FSM state is zero (SIT_IDLE), close the file
+        else {
+            //this can only happen once
+            if (file_opened) {
+                mbedLogger().appendLogFile(current_state, 0);    //close log file
+                //sdLogger().appendLogFile(current_state, 0);    //close log file
+                
+                file_opened = false;
+                
+                pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
+            }
+        }
+    }   //END OF LOG LOOP
+    
+    log_loop = false;   // wait until the loop rate timer fires again
+}
 
-bool setup_complete = false;
-
-static void system_timer(void) {    
+static void system_timer(void) {
+    bTick = 1;
+    
     timer_counter++;
     
     //only start these updates when everything is properly setup (through setup function)
     if (setup_complete) {
-            if ( (timer_counter % 20) == 0 ) {    // 0.02 second intervals
-                rudder().runServo();
-            }
+        if ( (timer_counter % 1) == 0) {    //this runs at 0.001 second intervals (1000 Hz)
+            adc().update();  //every iteration of this the A/D converter runs   //now this runs at 0.01 second intervals 03/12/2018
+        }
+        
+        if ( (timer_counter % 10) == 0) {
+            bce().update();      //update() inside LinearActuator class (running at 0.01 second intervals)
+            batt().update();
+            led2() = !led2();
+        }
+        
+        if ( (timer_counter % 20) == 0 ) {    // 0.02 second intervals
+            rudder().runServo();
+        }
+        
+        if ( (timer_counter % 50) == 0 ) {    // 0.05 second intervals 
+            //imu().runIMU();
+        }
+        
+        if ( (timer_counter % 100) == 0) {     // 100,000 microseconds = 0.1 second intervals
+            depthLoop().runOuterLoop();
+            pitchLoop().runOuterLoop();
+            headingLoop().runOuterLoop();
+        }
+        
+        if ( (timer_counter % 500) == 0) {     // 500,000 microseconds = 0.5 second intervals
+            //serialComms().getDepthPitchHeading();
+            log_loop = true;
+            log_function();
+       }
     }
 }
 
-/****************** for rudder servo    ******************/
-
 void setup() {
     pc().baud(57600);
-    pc().printf("\n\n\rFSG PCB V1.4 2018-06-18 \n\n\r");
-
-/*    //setup data logger baud rate and write the start of the program (every time you reset)
-    datalogger().baud(57600);
-    datalogger().printf("SYSTEM, RESET\n");
-*/
-      
+    pc().printf("\n\n\r 2018_03_28_wireless (FSG bench test)\n\n\r");
+ 
     // start up the system timer
-    systemTime().start();
+    //systemTimer().start();
  
     // set up and start the adc. This runs on a fixed interval and is interrupt driven
-    adc().initialize();  
-    adc().start();
+    adc().initialize();
+    //adc().start();
+    
+    // setup and run the rudder(servo) pwm signal (start the ticker)
+    //rudder().init();
+    pc().printf("Rudder servo initialized!\n\r");
     
     // set up and start the imu. This polls in the background
     imu().initialize();
-    imu().start();
+    //imu().start();
     
     // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
     depth().init();
@@ -100,128 +203,97 @@
     local();
     
     // construct the SD card file system
-//    sd_card();
+    //sd_card();
  
     // load config data from files
-    configFileIO().load_BCE_config();      // load the buoyancy engine parameters from the file "bce.txt"
-    configFileIO().load_BATT_config();     // load the battery mass mover parameters from the file "batt.txt"
-    configFileIO().load_DEPTH_config();    // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
-    configFileIO().load_PITCH_config();    // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
- 
+    configFileIO().load_BCE_config();       // load the buoyancy engine parameters from the file "bce.txt"
+    configFileIO().load_BATT_config();      // load the battery mass mover parameters from the file "batt.txt"
+    
+    configFileIO().load_DEPTH_config();     // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
+    configFileIO().load_PITCH_config();     // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
+    
+    configFileIO().load_RUDDER_config();    // load the rudder servo inner loop parameters from the file "SERVO.txt"
+    configFileIO().load_HEADING_config();   // load the rudder servo outer loop HEADING control parameters from the file "HEADING.txt" (contains neutral position)
  
     // set up the linear actuators.  adc has to be running first.
+    bce().setPIDHighLimit(bce().getTravelLimit());     //travel limit of this linear actuator
     bce().init();
-    bce().start();
+    //bce().start();  //removed start, it's handled by the interrupt
     bce().pause(); // start by not moving
  
+    batt().setPIDHighLimit(batt().getTravelLimit());    //travel limit of this linear actuator
     batt().init();
-    batt().start();
+    batt().runLinearActuator(); // _init = true;
+    //batt().start();//removed start, it's handled by the interrupt
     batt().pause(); // start by not moving
  
-    // set up the depth and pitch outer loop controllers
+    // set up the depth, pitch, and rudder outer loop controllers
     depthLoop().init();
-    depthLoop().start();
+    //removed start, it's handled by the interrupt
     depthLoop().setCommand(stateMachine().getDepthCommand());
  
     pitchLoop().init();
-    pitchLoop().start();
+    //removed start, it's handled by the interrupt
     pitchLoop().setCommand(stateMachine().getPitchCommand());
     
-    //new 6/8/2018 for testing
     headingLoop().init();           
-    headingLoop().setCommand(0.0);  //setting heading to 0 for testing
-    //new 6/8/2018 for testing
+    //removed start, it's handled by the interrupt
+    //headingLoop().setCommand(stateMachine().getHeadingCommand());         // FIX LATER
+    //heading flag that adjust the PID error is set in the constructor
+    
+    //systemTicker.attach_us(&system_timer, 10000);         // Interrupt timer running at 0.01 seconds       (slower than original ADC time interval)
+    
+    
  
     // show that the PID gains are loading from the file
     pc().printf("bce    P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
     pc().printf("batt   P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
-    pc().printf("depth  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
-    pc().printf("pitch  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
+    pc().printf("rudder min pwm: %6.2f, max pwm: %6.2f, center pwm: %6.2f, min deg: %6.2f, max deg: %6.2f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
+    
+    pc().printf("depth   P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
+    pc().printf("pitch   P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
+    pc().printf("heading P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f deg (deadband: %0.1f)\r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset(), headingLoop().getDeadband());
+    
     pc().printf("\n\r");
          
     //load sequence from file
     sequenceController().loadSequence();
- 
-    // establish the main loop rate
-    loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate
     
-    // setup the data logger rate
-    log_loop_rate_ticker.attach(&log_loop_trigger, 1.0); // fires the ticker at 1 Hz rate (every second)
- 
     //set time of logger (to current or close-to-current time)
-    mbedLogger().setLogTime();   
-//    sdLogger().setLogTime();
+    mbedLogger().setLogTime();
+    //sdLogger().setLogTime();
     
     //create log files if not present on file system
     mbedLogger().initializeLogFile();
-//    sdLogger().initializeLogFile();
-
-    //tare the pressure sensor during setup
-    depth().tare();
+    //sdLogger().initializeLogFile();
+    
+    setup_complete = true;
+}
 
-    setup_complete = true;  //used for interrupt timing
-
-} 
- 
 int main() {
     setup();
     
-    //this is used to control timing on the rudder servo, it's based on the new timing model in the 5/29/18 code with one interrupt running everything...may or may not work here
+    unsigned int tNow = 0;
+    
+    pc().baud(57600);
+    pc().printf("\n\n\r  TICKER TEST 05/25/2018 running at 10 kHz (0.0001 second interval) \n\n\r");
     
     systemTicker.attach_us(&system_timer, 1000);         // Interrupt timer running at 0.001 seconds       (slower than original ADC time interval)
+        
+    while (1) {        
+        if( read_ticker() )                         // read_ticker runs at the speed of 10 kHz
+        {
+            ++tNow;
 
-    while(1) {
-        static int current_state = 0;
-        
-        static bool file_opened = false;
-        
-        // FSM loop runs at 10 hz
-        if(loop) {
-            led1() = !led1(); // blink led 1
-            current_state = stateMachine().runStateMachine();       //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
-            loop = false; // wait until the loop rate timer fires again
-        }
-        
-        // log loop runs at 1 hz
-        if (log_loop) {
-            //when the state machine is not in SIT_IDLE state (or a random keyboard press)
-            if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {
-                //main_loop_rate_ticker.detach();
-                //log_loop_rate_ticker.detach();
-                
-                ;   //pass
+            if ( (tNow % 10) == 0 ) {   // 0.001 second intervals               //if ( (tNow % 100) == 0 ) {   // 0.01 second intervals
+                fsm_loop = true;
+                FSM();
             }
             
-            else if(current_state != 0) {
-                if (!file_opened) {                                 //if the log file is not open, open it
-                    mbedLogger().appendLogFile(current_state, 0);   //open MBED file once
-//                    sdLogger().appendLogFile(current_state, 0);     //open SD file once
-                               
-                    file_opened = true;                             //stops it from continuing to open it
-
-                    pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
-                }
-                
-                //record to Mbed file system     
-                mbedLogger().appendLogFile(current_state, 1);    //writing data
-//                sdLogger().appendLogFile(current_state, 1);    //writing data
-            }
-            
-            //when the current FSM state is zero, reset the file
-            else {
-                //this can only happen once
-                if (file_opened) {
-                    mbedLogger().appendLogFile(current_state, 0);    //close log file
-//                    sdLogger().appendLogFile(current_state, 0);    //close log file
-                    
-                    file_opened = false;
-                    
-                    pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
-                }
-            }
-            
-            log_loop = false;   // wait until the loop rate timer fires again
-        }   //END OF LOG LOOP
+//            if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
+//                log_loop = true;
+//                log_function();
+//            }
+        }
     }
-
 }
\ No newline at end of file