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@0:dd126a1080d3, 2017-02-14 (annotated)

Committer:
Knillinux
Date:
Tue Feb 14 05:12:54 2017 +0000
Revision:
0:dd126a1080d3
Child:
1:40bdbe1a93b7
test

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 0:dd126a1080d3 4 \author A. Bylard, R. MacPherson
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 0:dd126a1080d3 13
Knillinux 0:dd126a1080d3 14 #include "defines.h"
Knillinux 0:dd126a1080d3 15 #include "utilities.h"
Knillinux 0:dd126a1080d3 16 #include "FreeFlyerHardware.h"
Knillinux 0:dd126a1080d3 17
Knillinux 0:dd126a1080d3 18 extern "C" void mbed_reset(); //!< Makes the C++ function "mbed_reset" have 'C'-compiler linkage
Knillinux 0:dd126a1080d3 19
Knillinux 0:dd126a1080d3 20 //#####################################################################################\\
Knillinux 0:dd126a1080d3 21
Knillinux 0:dd126a1080d3 22 /*=============================================+
Knillinux 0:dd126a1080d3 23 | GLOBALS
Knillinux 0:dd126a1080d3 24 +==============================================*/
Knillinux 0:dd126a1080d3 25
Knillinux 0:dd126a1080d3 26 // ROS
Knillinux 0:dd126a1080d3 27 ros::NodeHandle nh;
Knillinux 0:dd126a1080d3 28
Knillinux 0:dd126a1080d3 29 /*=============================================+
Knillinux 0:dd126a1080d3 30 | ENUMERATIONS
Knillinux 0:dd126a1080d3 31 +==============================================*/
Knillinux 0:dd126a1080d3 32 namespace mbed {
Knillinux 0:dd126a1080d3 33 // LEDs
Knillinux 0:dd126a1080d3 34 DigitalOut LED_thrusters(LED1);
Knillinux 0:dd126a1080d3 35 DigitalOut LED_motorA(LED2);
Knillinux 0:dd126a1080d3 36 DigitalOut LED_motorB(LED3);
Knillinux 0:dd126a1080d3 37 DigitalOut LED_error(LED4);
Knillinux 0:dd126a1080d3 38
Knillinux 0:dd126a1080d3 39 // Thrusters
Knillinux 0:dd126a1080d3 40 DigitalOut thruster1(p12);
Knillinux 0:dd126a1080d3 41 DigitalOut thruster2(p11);
Knillinux 0:dd126a1080d3 42 DigitalOut thruster3(p8);
Knillinux 0:dd126a1080d3 43 DigitalOut thruster4(p7);
Knillinux 0:dd126a1080d3 44 DigitalOut thruster5(p6);
Knillinux 0:dd126a1080d3 45 DigitalOut thruster6(p5);
Knillinux 0:dd126a1080d3 46 DigitalOut thruster7(p30);
Knillinux 0:dd126a1080d3 47 DigitalOut thruster8(p29);
Knillinux 0:dd126a1080d3 48 DigitalOut thruster_pinouts[] = {
Knillinux 0:dd126a1080d3 49 (thruster1), (thruster2),
Knillinux 0:dd126a1080d3 50 (thruster3), (thruster4),
Knillinux 0:dd126a1080d3 51 (thruster5), (thruster6),
Knillinux 0:dd126a1080d3 52 (thruster7), (thruster8)}; //!< List of mBed thruster pinouts
Knillinux 0:dd126a1080d3 53
Knillinux 0:dd126a1080d3 54 // Reaction Wheel Motors
Knillinux 0:dd126a1080d3 55 PwmOut motorPWM(p21); //!< mBed pinout controlling momentum wheel motor H-Bridge input A
Knillinux 0:dd126a1080d3 56 DigitalOut motorA(p27);
Knillinux 0:dd126a1080d3 57 DigitalOut motorB(p28);
Knillinux 0:dd126a1080d3 58
Knillinux 0:dd126a1080d3 59 /*=============================================+
Knillinux 0:dd126a1080d3 60 | COMMANDS
Knillinux 0:dd126a1080d3 61 +==============================================*/
Knillinux 0:dd126a1080d3 62
Knillinux 0:dd126a1080d3 63 //! Relay motor command
Knillinux 0:dd126a1080d3 64 void commandMotor( float dutycycle ) {
Knillinux 0:dd126a1080d3 65 if (dutycycle >= 0.0) {
Knillinux 0:dd126a1080d3 66 mbed::motorPWM = dutycycle;
Knillinux 0:dd126a1080d3 67 mbed::motorA = 1;
Knillinux 0:dd126a1080d3 68 mbed::motorB = 0;
Knillinux 0:dd126a1080d3 69 } else {
Knillinux 0:dd126a1080d3 70 mbed::motorPWM = -dutycycle;
Knillinux 0:dd126a1080d3 71 mbed::motorA = 0;
Knillinux 0:dd126a1080d3 72 mbed::motorB = 1;
Knillinux 0:dd126a1080d3 73 }
Knillinux 0:dd126a1080d3 74 }
Knillinux 0:dd126a1080d3 75
Knillinux 0:dd126a1080d3 76 } // End "mbed" namespace
Knillinux 0:dd126a1080d3 77
Knillinux 0:dd126a1080d3 78 //#####################################################################################\\
Knillinux 0:dd126a1080d3 79
Knillinux 0:dd126a1080d3 80 /*=============================================+
Knillinux 0:dd126a1080d3 81 | MAIN FUNCTION
Knillinux 0:dd126a1080d3 82 +==============================================*/
Knillinux 0:dd126a1080d3 83
Knillinux 0:dd126a1080d3 84 int main() {
Knillinux 0:dd126a1080d3 85 bool pid_led_flag = false;
Knillinux 0:dd126a1080d3 86 bool verbose = false; // Toggle debug topic output
Knillinux 0:dd126a1080d3 87 Timer t_LED, t_PID, t_thrust, t_debug; // object to do timing to compute motor speed
Knillinux 0:dd126a1080d3 88
Knillinux 0:dd126a1080d3 89 mbed::motorPWM.period(MOTOR_PWM_PERIOD);
Knillinux 0:dd126a1080d3 90
Knillinux 0:dd126a1080d3 91 nh.getHardware()->setBaud(DEFAULT_BAUD_RATE);
Knillinux 0:dd126a1080d3 92 nh.initNode();
Knillinux 0:dd126a1080d3 93
Knillinux 0:dd126a1080d3 94 FreeFlyerHardware freeflyer(nh, mbed::thruster_pinouts);
Knillinux 0:dd126a1080d3 95
Knillinux 0:dd126a1080d3 96 t_LED.reset();
Knillinux 0:dd126a1080d3 97 t_PID.reset();
Knillinux 0:dd126a1080d3 98 t_thrust.reset();
Knillinux 0:dd126a1080d3 99 t_debug.reset();
Knillinux 0:dd126a1080d3 100
Knillinux 0:dd126a1080d3 101 t_LED.start();
Knillinux 0:dd126a1080d3 102 t_PID.start();
Knillinux 0:dd126a1080d3 103 t_thrust.start();
Knillinux 0:dd126a1080d3 104 t_debug.start();
Knillinux 0:dd126a1080d3 105
Knillinux 0:dd126a1080d3 106 while (true) {
Knillinux 0:dd126a1080d3 107
Knillinux 0:dd126a1080d3 108 // LED Blinking Loop
Knillinux 0:dd126a1080d3 109 if (t_LED.read_ms() >= LED_PERIOD) {
Knillinux 0:dd126a1080d3 110
Knillinux 0:dd126a1080d3 111 t_LED.reset();
Knillinux 0:dd126a1080d3 112 if (pid_led_flag)
Knillinux 0:dd126a1080d3 113 mbed::LED_error = 0;
Knillinux 0:dd126a1080d3 114 else
Knillinux 0:dd126a1080d3 115 mbed::LED_error = 1;
Knillinux 0:dd126a1080d3 116 pid_led_flag = !pid_led_flag;
Knillinux 0:dd126a1080d3 117 }
Knillinux 0:dd126a1080d3 118
Knillinux 0:dd126a1080d3 119 // PID Update Loop
Knillinux 0:dd126a1080d3 120 if (t_PID.read_ms() >= PID_PERIOD) {
Knillinux 0:dd126a1080d3 121
Knillinux 0:dd126a1080d3 122 t_PID.reset();
Knillinux 0:dd126a1080d3 123 freeflyer.updatePID();
Knillinux 0:dd126a1080d3 124 mbed::commandMotor(0.0);
Knillinux 0:dd126a1080d3 125 //mbed::commandMotor(freeflyer.getVoltageOut()/VOLTAGE_MAX*PWM_LIMIT);
Knillinux 0:dd126a1080d3 126 }
Knillinux 0:dd126a1080d3 127
Knillinux 0:dd126a1080d3 128 // Thruster PWM Loop
Knillinux 0:dd126a1080d3 129 if (t_thrust.read_ms() >= THRUST_PERIOD) {
Knillinux 0:dd126a1080d3 130
Knillinux 0:dd126a1080d3 131 t_thrust.reset();
Knillinux 0:dd126a1080d3 132 freeflyer.stepThrusterPWM();
Knillinux 0:dd126a1080d3 133 }
Knillinux 0:dd126a1080d3 134
Knillinux 0:dd126a1080d3 135 // Debug Loop
Knillinux 0:dd126a1080d3 136 if (verbose && t_debug.read_ms() >= DEBUG_PERIOD) {
Knillinux 0:dd126a1080d3 137
Knillinux 0:dd126a1080d3 138 t_debug.reset();
Knillinux 0:dd126a1080d3 139 freeflyer.publishDebug();
Knillinux 0:dd126a1080d3 140 }
Knillinux 0:dd126a1080d3 141
Knillinux 0:dd126a1080d3 142 nh.spinOnce();
Knillinux 0:dd126a1080d3 143
Knillinux 0:dd126a1080d3 144 }
Knillinux 0:dd126a1080d3 145 }
Knillinux 0:dd126a1080d3 146
Knillinux 0:dd126a1080d3 147 //#####################################################################################\\