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@2:984eb2c5e8ee, 2018-06-22 (annotated)

Committer:
Knillinux
Date:
Fri Jun 22 03:13:40 2018 +0000
Revision:
2:984eb2c5e8ee
Parent:
1:40bdbe1a93b7
Child:
3:402a6464f025
Updates

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>
Knillinux 0:dd126a1080d3 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"
Knillinux 0:dd126a1080d3 18
Knillinux 0:dd126a1080d3 19 extern "C" void mbed_reset(); //!< Makes the C++ function "mbed_reset" have 'C'-compiler linkage
Knillinux 0:dd126a1080d3 20
Knillinux 0:dd126a1080d3 21 //#####################################################################################\\
Knillinux 0:dd126a1080d3 22
Knillinux 0:dd126a1080d3 23 /*=============================================+
Knillinux 0:dd126a1080d3 24 | GLOBALS
Knillinux 0:dd126a1080d3 25 +==============================================*/
Knillinux 0:dd126a1080d3 26
Knillinux 0:dd126a1080d3 27 // ROS
Knillinux 1:40bdbe1a93b7 28 ros::NodeHandle nh; // Comment out during LED tests?
Knillinux 0:dd126a1080d3 29
Knillinux 0:dd126a1080d3 30 /*=============================================+
Knillinux 0:dd126a1080d3 31 | ENUMERATIONS
Knillinux 0:dd126a1080d3 32 +==============================================*/
Knillinux 0:dd126a1080d3 33 namespace mbed {
Knillinux 0:dd126a1080d3 34 // LEDs
Knillinux 0:dd126a1080d3 35 DigitalOut LED_thrusters(LED1);
Knillinux 1:40bdbe1a93b7 36 DigitalOut LED_debugA(LED2);
Knillinux 1:40bdbe1a93b7 37 DigitalOut LED_debugB(LED3);
Knillinux 0:dd126a1080d3 38 DigitalOut LED_error(LED4);
Knillinux 0:dd126a1080d3 39
Knillinux 0:dd126a1080d3 40 // Thrusters
Knillinux 1:40bdbe1a93b7 41 DigitalOut thruster1(PIN_THR1);
Knillinux 1:40bdbe1a93b7 42 DigitalOut thruster2(PIN_THR2);
Knillinux 1:40bdbe1a93b7 43 DigitalOut thruster3(PIN_THR3);
Knillinux 1:40bdbe1a93b7 44 DigitalOut thruster4(PIN_THR4);
Knillinux 1:40bdbe1a93b7 45 DigitalOut thruster5(PIN_THR5);
Knillinux 1:40bdbe1a93b7 46 DigitalOut thruster6(PIN_THR6);
Knillinux 1:40bdbe1a93b7 47 DigitalOut thruster7(PIN_THR7);
Knillinux 1:40bdbe1a93b7 48 DigitalOut thruster8(PIN_THR8);
Knillinux 0:dd126a1080d3 49 DigitalOut thruster_pinouts[] = {
Knillinux 0:dd126a1080d3 50 (thruster1), (thruster2),
Knillinux 0:dd126a1080d3 51 (thruster3), (thruster4),
Knillinux 0:dd126a1080d3 52 (thruster5), (thruster6),
Knillinux 0:dd126a1080d3 53 (thruster7), (thruster8)}; //!< List of mBed thruster pinouts
Knillinux 0:dd126a1080d3 54
Knillinux 0:dd126a1080d3 55 // Reaction Wheel Motors
Knillinux 1:40bdbe1a93b7 56 PwmOut motorPWM(PIN_WMPWM); //!< mBed pinout controlling momentum wheel motor H-Bridge input A
Knillinux 1:40bdbe1a93b7 57 DigitalOut motorA(PIN_WMA);
Knillinux 1:40bdbe1a93b7 58 DigitalOut motorB(PIN_WMB);
Knillinux 1:40bdbe1a93b7 59
Knillinux 1:40bdbe1a93b7 60 I2C i2c(PIN_I2CSDA, PIN_I2CSCL);
Knillinux 1:40bdbe1a93b7 61 DigitalOut led_inv_out_en(PIN_LEDIOE);
Knillinux 1:40bdbe1a93b7 62
Knillinux 0:dd126a1080d3 63 /*=============================================+
Knillinux 0:dd126a1080d3 64 | COMMANDS
Knillinux 0:dd126a1080d3 65 +==============================================*/
Knillinux 0:dd126a1080d3 66
Knillinux 0:dd126a1080d3 67 //! Relay motor command
Knillinux 0:dd126a1080d3 68 void commandMotor( float dutycycle ) {
Knillinux 0:dd126a1080d3 69 if (dutycycle >= 0.0) {
Knillinux 0:dd126a1080d3 70 mbed::motorPWM = dutycycle;
Knillinux 0:dd126a1080d3 71 mbed::motorA = 1;
Knillinux 0:dd126a1080d3 72 mbed::motorB = 0;
Knillinux 0:dd126a1080d3 73 } else {
Knillinux 0:dd126a1080d3 74 mbed::motorPWM = -dutycycle;
Knillinux 0:dd126a1080d3 75 mbed::motorA = 0;
Knillinux 0:dd126a1080d3 76 mbed::motorB = 1;
Knillinux 0:dd126a1080d3 77 }
Knillinux 0:dd126a1080d3 78 }
Knillinux 0:dd126a1080d3 79
Knillinux 0:dd126a1080d3 80 } // End "mbed" namespace
Knillinux 0:dd126a1080d3 81
Knillinux 0:dd126a1080d3 82 //#####################################################################################\\
Knillinux 0:dd126a1080d3 83
Knillinux 1:40bdbe1a93b7 84 /*Serial pc(USBTX, USBRX);
Knillinux 1:40bdbe1a93b7 85 void printBits(char myByte){
Knillinux 1:40bdbe1a93b7 86 for (char mask = 0x80; mask; mask >>= 1){
Knillinux 1:40bdbe1a93b7 87 if(mask & myByte)
Knillinux 1:40bdbe1a93b7 88 pc.putc('1');
Knillinux 1:40bdbe1a93b7 89 else
Knillinux 1:40bdbe1a93b7 90 pc.putc('0');
Knillinux 1:40bdbe1a93b7 91 }
Knillinux 1:40bdbe1a93b7 92 }*/
Knillinux 1:40bdbe1a93b7 93
Knillinux 1:40bdbe1a93b7 94 /*
Knillinux 1:40bdbe1a93b7 95 ////////
Knillinux 1:40bdbe1a93b7 96 // Blinky test code
Knillinux 1:40bdbe1a93b7 97 void messageCb(const std_msgs::Empty& toggle_msg){
Knillinux 1:40bdbe1a93b7 98 LED_debugA = !LED_debugA; // blink the led
Knillinux 1:40bdbe1a93b7 99 }
Knillinux 1:40bdbe1a93b7 100
Knillinux 1:40bdbe1a93b7 101 ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb);
Knillinux 1:40bdbe1a93b7 102 */
Knillinux 1:40bdbe1a93b7 103
Knillinux 0:dd126a1080d3 104 /*=============================================+
Knillinux 0:dd126a1080d3 105 | MAIN FUNCTION
Knillinux 0:dd126a1080d3 106 +==============================================*/
Knillinux 0:dd126a1080d3 107
Knillinux 1:40bdbe1a93b7 108 int main() {
Knillinux 1:40bdbe1a93b7 109
Knillinux 1:40bdbe1a93b7 110 mbed::i2c.frequency(10000); // set required i2c frequency
Knillinux 1:40bdbe1a93b7 111 ////////
Knillinux 1:40bdbe1a93b7 112 // Blinky test code
Knillinux 1:40bdbe1a93b7 113 /* nh.getHardware()->setBaud(115200);
Knillinux 1:40bdbe1a93b7 114 nh.initNode();
Knillinux 1:40bdbe1a93b7 115 nh.subscribe(sub);
Knillinux 1:40bdbe1a93b7 116
Knillinux 1:40bdbe1a93b7 117 while (1) {
Knillinux 1:40bdbe1a93b7 118 nh.spinOnce();
Knillinux 1:40bdbe1a93b7 119 wait_ms(1);
Knillinux 1:40bdbe1a93b7 120 }
Knillinux 1:40bdbe1a93b7 121 */
Knillinux 1:40bdbe1a93b7 122 /*
Knillinux 1:40bdbe1a93b7 123 // I2C device searching script
Knillinux 1:40bdbe1a93b7 124 //mbed::i2c.frequency(10000); // set required i2c frequency
Knillinux 1:40bdbe1a93b7 125
Knillinux 1:40bdbe1a93b7 126 pc.printf("I2C Searching!\n\n");
Knillinux 1:40bdbe1a93b7 127 pc.printf("Starting....\n\n");
Knillinux 1:40bdbe1a93b7 128 while (1) {
Knillinux 1:40bdbe1a93b7 129 int count = 0;
Knillinux 1:40bdbe1a93b7 130 for (int address=0; address<256; address+=2) {
Knillinux 1:40bdbe1a93b7 131 if (!mbed::i2c.write(address, NULL, 0)) { // 0 returned is ok
Knillinux 1:40bdbe1a93b7 132 pc.printf("I2C address 0x%02X\n", address);
Knillinux 1:40bdbe1a93b7 133 count++;
Knillinux 1:40bdbe1a93b7 134 }
Knillinux 1:40bdbe1a93b7 135 }
Knillinux 1:40bdbe1a93b7 136 pc.printf("%d devices found\n\n\n", count);
Knillinux 1:40bdbe1a93b7 137 wait(0.000001);
Knillinux 1:40bdbe1a93b7 138 }
Knillinux 1:40bdbe1a93b7 139
Knillinux 1:40bdbe1a93b7 140 //mbed::pc.printf("Hello World!\n");
Knillinux 1:40bdbe1a93b7 141
Knillinux 1:40bdbe1a93b7 142 */
Knillinux 1:40bdbe1a93b7 143
Knillinux 1:40bdbe1a93b7 144 /*======
Knillinux 1:40bdbe1a93b7 145 // Testing bit masking
Knillinux 1:40bdbe1a93b7 146 =======*/
Knillinux 1:40bdbe1a93b7 147
Knillinux 1:40bdbe1a93b7 148 /*char cmd;
Knillinux 1:40bdbe1a93b7 149 while (true) {
Knillinux 1:40bdbe1a93b7 150 cmd = 0x00;
Knillinux 1:40bdbe1a93b7 151 cmd |= (1UL << (0x02-2));
Knillinux 1:40bdbe1a93b7 152 printBits(cmd);
Knillinux 1:40bdbe1a93b7 153 pc.printf("\n");
Knillinux 1:40bdbe1a93b7 154 wait(1.0);
Knillinux 1:40bdbe1a93b7 155 }*/
Knillinux 1:40bdbe1a93b7 156
Knillinux 1:40bdbe1a93b7 157 /******************************************
Knillinux 1:40bdbe1a93b7 158 RGB LED testing code, FF Driver v2.0
Knillinux 1:40bdbe1a93b7 159 ******************************************/
Knillinux 1:40bdbe1a93b7 160 /*char cmd[19];
Knillinux 1:40bdbe1a93b7 161 led_inv_out_en = 0;
Knillinux 1:40bdbe1a93b7 162
Knillinux 1:40bdbe1a93b7 163 while (true) {
Knillinux 1:40bdbe1a93b7 164
Knillinux 1:40bdbe1a93b7 165 wait(0.5);
Knillinux 1:40bdbe1a93b7 166
Knillinux 1:40bdbe1a93b7 167 cmd[0] = 0x80; // Send control register: start with register 00, autoincrement
Knillinux 1:40bdbe1a93b7 168 cmd[1] = 0x80; // MODE1 (default value, in normal power mode)
Knillinux 1:40bdbe1a93b7 169 cmd[2] = 0x09; // MODE2 (default value, but with invert off, outputs change on ack, open-drain)
Knillinux 1:40bdbe1a93b7 170 cmd[3] = 0x00; // PWM0 (B2)
Knillinux 1:40bdbe1a93b7 171 cmd[4] = 0x00; // PWM1 (W2 - ignore)
Knillinux 1:40bdbe1a93b7 172 cmd[5] = 0x00; // PWM2 (G2)
Knillinux 1:40bdbe1a93b7 173 cmd[6] = 0x00; // PWM3 (R2)
Knillinux 1:40bdbe1a93b7 174 cmd[7] = 0x00; // PWM4 (B1)
Knillinux 1:40bdbe1a93b7 175 cmd[8] = 0x00; // PWM5 (W1 - ignore)
Knillinux 1:40bdbe1a93b7 176 cmd[9] = 0x00; // PWM6 (G1)
Knillinux 1:40bdbe1a93b7 177 cmd[10] = 0x00; // PWM7 (R1)
Knillinux 1:40bdbe1a93b7 178 cmd[11] = 0xff; // GRPPWM (default value)
Knillinux 1:40bdbe1a93b7 179 cmd[12] = 0x00; // GRPFREQ (default value)
Knillinux 1:40bdbe1a93b7 180 cmd[13] = 0x55; // LEDOUT0 (0x55: all fully on)
Knillinux 1:40bdbe1a93b7 181 cmd[14] = 0x55; // LEDOUT1 (0x55: all fully on)
Knillinux 1:40bdbe1a93b7 182 cmd[13] = 0x00; // LEDOUT0 (0x55: all fully off)
Knillinux 1:40bdbe1a93b7 183 cmd[14] = 0x00; // LEDOUT1 (0x55: all fully off)
Knillinux 1:40bdbe1a93b7 184 cmd[15] = 0x00; // SUBADR1
Knillinux 1:40bdbe1a93b7 185 cmd[16] = 0x00; // SUBADR2
Knillinux 1:40bdbe1a93b7 186 cmd[17] = 0x00; // SUBADR3
Knillinux 1:40bdbe1a93b7 187 cmd[18] = 0x00; // ALLCALLADR
Knillinux 1:40bdbe1a93b7 188
Knillinux 1:40bdbe1a93b7 189 mbed::i2c.write(ADDR_RGB, cmd, 19);
Knillinux 1:40bdbe1a93b7 190
Knillinux 1:40bdbe1a93b7 191 //cmd[0] = 0x8d; // Send control register: start with register 12, autoincrement
Knillinux 1:40bdbe1a93b7 192 //cmd[1] = 0x55; // LEDOUT0 (0x55: all fully on)
Knillinux 1:40bdbe1a93b7 193 //cmd[2] = 0x55; // LEDOUT1 (0x55: all fully on)
Knillinux 1:40bdbe1a93b7 194 //cmd[1] = 0x00; // LEDOUT0 (0x55: all fully off)
Knillinux 1:40bdbe1a93b7 195 //cmd[2] = 0x00; // LEDOUT1 (0x55: all fully off)
Knillinux 1:40bdbe1a93b7 196
Knillinux 1:40bdbe1a93b7 197 //mbed::i2c.write(ADDR_RGB, cmd, 3);
Knillinux 1:40bdbe1a93b7 198 //mbed::i2c.write(mbed::addr, cmd, 3);
Knillinux 1:40bdbe1a93b7 199
Knillinux 1:40bdbe1a93b7 200 cmd[0] = 0x80;
Knillinux 1:40bdbe1a93b7 201 mbed::i2c.write(ADDR_RGB, cmd, 1);
Knillinux 1:40bdbe1a93b7 202 for (int i = 0; i < 18; i++)
Knillinux 1:40bdbe1a93b7 203 cmd[i] = 0;
Knillinux 1:40bdbe1a93b7 204
Knillinux 1:40bdbe1a93b7 205 mbed::i2c.read(ADDR_RGB, cmd, 18);
Knillinux 1:40bdbe1a93b7 206
Knillinux 1:40bdbe1a93b7 207 for (int i = 0; i < 18; i++) {
Knillinux 1:40bdbe1a93b7 208 printBits(cmd[i]);
Knillinux 1:40bdbe1a93b7 209 pc.printf("\n");
Knillinux 1:40bdbe1a93b7 210 }
Knillinux 1:40bdbe1a93b7 211 pc.printf("\n");
Knillinux 1:40bdbe1a93b7 212 }*/
Knillinux 1:40bdbe1a93b7 213
Knillinux 1:40bdbe1a93b7 214 /******************************
Knillinux 1:40bdbe1a93b7 215 Normal code
Knillinux 1:40bdbe1a93b7 216 *******************************/
Knillinux 1:40bdbe1a93b7 217 bool verbose = true; // Toggle debug topic output
Knillinux 1:40bdbe1a93b7 218 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 219
Knillinux 0:dd126a1080d3 220 mbed::motorPWM.period(MOTOR_PWM_PERIOD);
Knillinux 0:dd126a1080d3 221
Knillinux 1:40bdbe1a93b7 222 nh.getHardware()->setBaud(SERIAL_BAUD_RATE);
Knillinux 0:dd126a1080d3 223 nh.initNode();
Knillinux 0:dd126a1080d3 224
Knillinux 1:40bdbe1a93b7 225 FreeFlyerHardware freeflyer(nh, &mbed::i2c, mbed::thruster_pinouts,
Knillinux 1:40bdbe1a93b7 226 &mbed::led_inv_out_en, verbose);
Knillinux 0:dd126a1080d3 227
Knillinux 0:dd126a1080d3 228 t_LED.reset();
Knillinux 1:40bdbe1a93b7 229 t_RGB_LED.reset();
Knillinux 0:dd126a1080d3 230 t_PID.reset();
Knillinux 0:dd126a1080d3 231 t_thrust.reset();
Knillinux 1:40bdbe1a93b7 232 t_meas.reset();
Knillinux 1:40bdbe1a93b7 233 t_pid_debug.reset();
Knillinux 0:dd126a1080d3 234
Knillinux 0:dd126a1080d3 235 t_LED.start();
Knillinux 1:40bdbe1a93b7 236 t_RGB_LED.start();
Knillinux 0:dd126a1080d3 237 t_PID.start();
Knillinux 0:dd126a1080d3 238 t_thrust.start();
Knillinux 1:40bdbe1a93b7 239 t_meas.start();
Knillinux 1:40bdbe1a93b7 240 t_pid_debug.start();
Knillinux 1:40bdbe1a93b7 241
Knillinux 1:40bdbe1a93b7 242 #if (MODE == DEBUG_THRUSTERS)
Knillinux 1:40bdbe1a93b7 243 int thruster_ID_on = 0;
Knillinux 1:40bdbe1a93b7 244 int thruster_pinout_cmd[8];
Knillinux 1:40bdbe1a93b7 245 #elif (MODE == DEBUG_LED_CONST)
Knillinux 1:40bdbe1a93b7 246 freeflyer.rgba_led_->setColor(color::magenta);
Knillinux 1:40bdbe1a93b7 247 freeflyer.rgba_led_->setBrightness(1.0);
Knillinux 1:40bdbe1a93b7 248 #elif (MODE == DEBUG_LED_COLOR_SEQ)
Knillinux 1:40bdbe1a93b7 249 freeflyer.rgba_led_->setColor(color::blue);
Knillinux 1:40bdbe1a93b7 250 freeflyer.rgba_led_->setBrightness(1.0);
Knillinux 1:40bdbe1a93b7 251 freeflyer.rgba_led_->setColorProgram(color_program::fade1, color_time_program::fade1);
Knillinux 1:40bdbe1a93b7 252 #endif
Knillinux 0:dd126a1080d3 253
Knillinux 0:dd126a1080d3 254 while (true) {
Knillinux 1:40bdbe1a93b7 255
Knillinux 0:dd126a1080d3 256 // LED Blinking Loop
Knillinux 1:40bdbe1a93b7 257 if (t_LED.read_ms() >= LED_PERIOD) {
Knillinux 0:dd126a1080d3 258 t_LED.reset();
Knillinux 1:40bdbe1a93b7 259 mbed::LED_error = !mbed::LED_error;
Knillinux 0:dd126a1080d3 260 }
Knillinux 0:dd126a1080d3 261
Knillinux 0:dd126a1080d3 262 // PID Update Loop
Knillinux 1:40bdbe1a93b7 263 if (t_PID.read_ms() >= PID_PERIOD) {
Knillinux 0:dd126a1080d3 264 t_PID.reset();
Knillinux 1:40bdbe1a93b7 265 #if (MODE == DEBUG_WHEEL_CONST)
Knillinux 1:40bdbe1a93b7 266 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 267 freeflyer.setPWMOut(DWC_PWM);
Knillinux 1:40bdbe1a93b7 268 #elif (MODE == DEBUG_WHEEL_PID)
Knillinux 1:40bdbe1a93b7 269 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 270 freeflyer.setPIDSetpoint(DWP_SETPOINT);
Knillinux 0:dd126a1080d3 271 freeflyer.updatePID();
Knillinux 1:40bdbe1a93b7 272 #else
Knillinux 1:40bdbe1a93b7 273 if (!freeflyer.duty_cycle_command_mode_) {
Knillinux 1:40bdbe1a93b7 274 freeflyer.updatePID();
Knillinux 1:40bdbe1a93b7 275 } else {
Knillinux 1:40bdbe1a93b7 276 freeflyer.setPWMOut(freeflyer.duty_cycle_cmd_);
Knillinux 1:40bdbe1a93b7 277 }
Knillinux 1:40bdbe1a93b7 278 #endif
Knillinux 1:40bdbe1a93b7 279
Knillinux 1:40bdbe1a93b7 280 mbed::commandMotor(utils::min(freeflyer.getPWMOut(), PWM_LIMIT));
Knillinux 1:40bdbe1a93b7 281 }
Knillinux 1:40bdbe1a93b7 282
Knillinux 1:40bdbe1a93b7 283 // PID parameter publishing loop
Knillinux 1:40bdbe1a93b7 284 if (t_pid_debug.read_ms() >= PID_DEBUG_PERIOD && nh.connected() && !freeflyer.duty_cycle_command_mode_) {
Knillinux 1:40bdbe1a93b7 285 t_pid_debug.reset();
Knillinux 1:40bdbe1a93b7 286 freeflyer.publishPIDParam();
Knillinux 0:dd126a1080d3 287 }
Knillinux 0:dd126a1080d3 288
Knillinux 0:dd126a1080d3 289 // Thruster PWM Loop
Knillinux 1:40bdbe1a93b7 290 #if (MODE == DEBUG_THRUSTERS)
Knillinux 1:40bdbe1a93b7 291 if (t_thrust.read_ms() >= DT_PERIOD) {
Knillinux 1:40bdbe1a93b7 292 t_thrust.reset();
Knillinux 1:40bdbe1a93b7 293 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 294 if (++thruster_ID_on == N_THRUSTERS) {
Knillinux 1:40bdbe1a93b7 295 thruster_ID_on = 0;
Knillinux 1:40bdbe1a93b7 296 mbed::LED_debugB = !mbed::LED_debugB;
Knillinux 1:40bdbe1a93b7 297 }
Knillinux 0:dd126a1080d3 298
Knillinux 1:40bdbe1a93b7 299 for (int i = 0; i < N_THRUSTERS; i++)
Knillinux 1:40bdbe1a93b7 300 if (i == thruster_ID_on)
Knillinux 1:40bdbe1a93b7 301 thruster_pinout_cmd[i] = 1;
Knillinux 1:40bdbe1a93b7 302 else
Knillinux 1:40bdbe1a93b7 303 thruster_pinout_cmd[i] = 0;
Knillinux 1:40bdbe1a93b7 304
Knillinux 1:40bdbe1a93b7 305 freeflyer.commandThrusters(thruster_pinout_cmd);
Knillinux 1:40bdbe1a93b7 306 }
Knillinux 1:40bdbe1a93b7 307 #else
Knillinux 1:40bdbe1a93b7 308 if (t_thrust.read_ms() >= THRUST_PERIOD/THRUST_PWM_N) {
Knillinux 0:dd126a1080d3 309 t_thrust.reset();
Knillinux 0:dd126a1080d3 310 freeflyer.stepThrusterPWM();
Knillinux 0:dd126a1080d3 311 }
Knillinux 1:40bdbe1a93b7 312 #endif
Knillinux 0:dd126a1080d3 313
Knillinux 1:40bdbe1a93b7 314 // Measurement publishing loop
Knillinux 1:40bdbe1a93b7 315 if (t_meas.read_ms() >= MEAS_PERIOD && nh.connected()) {
Knillinux 1:40bdbe1a93b7 316 t_meas.reset();
Knillinux 1:40bdbe1a93b7 317 freeflyer.publishWheelMeas();
Knillinux 0:dd126a1080d3 318 }
Knillinux 0:dd126a1080d3 319
Knillinux 1:40bdbe1a93b7 320 // RGB LED update loop
Knillinux 1:40bdbe1a93b7 321 if (t_RGB_LED.read_ms() >= freeflyer.rgba_led_->getStepTime()) {
Knillinux 1:40bdbe1a93b7 322 t_RGB_LED.reset();
Knillinux 1:40bdbe1a93b7 323 freeflyer.rgba_led_->step();
Knillinux 1:40bdbe1a93b7 324 }
Knillinux 1:40bdbe1a93b7 325
Knillinux 0:dd126a1080d3 326 nh.spinOnce();
Knillinux 0:dd126a1080d3 327 }
Knillinux 0:dd126a1080d3 328
Knillinux 1:40bdbe1a93b7 329 }