File content as of revision 0:dd126a1080d3:

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

#include "mbed.h"
#include <string>

#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>

#include "defines.h"
#include "utilities.h"
#include "FreeFlyerHardware.h"

extern "C" void mbed_reset();       //!< Makes the C++ function "mbed_reset" have 'C'-compiler linkage

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

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

// ROS
ros::NodeHandle nh;

/*=============================================+
|   ENUMERATIONS
+==============================================*/
namespace mbed {
    // LEDs
    DigitalOut LED_thrusters(LED1);
    DigitalOut LED_motorA(LED2);
    DigitalOut LED_motorB(LED3);
    DigitalOut LED_error(LED4);
    
    // Thrusters
    DigitalOut thruster1(p12);
    DigitalOut thruster2(p11);
    DigitalOut thruster3(p8);
    DigitalOut thruster4(p7);
    DigitalOut thruster5(p6);
    DigitalOut thruster6(p5);
    DigitalOut thruster7(p30);
    DigitalOut thruster8(p29);
    DigitalOut thruster_pinouts[] = {
        (thruster1), (thruster2),
        (thruster3), (thruster4), 
        (thruster5), (thruster6),
        (thruster7), (thruster8)};      //!< List of mBed thruster pinouts
    
    // Reaction Wheel Motors
    PwmOut motorPWM(p21);                 //!< mBed pinout controlling momentum wheel motor H-Bridge input A
    DigitalOut motorA(p27);
    DigitalOut motorB(p28);

    /*=============================================+
    |   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() {    
    bool pid_led_flag = false;
    bool verbose = false;            // Toggle debug topic output
    Timer t_LED, t_PID, t_thrust, t_debug;      // object to do timing to compute motor speed
    
    mbed::motorPWM.period(MOTOR_PWM_PERIOD);
    
    nh.getHardware()->setBaud(DEFAULT_BAUD_RATE);
    nh.initNode();
    
    FreeFlyerHardware freeflyer(nh, mbed::thruster_pinouts);
    
    t_LED.reset();
    t_PID.reset();
    t_thrust.reset();
    t_debug.reset();
   
    t_LED.start();
    t_PID.start();
    t_thrust.start();
    t_debug.start();
    
    while (true) {
        
        // LED Blinking Loop
        if (t_LED.read_ms() >= LED_PERIOD) {
            
            t_LED.reset();
            if (pid_led_flag)
                mbed::LED_error = 0;
            else
                mbed::LED_error = 1;
            pid_led_flag = !pid_led_flag;
        }
        
        // PID Update Loop
        if (t_PID.read_ms() >= PID_PERIOD) {
            
            t_PID.reset();
            freeflyer.updatePID();
            mbed::commandMotor(0.0);
            //mbed::commandMotor(freeflyer.getVoltageOut()/VOLTAGE_MAX*PWM_LIMIT);
        }
        
        // Thruster PWM Loop
        if (t_thrust.read_ms() >= THRUST_PERIOD) {
            
            t_thrust.reset();
            freeflyer.stepThrusterPWM();
        }
        
        // Debug Loop
        if (verbose && t_debug.read_ms() >= DEBUG_PERIOD) {
            
            t_debug.reset();
            freeflyer.publishDebug();
        }
        
        nh.spinOnce();
        
    }
}

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