File content as of revision 1:40bdbe1a93b7:

/*! \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"

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

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

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

// ROS
ros::NodeHandle nh; // Comment out during LED tests?

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

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

/*Serial pc(USBTX, USBRX);
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
////////
// 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
=======*/

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

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
*******************************/
    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
    
    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 == 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();
#else
            if (!freeflyer.duty_cycle_command_mode_) {
                freeflyer.updatePID();
            } else {
                freeflyer.setPWMOut(freeflyer.duty_cycle_cmd_);
            }
#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 == 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);
        }
#else
        if (t_thrust.read_ms() >= THRUST_PERIOD/THRUST_PWM_N) {
            t_thrust.reset();
            freeflyer.stepThrusterPWM();
        }
#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();
    }

}