Thomas Lew / Mbed 2 deprecated FreeFlyerROS

Dependencies:   mbed ros_lib_kinetic

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