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