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@4:cae255669971, 2018-06-29 (annotated)

Committer:
ambyld
Date:
Fri Jun 29 02:30:38 2018 +0000
Revision:
4:cae255669971
Parent:
3:402a6464f025
Child:
5:864709d3eb76
Enabled negative wheel speed measurement

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"
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
ambyld 4:cae255669971 28 ros::NodeHandle nh; // Comment out during serial 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
ambyld 4:cae255669971 84 /*
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
ambyld 4:cae255669971 111
ambyld 4:cae255669971 112 // Testing Wheel Encoder
ambyld 4:cae255669971 113 /*Serial pc(USBTX, USBRX); // Comment out during normal operation
ambyld 4:cae255669971 114 pc.printf("Hello World!\n");
ambyld 4:cae255669971 115 QEI wheel_encoder(PIN_WENCA, PIN_WENCB, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
ambyld 4:cae255669971 116 Timer t_QEI;
ambyld 4:cae255669971 117 t_QEI.reset();
ambyld 4:cae255669971 118 t_QEI.start();
ambyld 4:cae255669971 119 while (true) {
ambyld 4:cae255669971 120 if (t_QEI.read_ms() >= 500) {
ambyld 4:cae255669971 121 t_QEI.reset();
ambyld 4:cae255669971 122 pc.printf("Current number of counts: %d\n", wheel_encoder.getPulses());
ambyld 4:cae255669971 123 pc.printf("Measured wheel speed: %f\n", -wheel_encoder.getSpeed()*COUNTS2SHAFT);
ambyld 4:cae255669971 124 }
ambyld 4:cae255669971 125 }*/
ambyld 4:cae255669971 126
ambyld 4:cae255669971 127
Knillinux 1:40bdbe1a93b7 128 ////////
Knillinux 1:40bdbe1a93b7 129 // Blinky test code
Knillinux 1:40bdbe1a93b7 130 /* nh.getHardware()->setBaud(115200);
Knillinux 1:40bdbe1a93b7 131 nh.initNode();
Knillinux 1:40bdbe1a93b7 132 nh.subscribe(sub);
Knillinux 1:40bdbe1a93b7 133
Knillinux 1:40bdbe1a93b7 134 while (1) {
Knillinux 1:40bdbe1a93b7 135 nh.spinOnce();
Knillinux 1:40bdbe1a93b7 136 wait_ms(1);
Knillinux 1:40bdbe1a93b7 137 }
Knillinux 1:40bdbe1a93b7 138 */
Knillinux 1:40bdbe1a93b7 139 /*
Knillinux 1:40bdbe1a93b7 140 // I2C device searching script
Knillinux 1:40bdbe1a93b7 141 //mbed::i2c.frequency(10000); // set required i2c frequency
Knillinux 1:40bdbe1a93b7 142
Knillinux 1:40bdbe1a93b7 143 pc.printf("I2C Searching!\n\n");
Knillinux 1:40bdbe1a93b7 144 pc.printf("Starting....\n\n");
Knillinux 1:40bdbe1a93b7 145 while (1) {
Knillinux 1:40bdbe1a93b7 146 int count = 0;
Knillinux 1:40bdbe1a93b7 147 for (int address=0; address<256; address+=2) {
Knillinux 1:40bdbe1a93b7 148 if (!mbed::i2c.write(address, NULL, 0)) { // 0 returned is ok
Knillinux 1:40bdbe1a93b7 149 pc.printf("I2C address 0x%02X\n", address);
Knillinux 1:40bdbe1a93b7 150 count++;
Knillinux 1:40bdbe1a93b7 151 }
Knillinux 1:40bdbe1a93b7 152 }
Knillinux 1:40bdbe1a93b7 153 pc.printf("%d devices found\n\n\n", count);
Knillinux 1:40bdbe1a93b7 154 wait(0.000001);
Knillinux 1:40bdbe1a93b7 155 }
Knillinux 1:40bdbe1a93b7 156
Knillinux 1:40bdbe1a93b7 157 //mbed::pc.printf("Hello World!\n");
Knillinux 1:40bdbe1a93b7 158
Knillinux 1:40bdbe1a93b7 159 */
Knillinux 1:40bdbe1a93b7 160
Knillinux 1:40bdbe1a93b7 161 /*======
Knillinux 1:40bdbe1a93b7 162 // Testing bit masking
Knillinux 1:40bdbe1a93b7 163 =======*/
Knillinux 1:40bdbe1a93b7 164
Knillinux 1:40bdbe1a93b7 165 /*char cmd;
Knillinux 1:40bdbe1a93b7 166 while (true) {
Knillinux 1:40bdbe1a93b7 167 cmd = 0x00;
Knillinux 1:40bdbe1a93b7 168 cmd |= (1UL << (0x02-2));
Knillinux 1:40bdbe1a93b7 169 printBits(cmd);
Knillinux 1:40bdbe1a93b7 170 pc.printf("\n");
Knillinux 1:40bdbe1a93b7 171 wait(1.0);
Knillinux 1:40bdbe1a93b7 172 }*/
Knillinux 1:40bdbe1a93b7 173
Knillinux 1:40bdbe1a93b7 174 /******************************************
Knillinux 1:40bdbe1a93b7 175 RGB LED testing code, FF Driver v2.0
Knillinux 1:40bdbe1a93b7 176 ******************************************/
Knillinux 1:40bdbe1a93b7 177 /*char cmd[19];
Knillinux 1:40bdbe1a93b7 178 led_inv_out_en = 0;
Knillinux 1:40bdbe1a93b7 179
Knillinux 1:40bdbe1a93b7 180 while (true) {
Knillinux 1:40bdbe1a93b7 181
Knillinux 1:40bdbe1a93b7 182 wait(0.5);
Knillinux 1:40bdbe1a93b7 183
Knillinux 1:40bdbe1a93b7 184 cmd[0] = 0x80; // Send control register: start with register 00, autoincrement
Knillinux 1:40bdbe1a93b7 185 cmd[1] = 0x80; // MODE1 (default value, in normal power mode)
Knillinux 1:40bdbe1a93b7 186 cmd[2] = 0x09; // MODE2 (default value, but with invert off, outputs change on ack, open-drain)
Knillinux 1:40bdbe1a93b7 187 cmd[3] = 0x00; // PWM0 (B2)
Knillinux 1:40bdbe1a93b7 188 cmd[4] = 0x00; // PWM1 (W2 - ignore)
Knillinux 1:40bdbe1a93b7 189 cmd[5] = 0x00; // PWM2 (G2)
Knillinux 1:40bdbe1a93b7 190 cmd[6] = 0x00; // PWM3 (R2)
Knillinux 1:40bdbe1a93b7 191 cmd[7] = 0x00; // PWM4 (B1)
Knillinux 1:40bdbe1a93b7 192 cmd[8] = 0x00; // PWM5 (W1 - ignore)
Knillinux 1:40bdbe1a93b7 193 cmd[9] = 0x00; // PWM6 (G1)
Knillinux 1:40bdbe1a93b7 194 cmd[10] = 0x00; // PWM7 (R1)
Knillinux 1:40bdbe1a93b7 195 cmd[11] = 0xff; // GRPPWM (default value)
Knillinux 1:40bdbe1a93b7 196 cmd[12] = 0x00; // GRPFREQ (default value)
Knillinux 1:40bdbe1a93b7 197 cmd[13] = 0x55; // LEDOUT0 (0x55: all fully on)
Knillinux 1:40bdbe1a93b7 198 cmd[14] = 0x55; // LEDOUT1 (0x55: all fully on)
Knillinux 1:40bdbe1a93b7 199 cmd[13] = 0x00; // LEDOUT0 (0x55: all fully off)
Knillinux 1:40bdbe1a93b7 200 cmd[14] = 0x00; // LEDOUT1 (0x55: all fully off)
Knillinux 1:40bdbe1a93b7 201 cmd[15] = 0x00; // SUBADR1
Knillinux 1:40bdbe1a93b7 202 cmd[16] = 0x00; // SUBADR2
Knillinux 1:40bdbe1a93b7 203 cmd[17] = 0x00; // SUBADR3
Knillinux 1:40bdbe1a93b7 204 cmd[18] = 0x00; // ALLCALLADR
Knillinux 1:40bdbe1a93b7 205
Knillinux 1:40bdbe1a93b7 206 mbed::i2c.write(ADDR_RGB, cmd, 19);
Knillinux 1:40bdbe1a93b7 207
Knillinux 1:40bdbe1a93b7 208 //cmd[0] = 0x8d; // Send control register: start with register 12, autoincrement
Knillinux 1:40bdbe1a93b7 209 //cmd[1] = 0x55; // LEDOUT0 (0x55: all fully on)
Knillinux 1:40bdbe1a93b7 210 //cmd[2] = 0x55; // LEDOUT1 (0x55: all fully on)
Knillinux 1:40bdbe1a93b7 211 //cmd[1] = 0x00; // LEDOUT0 (0x55: all fully off)
Knillinux 1:40bdbe1a93b7 212 //cmd[2] = 0x00; // LEDOUT1 (0x55: all fully off)
Knillinux 1:40bdbe1a93b7 213
Knillinux 1:40bdbe1a93b7 214 //mbed::i2c.write(ADDR_RGB, cmd, 3);
Knillinux 1:40bdbe1a93b7 215 //mbed::i2c.write(mbed::addr, cmd, 3);
Knillinux 1:40bdbe1a93b7 216
Knillinux 1:40bdbe1a93b7 217 cmd[0] = 0x80;
Knillinux 1:40bdbe1a93b7 218 mbed::i2c.write(ADDR_RGB, cmd, 1);
Knillinux 1:40bdbe1a93b7 219 for (int i = 0; i < 18; i++)
Knillinux 1:40bdbe1a93b7 220 cmd[i] = 0;
Knillinux 1:40bdbe1a93b7 221
Knillinux 1:40bdbe1a93b7 222 mbed::i2c.read(ADDR_RGB, cmd, 18);
Knillinux 1:40bdbe1a93b7 223
Knillinux 1:40bdbe1a93b7 224 for (int i = 0; i < 18; i++) {
Knillinux 1:40bdbe1a93b7 225 printBits(cmd[i]);
Knillinux 1:40bdbe1a93b7 226 pc.printf("\n");
Knillinux 1:40bdbe1a93b7 227 }
Knillinux 1:40bdbe1a93b7 228 pc.printf("\n");
Knillinux 1:40bdbe1a93b7 229 }*/
Knillinux 1:40bdbe1a93b7 230
Knillinux 1:40bdbe1a93b7 231 /******************************
Knillinux 1:40bdbe1a93b7 232 Normal code
Knillinux 1:40bdbe1a93b7 233 *******************************/
ambyld 4:cae255669971 234
Knillinux 1:40bdbe1a93b7 235 bool verbose = true; // Toggle debug topic output
Knillinux 1:40bdbe1a93b7 236 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 237
Knillinux 0:dd126a1080d3 238 mbed::motorPWM.period(MOTOR_PWM_PERIOD);
Knillinux 0:dd126a1080d3 239
Knillinux 1:40bdbe1a93b7 240 nh.getHardware()->setBaud(SERIAL_BAUD_RATE);
Knillinux 0:dd126a1080d3 241 nh.initNode();
Knillinux 0:dd126a1080d3 242
Knillinux 1:40bdbe1a93b7 243 FreeFlyerHardware freeflyer(nh, &mbed::i2c, mbed::thruster_pinouts,
Knillinux 1:40bdbe1a93b7 244 &mbed::led_inv_out_en, verbose);
ambyld 4:cae255669971 245
Knillinux 0:dd126a1080d3 246 t_LED.reset();
Knillinux 1:40bdbe1a93b7 247 t_RGB_LED.reset();
Knillinux 0:dd126a1080d3 248 t_PID.reset();
Knillinux 0:dd126a1080d3 249 t_thrust.reset();
Knillinux 1:40bdbe1a93b7 250 t_meas.reset();
Knillinux 1:40bdbe1a93b7 251 t_pid_debug.reset();
Knillinux 0:dd126a1080d3 252
Knillinux 0:dd126a1080d3 253 t_LED.start();
Knillinux 1:40bdbe1a93b7 254 t_RGB_LED.start();
Knillinux 0:dd126a1080d3 255 t_PID.start();
Knillinux 0:dd126a1080d3 256 t_thrust.start();
Knillinux 1:40bdbe1a93b7 257 t_meas.start();
Knillinux 1:40bdbe1a93b7 258 t_pid_debug.start();
Knillinux 1:40bdbe1a93b7 259
Knillinux 1:40bdbe1a93b7 260 #if (MODE == DEBUG_THRUSTERS)
Knillinux 1:40bdbe1a93b7 261 int thruster_ID_on = 0;
Knillinux 1:40bdbe1a93b7 262 int thruster_pinout_cmd[8];
Knillinux 1:40bdbe1a93b7 263 #elif (MODE == DEBUG_LED_CONST)
Knillinux 1:40bdbe1a93b7 264 freeflyer.rgba_led_->setColor(color::magenta);
Knillinux 1:40bdbe1a93b7 265 freeflyer.rgba_led_->setBrightness(1.0);
Knillinux 1:40bdbe1a93b7 266 #elif (MODE == DEBUG_LED_COLOR_SEQ)
Knillinux 1:40bdbe1a93b7 267 freeflyer.rgba_led_->setColor(color::blue);
Knillinux 1:40bdbe1a93b7 268 freeflyer.rgba_led_->setBrightness(1.0);
Knillinux 1:40bdbe1a93b7 269 freeflyer.rgba_led_->setColorProgram(color_program::fade1, color_time_program::fade1);
Knillinux 1:40bdbe1a93b7 270 #endif
Knillinux 0:dd126a1080d3 271
Knillinux 0:dd126a1080d3 272 while (true) {
Knillinux 1:40bdbe1a93b7 273
Knillinux 0:dd126a1080d3 274 // LED Blinking Loop
Knillinux 1:40bdbe1a93b7 275 if (t_LED.read_ms() >= LED_PERIOD) {
Knillinux 0:dd126a1080d3 276 t_LED.reset();
Knillinux 1:40bdbe1a93b7 277 mbed::LED_error = !mbed::LED_error;
Knillinux 0:dd126a1080d3 278 }
Knillinux 0:dd126a1080d3 279
Knillinux 0:dd126a1080d3 280 // PID Update Loop
Knillinux 1:40bdbe1a93b7 281 if (t_PID.read_ms() >= PID_PERIOD) {
Knillinux 0:dd126a1080d3 282 t_PID.reset();
Knillinux 1:40bdbe1a93b7 283 #if (MODE == DEBUG_WHEEL_CONST)
Knillinux 1:40bdbe1a93b7 284 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 285 freeflyer.setPWMOut(DWC_PWM);
Knillinux 1:40bdbe1a93b7 286 #elif (MODE == DEBUG_WHEEL_PID)
Knillinux 1:40bdbe1a93b7 287 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 288 freeflyer.setPIDSetpoint(DWP_SETPOINT);
Knillinux 0:dd126a1080d3 289 freeflyer.updatePID();
Knillinux 1:40bdbe1a93b7 290 #else
Knillinux 1:40bdbe1a93b7 291 if (!freeflyer.duty_cycle_command_mode_) {
Knillinux 1:40bdbe1a93b7 292 freeflyer.updatePID();
Knillinux 1:40bdbe1a93b7 293 } else {
Knillinux 1:40bdbe1a93b7 294 freeflyer.setPWMOut(freeflyer.duty_cycle_cmd_);
Knillinux 1:40bdbe1a93b7 295 }
Knillinux 1:40bdbe1a93b7 296 #endif
Knillinux 1:40bdbe1a93b7 297
Knillinux 1:40bdbe1a93b7 298 mbed::commandMotor(utils::min(freeflyer.getPWMOut(), PWM_LIMIT));
Knillinux 1:40bdbe1a93b7 299 }
Knillinux 1:40bdbe1a93b7 300
Knillinux 1:40bdbe1a93b7 301 // PID parameter publishing loop
Knillinux 1:40bdbe1a93b7 302 if (t_pid_debug.read_ms() >= PID_DEBUG_PERIOD && nh.connected() && !freeflyer.duty_cycle_command_mode_) {
Knillinux 1:40bdbe1a93b7 303 t_pid_debug.reset();
Knillinux 1:40bdbe1a93b7 304 freeflyer.publishPIDParam();
Knillinux 0:dd126a1080d3 305 }
Knillinux 0:dd126a1080d3 306
Knillinux 0:dd126a1080d3 307 // Thruster PWM Loop
Knillinux 1:40bdbe1a93b7 308 #if (MODE == DEBUG_THRUSTERS)
Knillinux 1:40bdbe1a93b7 309 if (t_thrust.read_ms() >= DT_PERIOD) {
Knillinux 1:40bdbe1a93b7 310 t_thrust.reset();
Knillinux 1:40bdbe1a93b7 311 mbed::LED_debugA = !mbed::LED_debugA;
Knillinux 1:40bdbe1a93b7 312 if (++thruster_ID_on == N_THRUSTERS) {
Knillinux 1:40bdbe1a93b7 313 thruster_ID_on = 0;
Knillinux 1:40bdbe1a93b7 314 mbed::LED_debugB = !mbed::LED_debugB;
Knillinux 1:40bdbe1a93b7 315 }
Knillinux 0:dd126a1080d3 316
Knillinux 1:40bdbe1a93b7 317 for (int i = 0; i < N_THRUSTERS; i++)
Knillinux 1:40bdbe1a93b7 318 if (i == thruster_ID_on)
Knillinux 1:40bdbe1a93b7 319 thruster_pinout_cmd[i] = 1;
Knillinux 1:40bdbe1a93b7 320 else
Knillinux 1:40bdbe1a93b7 321 thruster_pinout_cmd[i] = 0;
Knillinux 1:40bdbe1a93b7 322
Knillinux 1:40bdbe1a93b7 323 freeflyer.commandThrusters(thruster_pinout_cmd);
Knillinux 1:40bdbe1a93b7 324 }
Knillinux 1:40bdbe1a93b7 325 #else
Knillinux 1:40bdbe1a93b7 326 if (t_thrust.read_ms() >= THRUST_PERIOD/THRUST_PWM_N) {
Knillinux 0:dd126a1080d3 327 t_thrust.reset();
Knillinux 0:dd126a1080d3 328 freeflyer.stepThrusterPWM();
Knillinux 0:dd126a1080d3 329 }
Knillinux 1:40bdbe1a93b7 330 #endif
Knillinux 0:dd126a1080d3 331
Knillinux 1:40bdbe1a93b7 332 // Measurement publishing loop
Knillinux 1:40bdbe1a93b7 333 if (t_meas.read_ms() >= MEAS_PERIOD && nh.connected()) {
Knillinux 1:40bdbe1a93b7 334 t_meas.reset();
Knillinux 1:40bdbe1a93b7 335 freeflyer.publishWheelMeas();
Knillinux 0:dd126a1080d3 336 }
Knillinux 0:dd126a1080d3 337
Knillinux 1:40bdbe1a93b7 338 // RGB LED update loop
Knillinux 1:40bdbe1a93b7 339 if (t_RGB_LED.read_ms() >= freeflyer.rgba_led_->getStepTime()) {
Knillinux 1:40bdbe1a93b7 340 t_RGB_LED.reset();
Knillinux 1:40bdbe1a93b7 341 freeflyer.rgba_led_->step();
Knillinux 1:40bdbe1a93b7 342 }
Knillinux 1:40bdbe1a93b7 343
Knillinux 0:dd126a1080d3 344 nh.spinOnce();
ambyld 4:cae255669971 345
Knillinux 0:dd126a1080d3 346 }
Knillinux 0:dd126a1080d3 347
Knillinux 1:40bdbe1a93b7 348 }