Diff: FreeFlyerHardware.h

Revision:

1:40bdbe1a93b7

Parent:

0:dd126a1080d3

Child:

4:cae255669971

--- a/FreeFlyerHardware.h	Tue Feb 14 05:12:54 2017 +0000
+++ b/FreeFlyerHardware.h	Fri Jun 22 02:09:50 2018 +0000
@@ -9,20 +9,32 @@
 #include "PID.h"
 #include "QEI.h"
 #include "defines.h"
+#include "RGBA_LED.h"
 #include "utilities.h"
+#include <algorithm>
 
 class FreeFlyerHardware
 {
 public:
     
-    FreeFlyerHardware(ros::NodeHandle &nh, DigitalOut *thruster_pinouts);
+    FreeFlyerHardware(ros::NodeHandle &nh, I2C *i2c, DigitalOut *thruster_pinouts,
+        DigitalOut *led_inv_out_en, bool debug_flag);
     
     void updatePID();
-    void publishDebug();
     void commandThrusters(int *thruster_on_off_cmd);    // switch to char for easy use?
     void stepThrusterPWM();
+    void publishWheelMeas();
+    void publishPIDParam();
     
-    float getVoltageOut();
+    float getPWMOut();
+    void setPWMOut(float pwm_out);
+    void setPIDSetpoint(float pid_setpoint);
+    
+    float duty_cycle_cmd_;          // Only for duty cycle command mode
+    bool duty_cycle_command_mode_;
+    bool feed_forward_mode_;
+    
+    RGBA_LED *rgba_led_;
     
 protected:
     int NUM_THRUSTERS;
@@ -34,29 +46,33 @@
     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_;
+    float shaft_vel_meas_;          // [RPM] Measured shaft speed
+    float current_out_, voltage_out_, pwm_out_;
+
+    float slope_ff_, inter_ff_;     // Feedforward slope, intercept terms
     
     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_;
+    ros::Publisher *velocity_sns_pub_, *setpoint_pub_, *acc_error_pub_;
+    ros::Publisher *pid_param_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_;
+    std_msgs::Float32 velocity_sns_msg_, setpoint_msg_, acc_error_msg_;
+    std_msgs::Float32MultiArray pid_param_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_;
+    ros::Subscriber<std_msgs::Float32, FreeFlyerHardware> *wheel_vel_cmd_sub_,
+                                                          *wheel_duty_cycle_cmd_sub_;
+    ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware> *thruster_duty_cmd_sub_,
+                                                                    *wheel_pid_params_cmd_sub_;
     
     void wheelVelCmdCallback(const std_msgs::Float32& msg);
-    void wheelPidGainsCmdCallback(const std_msgs::Float32MultiArray& msg);
+    void wheelDutyCycleCmdCallback(const std_msgs::Float32& msg);
+    void wheelPidParamsCmdCallback(const std_msgs::Float32MultiArray& msg);
     void thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg);
     
+    bool debug_flag_;
 };