main.cpp

00001 
00002 /*! \file main.cpp
00003 \brief Main file for operating the ASL 3-DOF Spacecraft driver board
00004 \author A. Bylard
00005 */
00006 
00007 // test comment
00008 
00009 #include "mbed.h"
00010 #include <string>
00011  
00012 #include <ros.h>
00013 #include <std_msgs/Float32.h>
00014 #include <std_msgs/Float32MultiArray.h>
00015 #include <std_msgs/Empty.h>
00016 
00017 #include "defines.h"
00018 #include "utilities.h"
00019 #include "FreeFlyerHardware.h"
00020 #include "debug.h"
00021 
00022 extern "C" void mbed_reset();       //!< Makes the C++ function "mbed_reset" have 'C'-compiler linkage
00023 
00024 //#####################################################################################\\
00025 
00026 /*=============================================+
00027 |   GLOBALS
00028 +==============================================*/
00029 
00030 // ROS
00031 #if (ROS_SERIAL_MODE)
00032 ros::NodeHandle nh; // Comment out during serial tests
00033 #else
00034 Serial pc(USBTX, USBRX);
00035 #endif
00036 
00037 /*=============================================+
00038 |   ENUMERATIONS
00039 +==============================================*/
00040 namespace mbed {
00041     // LEDs
00042     DigitalOut LED_thrusters(LED1);
00043     DigitalOut LED_debugA(LED2);
00044     DigitalOut LED_debugB(LED3);
00045     DigitalOut LED_error(LED4);
00046     
00047     // Thrusters
00048     DigitalOut thruster1(PIN_THR1);
00049     DigitalOut thruster2(PIN_THR2);
00050     DigitalOut thruster3(PIN_THR3);
00051     DigitalOut thruster4(PIN_THR4);
00052     DigitalOut thruster5(PIN_THR5);
00053     DigitalOut thruster6(PIN_THR6);
00054     DigitalOut thruster7(PIN_THR7);
00055     DigitalOut thruster8(PIN_THR8);
00056     DigitalOut thruster_pinouts[] = {
00057         (thruster1), (thruster2),
00058         (thruster3), (thruster4), 
00059         (thruster5), (thruster6),
00060         (thruster7), (thruster8)};      //!< List of mBed thruster pinouts
00061     
00062     // Reaction Wheel Motors
00063     PwmOut motorPWM(PIN_WMPWM);                 //!< mBed pinout controlling momentum wheel motor H-Bridge input A
00064     DigitalOut motorA(PIN_WMA);
00065     DigitalOut motorB(PIN_WMB);
00066     
00067     I2C i2c(PIN_I2CSDA, PIN_I2CSCL);
00068     DigitalOut led_inv_out_en(PIN_LEDIOE);
00069     
00070     /*=============================================+
00071     |   COMMANDS
00072     +==============================================*/
00073     
00074     //! Relay motor command
00075     void commandMotor( float dutycycle ) {
00076         if (dutycycle >= 0.0) {
00077             mbed::motorPWM = dutycycle;
00078             mbed::motorA = 1;
00079             mbed::motorB = 0;
00080         } else {
00081             mbed::motorPWM = -dutycycle;
00082             mbed::motorA = 0;
00083             mbed::motorB = 1;
00084         }
00085     }
00086 
00087 }   // End "mbed" namespace
00088 
00089 /*=============================================+
00090 |   MAIN FUNCTION
00091 +==============================================*/
00092 int main() {
00093 
00094     mbed::i2c.frequency(10000); // set required i2c frequency
00095 
00096     /****************************************************
00097        Debug Scripts (Toggle Serial in debug_defines.h)
00098     *****************************************************/
00099     //testWheelEncoder(pc);               // Testing Wheel Encoder
00100     //searchForI2CDevices(pc, mbed::i2c); // I2C device searching script
00101     //testBitMasking(pc);
00102     //RGBLEDTesting(pc, mbed::i2c, led_inv_out_en); // RGB LED testing workspace
00103 
00104     /************
00105         Setup
00106     ************/
00107 #if (ROS_SERIAL_MODE)
00108     bool verbose = true;            // Toggle debug topic output
00109     Timer t_LED, t_RGB_LED, t_PID, t_thrust, t_meas, t_pid_debug;   // object to do timing to compute motor speed
00110     
00111     mbed::motorPWM.period(MOTOR_PWM_PERIOD);
00112     
00113     nh.getHardware()->setBaud(SERIAL_BAUD_RATE);
00114     nh.initNode();
00115     
00116     FreeFlyerHardware freeflyer(nh, &mbed::i2c, mbed::thruster_pinouts,
00117         &mbed::led_inv_out_en, verbose);
00118 
00119     t_LED.reset();
00120     t_RGB_LED.reset();
00121     t_PID.reset();
00122     t_thrust.reset();
00123     t_meas.reset();
00124     t_pid_debug.reset();
00125    
00126     t_LED.start();
00127     t_RGB_LED.start();
00128     t_PID.start();
00129     t_thrust.start();
00130     t_meas.start();
00131     t_pid_debug.start();
00132     
00133     #if (MODE == DEBUG_THRUSTERS)
00134     int thruster_ID_on = 0;
00135     int thruster_pinout_cmd[8];
00136     #elif (MODE == DEBUG_LED_CONST)
00137     freeflyer.rgba_led_->setColor(color::magenta);
00138     freeflyer.rgba_led_->setBrightness(1.0);
00139     #elif (MODE == DEBUG_LED_COLOR_SEQ)
00140     freeflyer.rgba_led_->setColor(color::blue);
00141     freeflyer.rgba_led_->setBrightness(1.0);
00142     freeflyer.rgba_led_->setColorProgram(color_program::fade1, color_time_program::fade1);
00143     #endif
00144     
00145     /***************
00146         Main Loop
00147     ****************/
00148     while (true) {
00149     
00150         // LED Blinking Loop
00151         if (t_LED.read_ms() >= LED_PERIOD) {    
00152             t_LED.reset();
00153             mbed::LED_error = !mbed::LED_error;
00154         }
00155         
00156         // PID Update Loop
00157         if (t_PID.read_ms() >= PID_PERIOD) {    
00158             t_PID.reset();
00159             
00160             #if (MODE == NORMAL)
00161             if (!freeflyer.duty_cycle_command_mode_)
00162                 freeflyer.updatePID();
00163             else
00164                 freeflyer.setPWMOut(freeflyer.duty_cycle_cmd_);
00165             #elif (MODE == DEBUG_WHEEL_CONST)
00166             mbed::LED_debugA = !mbed::LED_debugA;
00167             freeflyer.setPWMOut(DWC_PWM);
00168             #elif (MODE == DEBUG_WHEEL_PID)
00169             mbed::LED_debugA = !mbed::LED_debugA;
00170             freeflyer.setPIDSetpoint(DWP_SETPOINT);
00171             freeflyer.updatePID();
00172             #endif
00173             mbed::commandMotor(utils::min(freeflyer.getPWMOut(), PWM_LIMIT));
00174         }
00175             
00176         // PID parameter publishing loop
00177         if (t_pid_debug.read_ms() >= PID_DEBUG_PERIOD && nh.connected() && !freeflyer.duty_cycle_command_mode_) {
00178             t_pid_debug.reset();
00179             freeflyer.publishPIDParam();
00180         }
00181         
00182         // Thruster PWM Loop
00183         #if (MODE == NORMAL)
00184         if (t_thrust.read_ms() >= THRUST_PERIOD/THRUST_PWM_N) {
00185             t_thrust.reset();
00186             freeflyer.stepThrusterPWM();
00187         }
00188         #elif (MODE == DEBUG_THRUSTERS)
00189         if (t_thrust.read_ms() >= DT_PERIOD) {
00190             t_thrust.reset();
00191             mbed::LED_debugA = !mbed::LED_debugA;
00192             if (++thruster_ID_on == N_THRUSTERS) {
00193                 thruster_ID_on = 0;
00194                 mbed::LED_debugB = !mbed::LED_debugB;
00195             }
00196             
00197             for (int i = 0; i < N_THRUSTERS; i++)
00198                 if (i == thruster_ID_on)
00199                     thruster_pinout_cmd[i] = 1;
00200                 else
00201                     thruster_pinout_cmd[i] = 0;
00202             
00203             freeflyer.commandThrusters(thruster_pinout_cmd);
00204         }
00205         #endif
00206         
00207         // Measurement publishing loop
00208         if (t_meas.read_ms() >= MEAS_PERIOD && nh.connected()) {
00209             t_meas.reset();
00210             freeflyer.publishWheelMeas();
00211         }
00212         
00213         // RGB LED update loop
00214         if (t_RGB_LED.read_ms() >= freeflyer.rgba_led_->getStepTime()) {
00215             t_RGB_LED.reset();
00216             freeflyer.rgba_led_->step();
00217         }
00218 
00219         nh.spinOnce();
00220     }
00221     
00222 #endif
00223 }