Diff: main.cpp

Revision:

0:dd126a1080d3

Child:

1:40bdbe1a93b7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 14 05:12:54 2017 +0000
@@ -0,0 +1,147 @@
+
+/*! \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();
+        
+    }
+}
+
+//#####################################################################################\\
\ No newline at end of file