Diff: main.cpp

Revision:

1:40bdbe1a93b7

Parent:

0:dd126a1080d3

Child:

2:984eb2c5e8ee

--- a/main.cpp	Tue Feb 14 05:12:54 2017 +0000
+++ b/main.cpp	Fri Jun 22 02:09:50 2018 +0000
@@ -1,7 +1,7 @@
 
 /*! \file main.cpp
 \brief Main file for operating the ASL 3-DOF Spacecraft driver board
-\author A. Bylard, R. MacPherson
+\author A. Bylard
 */
 
 #include "mbed.h"
@@ -10,6 +10,7 @@
 #include <ros.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Empty.h>
 
 #include "defines.h"
 #include "utilities.h"
@@ -24,7 +25,7 @@
 +==============================================*/
 
 // ROS
-ros::NodeHandle nh;
+ros::NodeHandle nh; // Comment out during LED tests?
 
 /*=============================================+
 |   ENUMERATIONS
@@ -32,19 +33,19 @@
 namespace mbed {
     // LEDs
     DigitalOut LED_thrusters(LED1);
-    DigitalOut LED_motorA(LED2);
-    DigitalOut LED_motorB(LED3);
+    DigitalOut LED_debugA(LED2);
+    DigitalOut LED_debugB(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 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), 
@@ -52,10 +53,13 @@
         (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);
-
+    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
     +==============================================*/
@@ -77,71 +81,250 @@
 
 //#####################################################################################\\
 
+/*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() {    
-    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
+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(DEFAULT_BAUD_RATE);
+    nh.getHardware()->setBaud(SERIAL_BAUD_RATE);
     nh.initNode();
     
-    FreeFlyerHardware freeflyer(nh, mbed::thruster_pinouts);
+    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_debug.reset();
+    t_meas.reset();
+    t_pid_debug.reset();
    
     t_LED.start();
+    t_RGB_LED.start();
     t_PID.start();
     t_thrust.start();
-    t_debug.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) {
-            
+        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;
+            mbed::LED_error = !mbed::LED_error;
         }
         
         // PID Update Loop
-        if (t_PID.read_ms() >= PID_PERIOD) {
-            
+        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();
-            mbed::commandMotor(0.0);
-            //mbed::commandMotor(freeflyer.getVoltageOut()/VOLTAGE_MAX*PWM_LIMIT);
+#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 (t_thrust.read_ms() >= THRUST_PERIOD) {
+#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
         
-        // Debug Loop
-        if (verbose && t_debug.read_ms() >= DEBUG_PERIOD) {
-            
-            t_debug.reset();
-            freeflyer.publishDebug();
+        // 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();
-        
     }
-}
 
-//#####################################################################################\\
\ No newline at end of file
+}
\ No newline at end of file