File content as of revision 0:dd126a1080d3:

#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;
}