Diff: FreeFlyerHardware.cpp

Revision:

0:dd126a1080d3

Child:

1:40bdbe1a93b7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FreeFlyerHardware.cpp	Tue Feb 14 05:12:54 2017 +0000
@@ -0,0 +1,126 @@
+
+#include "FreeFlyerHardware.h"
+
+FreeFlyerHardware::FreeFlyerHardware(ros::NodeHandle &nh, DigitalOut *thruster_pinouts)
+: thruster_pwm_clock_(0)
+{
+    // Get params
+    NUM_THRUSTERS = N_THRUSTERS;
+    
+    // Init thruster pinouts (set all to 0)
+    thruster_pinouts_ = thruster_pinouts;
+    for (int i = 0; i < NUM_THRUSTERS; i++) {
+        thruster_pinouts_[i] = 0;
+        thruster_pwm_stop_[i] = 0;
+    }
+    
+    // Init PID controller
+    controller_ = new PID(KP, KP/KI, KD/KP, PID_PERIOD/1000.0); //Kc, Ti, Td, interval
+    controller_->setInterval(PID_PERIOD);
+    controller_->setInputLimits(0, SCALED_MAX);
+    controller_->setOutputLimits(-CURRENT_MAX, CURRENT_MAX);
+    controller_->setBias(0.0);
+    controller_->setAccLimit(ACC_LIMIT);
+    controller_->setSetPoint(INITIAL_SP);
+    
+    // Init encoder data processor
+    wheel_encoder_ = new QEI(p17, p18, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
+    
+    // ROS
+    root_nh_ = &nh;
+    
+    // Init pub/sub
+    velocity_sns_pub_ = new ros::Publisher("wheel/sensors/velocity", &velocity_sns_msg_);
+    setpoint_pub_ = new ros::Publisher("wheel/debug/setpoint", &setpoint_msg_);
+    estimate_pub_ = new ros::Publisher("wheel/debug/estimate", &estimate_msg_);
+    error_pub_ = new ros::Publisher("wheel/debug/error", &error_msg_);
+    acc_error_pub_ = new ros::Publisher("wheel/debug/acc_error", &acc_error_msg_);
+    current_cmd_pub_ = new ros::Publisher("wheel/debug/current_cmd", &current_cmd_msg_);
+    duty_cycle_pub_ = new ros::Publisher("wheel/debug/duty_cycle", &duty_cycle_msg_);
+    
+    root_nh_->advertise(*velocity_sns_pub_);
+    root_nh_->advertise(*setpoint_pub_);
+    root_nh_->advertise(*estimate_pub_);
+    root_nh_->advertise(*error_pub_);
+    root_nh_->advertise(*acc_error_pub_ );
+    root_nh_->advertise(*current_cmd_pub_);
+    root_nh_->advertise(*duty_cycle_pub_);
+    
+    wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
+    wheel_pid_gains_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("wheel/commands/pid_gains", &FreeFlyerHardware::wheelPidGainsCmdCallback, this);
+    thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("wheel/commands/pid_gains", &FreeFlyerHardware::wheelPidGainsCmdCallback, this);
+}
+
+void FreeFlyerHardware::wheelVelCmdCallback(const std_msgs::Float32& msg) {
+    
+    float motor_command = msg.data*SCALE_VSHAFT;
+    
+    if ((motor_command >= SCALED_SMIN_ENF) && (motor_command <= SCALED_SMAX_ENF))                   
+        controller_->setSetPoint(motor_command);
+    else if (motor_command <= SCALED_SMIN_ENF)
+        controller_->setSetPoint(SCALED_SMIN_ENF);
+    else if (motor_command >= SCALED_SMAX_ENF)
+        controller_->setSetPoint(SCALED_SMAX_ENF);
+}
+
+void FreeFlyerHardware::wheelPidGainsCmdCallback(const std_msgs::Float32MultiArray& msg) {
+    
+    float kp_new = msg.data[0];
+    float ki_new = msg.data[1];
+    float kd_new = msg.data[2];
+    
+    controller_->setTunings(kp_new, kp_new/ki_new, kd_new/kp_new);
+}
+
+void FreeFlyerHardware::thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg) {
+    for (int i = 0; i < NUM_THRUSTERS; i++)
+        thruster_pwm_stop_[i] = ceil(msg.data[i]*THRUST_PWM_N);
+}
+
+void FreeFlyerHardware::updatePID() {
+    scaled_meas_ = wheel_encoder_->getSpeed()*SCALE_COUNTS;
+    controller_->setProcessValue(scaled_meas_);
+    current_out_ = controller_->compute();             // Desired current
+    voltage_out_ = utils::smooth(R*current_out_ + KE_SCALED*scaled_meas_, SMOOTHING_VAL, voltage_out_);
+}
+
+void FreeFlyerHardware::publishDebug() {
+    setpoint_msg_.data = controller_->setPoint_;                 // Speed setpoint
+    estimate_msg_.data = scaled_meas_;                          // Speed estimate
+    error_msg_.data = controller_->setPoint_ - scaled_meas_;      // Error
+    acc_error_msg_.data = controller_->accError_;                 // AccError
+    current_cmd_msg_.data = current_out_;                       // Current out
+    duty_cycle_msg_.data = voltage_out_/VOLTAGE_MAX;            // Duty cycle out
+    
+    setpoint_pub_->publish(&setpoint_msg_);
+    estimate_pub_->publish(&estimate_msg_);
+    error_pub_->publish(&error_msg_);
+    acc_error_pub_->publish(&acc_error_msg_);
+    current_cmd_pub_->publish(&current_cmd_msg_);
+    duty_cycle_pub_->publish(&duty_cycle_msg_);
+}
+
+float FreeFlyerHardware::getVoltageOut() {
+    return voltage_out_;
+}
+
+void FreeFlyerHardware::commandThrusters(int *thruster_on_off_cmd) {
+    for (int i = 0; i < NUM_THRUSTERS; i++)
+        if (thruster_on_off_cmd[i] == 1)
+            thruster_pinouts_[i] == 1;
+        else
+            thruster_pinouts_[i] == 0;
+}
+
+void FreeFlyerHardware::stepThrusterPWM() {
+    thruster_pwm_clock_++;
+    
+    for (int i = 0; i < NUM_THRUSTERS; i++)
+        if (thruster_pwm_stop_[i] <= thruster_pwm_clock_)
+            thruster_pinouts_[i] = 1;
+        else
+            thruster_pinouts_[i] = 0;
+            
+    if (thruster_pwm_clock_ == THRUST_PWM_N)
+        thruster_pwm_clock_ = 0;
+}
\ No newline at end of file