File content as of revision 5:864709d3eb76:

/*! \file main.cpp
\brief Main file for operating the ASL 3-DOF Spacecraft driver board
\author A. Bylard
*/

#include "mbed.h"
#include <string>
 
#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Empty.h>

#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

//#####################################################################################\\

/*=============================================+
|   GLOBALS
+==============================================*/

// ROS
#if (ROS_SERIAL_MODE)
ros::NodeHandle nh; // Comment out during serial tests
#else
Serial pc(USBTX, USBRX);
#endif

/*=============================================+
|   ENUMERATIONS
+==============================================*/
namespace mbed {
    // LEDs
    DigitalOut LED_thrusters(LED1);
    DigitalOut LED_debugA(LED2);
    DigitalOut LED_debugB(LED3);
    DigitalOut LED_error(LED4);
    
    // Thrusters
    DigitalOut thruster1(PIN_THR1);
    DigitalOut thruster2(PIN_THR2);
    DigitalOut thruster3(PIN_THR3);
    DigitalOut thruster4(PIN_THR4);
    DigitalOut thruster5(PIN_THR5);
    DigitalOut thruster6(PIN_THR6);
    DigitalOut thruster7(PIN_THR7);
    DigitalOut thruster8(PIN_THR8);
    DigitalOut thruster_pinouts[] = {
        (thruster1), (thruster2),
        (thruster3), (thruster4), 
        (thruster5), (thruster6),
        (thruster7), (thruster8)};      //!< List of mBed thruster pinouts
    
    // Reaction Wheel Motors
    PwmOut motorPWM(PIN_WMPWM);                 //!< mBed pinout controlling momentum wheel motor H-Bridge input A
    DigitalOut motorA(PIN_WMA);
    DigitalOut motorB(PIN_WMB);
    
    I2C i2c(PIN_I2CSDA, PIN_I2CSCL);
    DigitalOut led_inv_out_en(PIN_LEDIOE);
    
    /*=============================================+
    |   COMMANDS
    +==============================================*/
    
    //! Relay motor command
    void commandMotor( float dutycycle ) {
        if (dutycycle >= 0.0) {
            mbed::motorPWM = dutycycle;
            mbed::motorA = 1;
            mbed::motorB = 0;
        } else {
            mbed::motorPWM = -dutycycle;
            mbed::motorA = 0;
            mbed::motorB = 1;
        }
    }

}   // End "mbed" namespace

/*=============================================+
|   MAIN FUNCTION
+==============================================*/
int main() {

    mbed::i2c.frequency(10000); // set required i2c frequency

    /****************************************************
       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

    /************
        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
    
    mbed::motorPWM.period(MOTOR_PWM_PERIOD);
    
    nh.getHardware()->setBaud(SERIAL_BAUD_RATE);
    nh.initNode();
    
    FreeFlyerHardware freeflyer(nh, &mbed::i2c, mbed::thruster_pinouts,
        &mbed::led_inv_out_en, verbose);

    t_LED.reset();
    t_RGB_LED.reset();
    t_PID.reset();
    t_thrust.reset();
    t_meas.reset();
    t_pid_debug.reset();
   
    t_LED.start();
    t_RGB_LED.start();
    t_PID.start();
    t_thrust.start();
    t_meas.start();
    t_pid_debug.start();
    
    #if (MODE == DEBUG_THRUSTERS)
    int thruster_ID_on = 0;
    int thruster_pinout_cmd[8];
    #elif (MODE == DEBUG_LED_CONST)
    freeflyer.rgba_led_->setColor(color::magenta);
    freeflyer.rgba_led_->setBrightness(1.0);
    #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
    
    /***************
        Main Loop
    ****************/
    while (true) {
    
        // LED Blinking Loop
        if (t_LED.read_ms() >= LED_PERIOD) {    
            t_LED.reset();
            mbed::LED_error = !mbed::LED_error;
        }
        
        // PID Update Loop
        if (t_PID.read_ms() >= PID_PERIOD) {    
            t_PID.reset();
            
            #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)
            mbed::LED_debugA = !mbed::LED_debugA;
            freeflyer.setPIDSetpoint(DWP_SETPOINT);
            freeflyer.updatePID();
            #endif
            mbed::commandMotor(utils::min(freeflyer.getPWMOut(), PWM_LIMIT));
        }
            
        // PID parameter publishing loop
        if (t_pid_debug.read_ms() >= PID_DEBUG_PERIOD && nh.connected() && !freeflyer.duty_cycle_command_mode_) {
            t_pid_debug.reset();
            freeflyer.publishPIDParam();
        }
        
        // Thruster PWM Loop
        #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;
            if (++thruster_ID_on == N_THRUSTERS) {
                thruster_ID_on = 0;
                mbed::LED_debugB = !mbed::LED_debugB;
            }
            
            for (int i = 0; i < N_THRUSTERS; i++)
                if (i == thruster_ID_on)
                    thruster_pinout_cmd[i] = 1;
                else
                    thruster_pinout_cmd[i] = 0;
            
            freeflyer.commandThrusters(thruster_pinout_cmd);
        }
        #endif
        
        // Measurement publishing loop
        if (t_meas.read_ms() >= MEAS_PERIOD && nh.connected()) {
            t_meas.reset();
            freeflyer.publishWheelMeas();
        }
        
        // RGB LED update loop
        if (t_RGB_LED.read_ms() >= freeflyer.rgba_led_->getStepTime()) {
            t_RGB_LED.reset();
            freeflyer.rgba_led_->step();
        }

        nh.spinOnce();
    }
    
#endif
}