Thomas Lew / Mbed 2 deprecated FreeFlyerROS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_kinetic

main.cpp@5:864709d3eb76, 2018-06-29 (annotated)

Committer:
ambyld
Date:
Fri Jun 29 17:49:40 2018 +0000
Revision:
5:864709d3eb76
Parent:
4:cae255669971 
Moved isolated debug scripts to separate header file

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Knillinux 0:dd126a1080d3 1
Knillinux 0:dd126a1080d3 2 /*! \file main.cpp
Knillinux 0:dd126a1080d3 3 \brief Main file for operating the ASL 3-DOF Spacecraft driver board
Knillinux 1:40bdbe1a93b7 4 \author A. Bylard
Knillinux 0:dd126a1080d3 5 */
Knillinux 0:dd126a1080d3 6
Knillinux 0:dd126a1080d3 7 #include "mbed.h"
Knillinux 0:dd126a1080d3 8 #include <string>
ambyld 3:402a6464f025 9
Knillinux 0:dd126a1080d3 10 #include <ros.h>
Knillinux 0:dd126a1080d3 11 #include <std_msgs/Float32.h>
Knillinux 0:dd126a1080d3 12 #include <std_msgs/Float32MultiArray.h>
Knillinux 1:40bdbe1a93b7 13 #include <std_msgs/Empty.h>
Knillinux 0:dd126a1080d3 14
Knillinux 0:dd126a1080d3 15 #include "defines.h"
Knillinux 0:dd126a1080d3 16 #include "utilities.h"
Knillinux 0:dd126a1080d3 17 #include "FreeFlyerHardware.h"
ambyld 5:864709d3eb76 18 #include "debug.h"
Knillinux 0:dd126a1080d3 19
Knillinux 0:dd126a1080d3 20 extern "C" void mbed_reset(); //!< Makes the C++ function "mbed_reset" have 'C'-compiler linkage
Knillinux 0:dd126a1080d3 21
Knillinux 0:dd126a1080d3 22 //#####################################################################################\\
Knillinux 0:dd126a1080d3 23
Knillinux 0:dd126a1080d3 24 /*=============================================+
Knillinux 0:dd126a1080d3 25 | GLOBALS
Knillinux 0:dd126a1080d3 26 +==============================================*/
Knillinux 0:dd126a1080d3 27
Knillinux 0:dd126a1080d3 28 // ROS
ambyld 5:864709d3eb76 29 #if (ROS_SERIAL_MODE)
ambyld 4:cae255669971 30 ros::NodeHandle nh; // Comment out during serial tests
ambyld 5:864709d3eb76 31 #else
ambyld 5:864709d3eb76 32 Serial pc(USBTX, USBRX);
ambyld 5:864709d3eb76 33 #endif
Knillinux 0:dd126a1080d3 34
Knillinux 0:dd126a1080d3 35 /*=============================================+
Knillinux 0:dd126a1080d3 36 | ENUMERATIONS
Knillinux 0:dd126a1080d3 37 +==============================================*/
Knillinux 0:dd126a1080d3 38 namespace mbed {
Knillinux 0:dd126a1080d3 39 // LEDs
Knillinux 0:dd126a1080d3 40 DigitalOut LED_thrusters(LED1);
Knillinux 1:40bdbe1a93b7 41 DigitalOut LED_debugA(LED2);
Knillinux 1:40bdbe1a93b7 42 DigitalOut LED_debugB(LED3);
Knillinux 0:dd126a1080d3 43 DigitalOut LED_error(LED4);
Knillinux 0:dd126a1080d3 44
Knillinux 0:dd126a1080d3 45 // Thrusters
Knillinux 1:40bdbe1a93b7 46 DigitalOut thruster1(PIN_THR1);
Knillinux 1:40bdbe1a93b7 47 DigitalOut thruster2(PIN_THR2);
Knillinux 1:40bdbe1a93b7 48 DigitalOut thruster3(PIN_THR3);
Knillinux 1:40bdbe1a93b7 49 DigitalOut thruster4(PIN_THR4);
Knillinux 1:40bdbe1a93b7 50 DigitalOut thruster5(PIN_THR5);
Knillinux 1:40bdbe1a93b7 51 DigitalOut thruster6(PIN_THR6);
Knillinux 1:40bdbe1a93b7 52 DigitalOut thruster7(PIN_THR7);
Knillinux 1:40bdbe1a93b7 53 DigitalOut thruster8(PIN_THR8);
Knillinux 0:dd126a1080d3 54 DigitalOut thruster_pinouts[] = {
Knillinux 0:dd126a1080d3 55 (thruster1), (thruster2),
Knillinux 0:dd126a1080d3 56 (thruster3), (thruster4),
Knillinux 0:dd126a1080d3 57 (thruster5), (thruster6),
Knillinux 0:dd126a1080d3 58 (thruster7), (thruster8)}; //!< List of mBed thruster pinouts
Knillinux 0:dd126a1080d3 59
Knillinux 0:dd126a1080d3 60 // Reaction Wheel Motors
Knillinux 1:40bdbe1a93b7 61 PwmOut motorPWM(PIN_WMPWM); //!< mBed pinout controlling momentum wheel motor H-Bridge input A
Knillinux 1:40bdbe1a93b7 62 DigitalOut motorA(PIN_WMA);
Knillinux 1:40bdbe1a93b7 63 DigitalOut motorB(PIN_WMB);
Knillinux 1:40bdbe1a93b7 64
Knillinux 1:40bdbe1a93b7 65 I2C i2c(PIN_I2CSDA, PIN_I2CSCL);
Knillinux 1:40bdbe1a93b7 66 DigitalOut led_inv_out_en(PIN_LEDIOE);
Knillinux 1:40bdbe1a93b7 67
Knillinux 0:dd126a1080d3 68 /*=============================================+
Knillinux 0:dd126a1080d3 69 | COMMANDS
Knillinux 0:dd126a1080d3 70 +==============================================*/
Knillinux 0:dd126a1080d3 71
Knillinux 0:dd126a1080d3 72 //! Relay motor command
Knillinux 0:dd126a1080d3 73 void commandMotor( float dutycycle ) {
Knillinux 0:dd126a1080d3 74 if (dutycycle >= 0.0) {
Knillinux 0:dd126a1080d3 75 mbed::motorPWM = dutycycle;
Knillinux 0:dd126a1080d3 76 mbed::motorA = 1;
Knillinux 0:dd126a1080d3 77 mbed::motorB = 0;
Knillinux 0:dd126a1080d3 78 } else {
Knillinux 0:dd126a1080d3 79 mbed::motorPWM = -dutycycle;
Knillinux 0:dd126a1080d3 80 mbed::motorA = 0;
Knillinux 0:dd126a1080d3 81 mbed::motorB = 1;
Knillinux 0:dd126a1080d3 82 }
Knillinux 0:dd126a1080d3 83 }
Knillinux 0:dd126a1080d3 84
Knillinux 0:dd126a1080d3 85 } // End "mbed" namespace
Knillinux 0:dd126a1080d3 86
Knillinux 0:dd126a1080d3 87 /*=============================================+
Knillinux 0:dd126a1080d3 88 | MAIN FUNCTION
Knillinux 0:dd126a1080d3 89 +==============================================*/
Knillinux 1:40bdbe1a93b7 90 int main() {
Knillinux 1:40bdbe1a93b7 91
ambyld 5:864709d3eb76 92 mbed::i2c.frequency(10000); // set required i2c frequency
Knillinux 1:40bdbe1a93b7 93
ambyld 5:864709d3eb76 94 /****************************************************
ambyld 5:864709d3eb76 95 Debug Scripts (Toggle Serial in debug_defines.h)
ambyld 5:864709d3eb76 96 *****************************************************/
ambyld 5:864709d3eb76 97 //testWheelEncoder(pc); // Testing Wheel Encoder
ambyld 5:864709d3eb76 98 //searchForI2CDevices(pc, mbed::i2c); // I2C device searching script
ambyld 5:864709d3eb76 99 //testBitMasking(pc);
ambyld 5:864709d3eb76 100 //RGBLEDTesting(pc, mbed::i2c, led_inv_out_en); // RGB LED testing workspace
Knillinux 1:40bdbe1a93b7 101
ambyld 5:864709d3eb76 102 /************
ambyld 5:864709d3eb76 103 Setup
ambyld 5:864709d3eb76 104 ************/
ambyld 5:864709d3eb76 105 #if (ROS_SERIAL_MODE)
Knillinux 1:40bdbe1a93b7 106 bool verbose = true; // Toggle debug topic output
Knillinux 1:40bdbe1a93b7 107 Timer t_LED, t_RGB_LED, t_PID, t_thrust, t_meas, t_pid_debug; // object to do timing to compute motor speed
Knillinux 0:dd126a1080d3 108
Knillinux 0:dd126a1080d3 109 mbed::motorPWM.period(MOTOR_PWM_PERIOD);
Knillinux 0:dd126a1080d3 110
Knillinux 1:40bdbe1a93b7 111 nh.getHardware()->setBaud(SERIAL_BAUD_RATE);
Knillinux 0:dd126a1080d3 112 nh.initNode();
Knillinux 0:dd126a1080d3 113
Knillinux 1:40bdbe1a93b7 114 FreeFlyerHardware freeflyer(nh, &mbed::i2c, mbed::thruster_pinouts,
Knillinux 1:40bdbe1a93b7 115 &mbed::led_inv_out_en, verbose);
ambyld 4:cae255669971 116
Knillinux 0:dd126a1080d3 117 t_LED.reset();
Knillinux 1:40bdbe1a93b7 118 t_RGB_LED.reset();
Knillinux 0:dd126a1080d3 119 t_PID.reset();
Knillinux 0:dd126a1080d3 120 t_thrust.reset();
Knillinux 1:40bdbe1a93b7 121 t_meas.reset();
Knillinux 1:40bdbe1a93b7 122 t_pid_debug.reset();
Knillinux 0:dd126a1080d3 123
Knillinux 0:dd126a1080d3 124 t_LED.start();
Knillinux 1:40bdbe1a93b7 125 t_RGB_LED.start();
Knillinux 0:dd126a1080d3 126 t_PID.start();
Knillinux 0:dd126a1080d3 127 t_thrust.start();
Knillinux 1:40bdbe1a93b7 128 t_meas.start();
Knillinux 1:40bdbe1a93b7 129 t_pid_debug.start();
Knillinux 1:40bdbe1a93b7 130
ambyld 5:864709d3eb76 131 #if (MODE == DEBUG_THRUSTERS)
Knillinux 1:40bdbe1a93b7 132 int thruster_ID_on = 0;
Knillinux 1:40bdbe1a93b7 133 int thruster_pinout_cmd[8];
ambyld 5:864709d3eb76 134 #elif (MODE == DEBUG_LED_CONST)
Knillinux 1:40bdbe1a93b7 135 freeflyer.rgba_led_->setColor(color::magenta);
Knillinux 1:40bdbe1a93b7 136 freeflyer.rgba_led_->setBrightness(1.0);
ambyld 5:864709d3eb76 137 #elif (MODE == DEBUG_LED_COLOR_SEQ)
Knillinux 1:40bdbe1a93b7 138 freeflyer.rgba_led_->setColor(color::blue);
Knillinux 1:40bdbe1a93b7 139 freeflyer.rgba_led_->setBrightness(1.0);
Knillinux 1:40bdbe1a93b7 140 freeflyer.rgba_led_->setColorProgram(color_program::fade1, color_time_program::fade1);
ambyld 5:864709d3eb76 141 #endif
Knillinux 0:dd126a1080d3 142
ambyld 5:864709d3eb76 143 /***************
ambyld 5:864709d3eb76 144 Main Loop
ambyld 5:864709d3eb76 145 ****************/
Knillinux 0:dd126a1080d3 146 while (true) {
Knillinux 1:40bdbe1a93b7 147
Knillinux 0:dd126a1080d3 148 // LED Blinking Loop
Knillinux 1:40bdbe1a93b7 149 if (t_LED.read_ms() >= LED_PERIOD) {
Knillinux 0:dd126a1080d3 150 t_LED.reset();
Knillinux 1:40bdbe1a93b7 151 mbed::LED_error = !mbed::LED_error;
Knillinux 0:dd126a1080d3 152 }
Knillinux 0:dd126a1080d3 153
Knillinux 0:dd126a1080d3 154 // PID Update Loop
Knillinux 1:40bdbe1a93b7 155 if (t_PID.read_ms() >= PID_PERIOD) {
Knillinux 0:dd126a1080d3 156 t_PID.reset();
ambyld 5:864709d3eb76 157
ambyld 5:864709d3eb76 158 #if (MODE == NORMAL)
ambyld 5:864709d3eb76 159 if (!freeflyer.duty_cycle_command_mode_)
ambyld 5:864709d3eb76 160 freeflyer.updatePID();
ambyld 5:864709d3eb76 161 else
ambyld 5:864709d3eb76 162 freeflyer.setPWMOut(freeflyer.duty_cycle_cmd_);
ambyld 5:864709d3eb76 163 #elif (MODE == DEBUG_WHEEL_CONST)
Knillinux 1:40bdbe1a93b7 164 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 165 freeflyer.setPWMOut(DWC_PWM);
ambyld 5:864709d3eb76 166 #elif (MODE == DEBUG_WHEEL_PID)
Knillinux 1:40bdbe1a93b7 167 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 168 freeflyer.setPIDSetpoint(DWP_SETPOINT);
Knillinux 0:dd126a1080d3 169 freeflyer.updatePID();
ambyld 5:864709d3eb76 170 #endif
Knillinux 1:40bdbe1a93b7 171 mbed::commandMotor(utils::min(freeflyer.getPWMOut(), PWM_LIMIT));
Knillinux 1:40bdbe1a93b7 172 }
Knillinux 1:40bdbe1a93b7 173
Knillinux 1:40bdbe1a93b7 174 // PID parameter publishing loop
Knillinux 1:40bdbe1a93b7 175 if (t_pid_debug.read_ms() >= PID_DEBUG_PERIOD && nh.connected() && !freeflyer.duty_cycle_command_mode_) {
Knillinux 1:40bdbe1a93b7 176 t_pid_debug.reset();
Knillinux 1:40bdbe1a93b7 177 freeflyer.publishPIDParam();
Knillinux 0:dd126a1080d3 178 }
Knillinux 0:dd126a1080d3 179
Knillinux 0:dd126a1080d3 180 // Thruster PWM Loop
ambyld 5:864709d3eb76 181 #if (MODE == NORMAL)
ambyld 5:864709d3eb76 182 if (t_thrust.read_ms() >= THRUST_PERIOD/THRUST_PWM_N) {
ambyld 5:864709d3eb76 183 t_thrust.reset();
ambyld 5:864709d3eb76 184 freeflyer.stepThrusterPWM();
ambyld 5:864709d3eb76 185 }
ambyld 5:864709d3eb76 186 #elif (MODE == DEBUG_THRUSTERS)
Knillinux 1:40bdbe1a93b7 187 if (t_thrust.read_ms() >= DT_PERIOD) {
Knillinux 1:40bdbe1a93b7 188 t_thrust.reset();
Knillinux 1:40bdbe1a93b7 189 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 190 if (++thruster_ID_on == N_THRUSTERS) {
Knillinux 1:40bdbe1a93b7 191 thruster_ID_on = 0;
Knillinux 1:40bdbe1a93b7 192 mbed::LED_debugB = !mbed::LED_debugB;
Knillinux 1:40bdbe1a93b7 193 }
Knillinux 0:dd126a1080d3 194
Knillinux 1:40bdbe1a93b7 195 for (int i = 0; i < N_THRUSTERS; i++)
Knillinux 1:40bdbe1a93b7 196 if (i == thruster_ID_on)
Knillinux 1:40bdbe1a93b7 197 thruster_pinout_cmd[i] = 1;
Knillinux 1:40bdbe1a93b7 198 else
Knillinux 1:40bdbe1a93b7 199 thruster_pinout_cmd[i] = 0;
Knillinux 1:40bdbe1a93b7 200
Knillinux 1:40bdbe1a93b7 201 freeflyer.commandThrusters(thruster_pinout_cmd);
Knillinux 1:40bdbe1a93b7 202 }
ambyld 5:864709d3eb76 203 #endif
Knillinux 0:dd126a1080d3 204
Knillinux 1:40bdbe1a93b7 205 // Measurement publishing loop
Knillinux 1:40bdbe1a93b7 206 if (t_meas.read_ms() >= MEAS_PERIOD && nh.connected()) {
Knillinux 1:40bdbe1a93b7 207 t_meas.reset();
Knillinux 1:40bdbe1a93b7 208 freeflyer.publishWheelMeas();
Knillinux 0:dd126a1080d3 209 }
Knillinux 0:dd126a1080d3 210
Knillinux 1:40bdbe1a93b7 211 // RGB LED update loop
Knillinux 1:40bdbe1a93b7 212 if (t_RGB_LED.read_ms() >= freeflyer.rgba_led_->getStepTime()) {
Knillinux 1:40bdbe1a93b7 213 t_RGB_LED.reset();
Knillinux 1:40bdbe1a93b7 214 freeflyer.rgba_led_->step();
Knillinux 1:40bdbe1a93b7 215 }
Knillinux 1:40bdbe1a93b7 216
Knillinux 0:dd126a1080d3 217 nh.spinOnce();
Knillinux 0:dd126a1080d3 218 }
ambyld 5:864709d3eb76 219
ambyld 5:864709d3eb76 220 #endif
Knillinux 1:40bdbe1a93b7 221 }