Diff: main.cpp

Revision:

5:864709d3eb76

Parent:

4:cae255669971

--- a/main.cpp	Fri Jun 29 02:30:38 2018 +0000
+++ b/main.cpp	Fri Jun 29 17:49:40 2018 +0000
@@ -15,6 +15,7 @@
 #include "defines.h"
 #include "utilities.h"
 #include "FreeFlyerHardware.h"
+#include "debug.h"
 
 extern "C" void mbed_reset();       //!< Makes the C++ function "mbed_reset" have 'C'-compiler linkage
 
@@ -25,7 +26,11 @@
 +==============================================*/
 
 // ROS
+#if (ROS_SERIAL_MODE)
 ros::NodeHandle nh; // Comment out during serial tests
+#else
+Serial pc(USBTX, USBRX);
+#endif
 
 /*=============================================+
 |   ENUMERATIONS
@@ -79,159 +84,25 @@
 
 }   // End "mbed" namespace
 
-//#####################################################################################\\
-
-/*
-void printBits(char myByte){
- for (char mask = 0x80; mask; mask >>= 1){
-   if(mask  & myByte)
-       pc.putc('1');
-   else
-       pc.putc('0');
- }
-}*/
-
-/*
-////////
-// Blinky test code
-void messageCb(const std_msgs::Empty& toggle_msg){
-    LED_debugA = !LED_debugA;   // blink the led
-}
-
-ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb);
-*/
-
 /*=============================================+
 |   MAIN FUNCTION
 +==============================================*/
-
 int main() {
 
-mbed::i2c.frequency(10000); // set required i2c frequency
-
-// Testing Wheel Encoder
-/*Serial pc(USBTX, USBRX); // Comment out during normal operation
-pc.printf("Hello World!\n");
-QEI wheel_encoder(PIN_WENCA, PIN_WENCB, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
-Timer t_QEI;
-t_QEI.reset();
-t_QEI.start();
-while (true) {
-    if (t_QEI.read_ms() >= 500) {    
-        t_QEI.reset();
-        pc.printf("Current number of counts: %d\n", wheel_encoder.getPulses());
-        pc.printf("Measured wheel speed: %f\n", -wheel_encoder.getSpeed()*COUNTS2SHAFT);
-    }
-}*/
-    
-
-////////
-// Blinky test code
-/*    nh.getHardware()->setBaud(115200);
-    nh.initNode();
-    nh.subscribe(sub);
-
-    while (1) {
-        nh.spinOnce();
-        wait_ms(1);
-    }
-*/
-/*
-    // I2C device searching script
-    //mbed::i2c.frequency(10000); // set required i2c frequency
-    
-    pc.printf("I2C Searching!\n\n");
-    pc.printf("Starting....\n\n");    
-    while (1) {
-        int count = 0;
-        for (int address=0; address<256; address+=2) {
-            if (!mbed::i2c.write(address, NULL, 0)) { // 0 returned is ok                
-                pc.printf("I2C address 0x%02X\n", address);
-                count++;
-            }
-        }        
-        pc.printf("%d devices found\n\n\n", count);
-        wait(0.000001);       
-    }
-    
-    //mbed::pc.printf("Hello World!\n");
-
-*/
-
-/*======
-// Testing bit masking
-=======*/
+    mbed::i2c.frequency(10000); // set required i2c frequency
 
-/*char cmd;
-while (true) {
-    cmd = 0x00;
-    cmd |= (1UL << (0x02-2));
-    printBits(cmd);
-    pc.printf("\n");
-    wait(1.0);
-}*/
-    
-/******************************************
-   RGB LED testing code, FF Driver v2.0
-******************************************/
-/*char cmd[19];
-led_inv_out_en = 0;
+    /****************************************************
+       Debug Scripts (Toggle Serial in debug_defines.h)
+    *****************************************************/
+    //testWheelEncoder(pc);               // Testing Wheel Encoder
+    //searchForI2CDevices(pc, mbed::i2c); // I2C device searching script
+    //testBitMasking(pc);
+    //RGBLEDTesting(pc, mbed::i2c, led_inv_out_en); // RGB LED testing workspace
 
-while (true) {
-    
-    wait(0.5);
-    
-    cmd[0] = 0x80;  // Send control register: start with register 00, autoincrement
-    cmd[1] = 0x80;  // MODE1 (default value, in normal power mode)
-    cmd[2] = 0x09;  // MODE2 (default value, but with invert off, outputs change on ack, open-drain)    
-    cmd[3] = 0x00;  // PWM0 (B2)
-    cmd[4] = 0x00;  // PWM1 (W2 - ignore)
-    cmd[5] = 0x00;  // PWM2 (G2)
-    cmd[6] = 0x00;  // PWM3 (R2)
-    cmd[7] = 0x00;  // PWM4  (B1)
-    cmd[8] = 0x00;  // PWM5 (W1 - ignore)
-    cmd[9] = 0x00;  // PWM6 (G1)
-    cmd[10] = 0x00;  // PWM7 (R1)
-    cmd[11] = 0xff;  // GRPPWM (default value)
-    cmd[12] = 0x00;  // GRPFREQ (default value)
-    cmd[13] = 0x55;  // LEDOUT0 (0x55: all fully on)
-    cmd[14] = 0x55;  // LEDOUT1 (0x55: all fully on)
-    cmd[13] = 0x00;  // LEDOUT0 (0x55: all fully off)
-    cmd[14] = 0x00;  // LEDOUT1 (0x55: all fully off)
-    cmd[15] = 0x00;  // SUBADR1
-    cmd[16] = 0x00;  // SUBADR2
-    cmd[17] = 0x00;  // SUBADR3
-    cmd[18] = 0x00;  // ALLCALLADR
-    
-    mbed::i2c.write(ADDR_RGB, cmd, 19);
-    
-    //cmd[0] = 0x8d;  // Send control register: start with register 12, autoincrement
-    //cmd[1] = 0x55;  // LEDOUT0 (0x55: all fully on)
-    //cmd[2] = 0x55;  // LEDOUT1 (0x55: all fully on)
-    //cmd[1] = 0x00;  // LEDOUT0 (0x55: all fully off)
-    //cmd[2] = 0x00;  // LEDOUT1 (0x55: all fully off)
-    
-    //mbed::i2c.write(ADDR_RGB, cmd, 3);
-    //mbed::i2c.write(mbed::addr, cmd, 3);
-    
-    cmd[0] = 0x80;
-    mbed::i2c.write(ADDR_RGB, cmd, 1);
-    for (int i = 0; i < 18; i++)
-        cmd[i] = 0;
-    
-    mbed::i2c.read(ADDR_RGB, cmd, 18);
-    
-    for (int i = 0; i < 18; i++) {
-        printBits(cmd[i]);
-        pc.printf("\n");
-    }
-    pc.printf("\n");
-}*/
-
-/******************************
-    Normal code
-*******************************/
-
+    /************
+        Setup
+    ************/
+#if (ROS_SERIAL_MODE)
     bool verbose = true;            // Toggle debug topic output
     Timer t_LED, t_RGB_LED, t_PID, t_thrust, t_meas, t_pid_debug;   // object to do timing to compute motor speed
     
@@ -257,18 +128,21 @@
     t_meas.start();
     t_pid_debug.start();
     
-#if (MODE == DEBUG_THRUSTERS)
+    #if (MODE == DEBUG_THRUSTERS)
     int thruster_ID_on = 0;
     int thruster_pinout_cmd[8];
-#elif (MODE == DEBUG_LED_CONST)
+    #elif (MODE == DEBUG_LED_CONST)
     freeflyer.rgba_led_->setColor(color::magenta);
     freeflyer.rgba_led_->setBrightness(1.0);
-#elif (MODE == DEBUG_LED_COLOR_SEQ)
+    #elif (MODE == DEBUG_LED_COLOR_SEQ)
     freeflyer.rgba_led_->setColor(color::blue);
     freeflyer.rgba_led_->setBrightness(1.0);
     freeflyer.rgba_led_->setColorProgram(color_program::fade1, color_time_program::fade1);
-#endif
+    #endif
     
+    /***************
+        Main Loop
+    ****************/
     while (true) {
     
         // LED Blinking Loop
@@ -280,21 +154,20 @@
         // PID Update Loop
         if (t_PID.read_ms() >= PID_PERIOD) {    
             t_PID.reset();
-#if (MODE == DEBUG_WHEEL_CONST)
+            
+            #if (MODE == NORMAL)
+            if (!freeflyer.duty_cycle_command_mode_)
+                freeflyer.updatePID();
+            else
+                freeflyer.setPWMOut(freeflyer.duty_cycle_cmd_);
+            #elif (MODE == DEBUG_WHEEL_CONST)
             mbed::LED_debugA = !mbed::LED_debugA;
             freeflyer.setPWMOut(DWC_PWM);
-#elif (MODE == DEBUG_WHEEL_PID)
+            #elif (MODE == DEBUG_WHEEL_PID)
             mbed::LED_debugA = !mbed::LED_debugA;
             freeflyer.setPIDSetpoint(DWP_SETPOINT);
             freeflyer.updatePID();
-#else
-            if (!freeflyer.duty_cycle_command_mode_) {
-                freeflyer.updatePID();
-            } else {
-                freeflyer.setPWMOut(freeflyer.duty_cycle_cmd_);
-            }
-#endif
-
+            #endif
             mbed::commandMotor(utils::min(freeflyer.getPWMOut(), PWM_LIMIT));
         }
             
@@ -305,7 +178,12 @@
         }
         
         // Thruster PWM Loop
-#if (MODE == DEBUG_THRUSTERS)
+        #if (MODE == NORMAL)
+        if (t_thrust.read_ms() >= THRUST_PERIOD/THRUST_PWM_N) {
+            t_thrust.reset();
+            freeflyer.stepThrusterPWM();
+        }
+        #elif (MODE == DEBUG_THRUSTERS)
         if (t_thrust.read_ms() >= DT_PERIOD) {
             t_thrust.reset();
             mbed::LED_debugA = !mbed::LED_debugA;
@@ -322,12 +200,7 @@
             
             freeflyer.commandThrusters(thruster_pinout_cmd);
         }
-#else
-        if (t_thrust.read_ms() >= THRUST_PERIOD/THRUST_PWM_N) {
-            t_thrust.reset();
-            freeflyer.stepThrusterPWM();
-        }
-#endif
+        #endif
         
         // Measurement publishing loop
         if (t_meas.read_ms() >= MEAS_PERIOD && nh.connected()) {
@@ -342,7 +215,7 @@
         }
 
         nh.spinOnce();
-        
     }
-
+    
+#endif
 }
\ No newline at end of file