FreeFlyerHardware.h

00001 #ifndef FREEFLYERHARDWARE_H
00002 #define FREEFLYERHARDWARE_H
00003 
00004 #include "mbed.h"
00005 #include "math.h"
00006 #include <ros.h>
00007 #include <std_msgs/Float32.h>
00008 #include <std_msgs/Float32MultiArray.h>
00009 #include "PID.h"
00010 #include "QEI.h"
00011 #include "defines.h"
00012 #include "RGBA_LED.h"
00013 #include "utilities.h"
00014 #include <algorithm>
00015 
00016 class FreeFlyerHardware
00017 {
00018 public:
00019     
00020     FreeFlyerHardware(ros::NodeHandle &nh, I2C *i2c, DigitalOut *thruster_pinouts,
00021         DigitalOut *led_inv_out_en, bool debug_flag);
00022     
00023     void updatePID();
00024     void commandThrusters(int *thruster_on_off_cmd);    // switch to char for easy use?
00025     void stepThrusterPWM();
00026     void publishWheelMeas();
00027     void publishPIDParam();
00028     
00029     float getPWMOut();
00030     void setPWMOut(float pwm_out);
00031     void setPIDSetpoint(float pid_setpoint);
00032     float getWheelVelocity();
00033     
00034     float duty_cycle_cmd_;          // Only for duty cycle command mode
00035     bool duty_cycle_command_mode_;
00036     bool feed_forward_mode_;
00037     
00038     RGBA_LED *rgba_led_;
00039     
00040 protected:
00041     int NUM_THRUSTERS;
00042     
00043     PID *controller_;
00044     
00045     QEI *wheel_encoder_; // object to interact with encoder outputs
00046     
00047     int thruster_pwm_stop_[8];       // Number that the PWM count reaches before each thruster turns off
00048     int thruster_pwm_clock_;
00049     
00050     float shaft_vel_meas_;          // [RPM] Measured shaft speed
00051     float current_out_, voltage_out_, pwm_out_;
00052 
00053     float slope_ff_, inter_ff_;     // Feedforward slope, intercept terms
00054     
00055     DigitalOut *thruster_pinouts_;
00056     
00057     // ROS
00058     ros::NodeHandle *root_nh_;
00059     
00060     ros::Publisher *velocity_sns_pub_, *setpoint_pub_, *acc_error_pub_;
00061     ros::Publisher *pid_param_pub_;
00062     
00063     std_msgs::Float32 velocity_sns_msg_, setpoint_msg_, acc_error_msg_;
00064     std_msgs::Float32MultiArray pid_param_msg_;
00065     
00066     ros::Subscriber<std_msgs::Float32, FreeFlyerHardware> *wheel_vel_cmd_sub_,
00067                                                           *wheel_duty_cycle_cmd_sub_;
00068     ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware> *thruster_duty_cmd_sub_,
00069                                                                     *wheel_pid_params_cmd_sub_;
00070     
00071     void wheelVelCmdCallback(const std_msgs::Float32& msg);
00072     void wheelDutyCycleCmdCallback(const std_msgs::Float32& msg);
00073     void wheelPidParamsCmdCallback(const std_msgs::Float32MultiArray& msg);
00074     void thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg);
00075     
00076     bool debug_flag_;
00077 };
00078     
00079     
00080 #endif