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
Diff: FreeFlyerHardware.h
- Revision:
- 0:dd126a1080d3
- Child:
- 1:40bdbe1a93b7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FreeFlyerHardware.h Tue Feb 14 05:12:54 2017 +0000 @@ -0,0 +1,63 @@ +#ifndef FREEFLYERHARDWARE_H +#define FREEFLYERHARDWARE_H + +#include "mbed.h" +#include "math.h" +#include <ros.h> +#include <std_msgs/Float32.h> +#include <std_msgs/Float32MultiArray.h> +#include "PID.h" +#include "QEI.h" +#include "defines.h" +#include "utilities.h" + +class FreeFlyerHardware +{ +public: + + FreeFlyerHardware(ros::NodeHandle &nh, DigitalOut *thruster_pinouts); + + void updatePID(); + void publishDebug(); + void commandThrusters(int *thruster_on_off_cmd); // switch to char for easy use? + void stepThrusterPWM(); + + float getVoltageOut(); + +protected: + int NUM_THRUSTERS; + + PID *controller_; + + QEI *wheel_encoder_; // object to interact with encoder outputs + + int thruster_pwm_stop_[8]; // Number that the PWM count reaches before each thruster turns off + int thruster_pwm_clock_; + + float scaled_meas_, current_out_, voltage_out_; + + DigitalOut *thruster_pinouts_; + + // ROS + ros::NodeHandle *root_nh_; + + ros::Publisher *velocity_sns_pub_; + ros::Publisher *setpoint_pub_, *estimate_pub_, *error_pub_; + ros::Publisher *acc_error_pub_, *current_cmd_pub_, *duty_cycle_pub_; + + std_msgs::Float32 velocity_sns_msg_; + std_msgs::Float32 setpoint_msg_, estimate_msg_, error_msg_; + std_msgs::Float32 acc_error_msg_, current_cmd_msg_, duty_cycle_msg_; + + ros::Subscriber<std_msgs::Float32, FreeFlyerHardware> *wheel_vel_cmd_sub_; + ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware> *wheel_pid_gains_cmd_sub_; + ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware> *thruster_duty_cmd_sub_; + + void wheelVelCmdCallback(const std_msgs::Float32& msg); + void wheelPidGainsCmdCallback(const std_msgs::Float32MultiArray& msg); + void thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg); + +}; + + +#endif \ No newline at end of file