Clara Keng / Mbed 2 deprecated FreeFlyerROS_clarakhl

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@7:e165f5119950, 2019-10-14 (annotated)

Committer:
clarakeng
Date:
Mon Oct 14 23:53:05 2019 +0000
Revision:
7:e165f5119950
Parent:
5:864709d3eb76 
Test comment

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