File content as of revision 1:40bdbe1a93b7:

#include "FreeFlyerHardware.h"

FreeFlyerHardware::FreeFlyerHardware(ros::NodeHandle &nh, I2C *i2c, DigitalOut *thruster_pinouts,
        DigitalOut *led_inv_out_en, bool debug_flag)
: thruster_pwm_clock_(0), shaft_vel_meas_(0), pwm_out_(0), slope_ff_(SLOPE_FF_INIT), inter_ff_(INTER_FF_INIT),
    debug_flag_(debug_flag), duty_cycle_command_mode_(MODE_DUTY_CYCLE_CMD), feed_forward_mode_(MODE_FEED_FORWARD)
{
    // 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
    if (feed_forward_mode_) 
        controller_ = new PID(KP_FF_INIT, KP_FF_INIT/KI_FF_INIT, KD_FF_INIT/KP_FF_INIT, PID_PERIOD/1000.0);  //Kc, Ti, Td, interval
    else
        controller_ = new PID(KP_INIT, KP_INIT/KI_INIT, KD_INIT/KP_INIT, PID_PERIOD/1000.0);  //Kc, Ti, Td, interval

    controller_->setInterval(PID_PERIOD);
    controller_->setInputLimits(V_SMIN_ENF, V_SMAX_ENF);    // In terms of motor speed
    controller_->setOutputLimits(-CURRENT_MAX, CURRENT_MAX);
    controller_->setBias(0.0);
    controller_->setAccLimit(ACC_LIMIT_INIT);
    controller_->setSetPoint(INITIAL_SP);
    
    // Init encoder data processor
    wheel_encoder_ = new QEI(PIN_WENCA, PIN_WENCB, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
    
    // Init RGBA LED driver
    rgba_led_ = new RGBA_LED(i2c, led_inv_out_en, ADDR_RGB, MODE_AMBER);
    
    // ROS
    root_nh_ = &nh;
    
    // Init pub/sub
    pid_param_msg_.layout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension));
    pid_param_msg_.layout.dim[0].size = 4;
    pid_param_msg_.layout.dim[0].stride = 1*4;
    pid_param_msg_.layout.data_offset = 0;
    pid_param_msg_.data_length = 4;
    pid_param_msg_.data = (float *) malloc(sizeof(float) * 4);
    
    velocity_sns_pub_ = new ros::Publisher("wheel/sensors/velocity", &velocity_sns_msg_);
    setpoint_pub_ = new ros::Publisher("wheel/setpoint", &setpoint_msg_);
    acc_error_pub_ = new ros::Publisher("wheel/acc_error", &acc_error_msg_);
    pid_param_pub_ = new ros::Publisher("wheel/pid_params", &pid_param_msg_);
    
    root_nh_->advertise(*velocity_sns_pub_);
    root_nh_->advertise(*setpoint_pub_);
    root_nh_->advertise(*acc_error_pub_);
    root_nh_->advertise(*pid_param_pub_);
    
    wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
    wheel_duty_cycle_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/duty_cycle", &FreeFlyerHardware::wheelDutyCycleCmdCallback, this);
    wheel_pid_params_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("wheel/commands/pid_params", &FreeFlyerHardware::wheelPidParamsCmdCallback, this);
    thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("thruster/commands/duty_cycle", &FreeFlyerHardware::thrusterDutyCmdCallback, this);
    
    root_nh_->subscribe(*wheel_vel_cmd_sub_);
    root_nh_->subscribe(*wheel_pid_params_cmd_sub_);
    root_nh_->subscribe(*thruster_duty_cmd_sub_);
    if (duty_cycle_command_mode_)
        root_nh_->subscribe(*wheel_duty_cycle_cmd_sub_);
}

void FreeFlyerHardware::wheelVelCmdCallback(const std_msgs::Float32& msg) {
    controller_->setSetPoint(msg.data);
}

void FreeFlyerHardware::wheelDutyCycleCmdCallback(const std_msgs::Float32& msg) {
    duty_cycle_cmd_ = msg.data;
}

void FreeFlyerHardware::wheelPidParamsCmdCallback(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);
    controller_->setAccLimit(msg.data[3]);
}

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() {
    if (feed_forward_mode_) {
        // Apply measured feed-forward term and perform PID for small adjustments on top of that
        shaft_vel_meas_ = wheel_encoder_->getSpeed()*COUNTS2SHAFT;
        controller_->setProcessValue(shaft_vel_meas_);
        pwm_out_ = utils::smooth(slope_ff_*controller_->getSetPoint() + inter_ff_ + controller_->compute(), SMOOTHING_VAL, pwm_out_);
   
    } else {
        // Directly apply PID for all control of the reaction wheel speed
        shaft_vel_meas_ = wheel_encoder_->getSpeed()*COUNTS2SHAFT;
        controller_->setProcessValue(shaft_vel_meas_);
        current_out_ = controller_->compute();              // Desired current
        voltage_out_ = utils::smooth(R*current_out_ + KE*shaft_vel_meas_*NGR, SMOOTHING_VAL, voltage_out_);
        pwm_out_ = voltage_out_/VOLTAGE_MAX;
    }

    if (debug_flag_) {
        setpoint_msg_.data = controller_->getSetPoint();    // Speed setpoint
        acc_error_msg_.data = controller_->getAccError();   // Accumulated error
                     
        setpoint_pub_->publish(&setpoint_msg_);
        acc_error_pub_->publish(&acc_error_msg_);
    }  
}

float FreeFlyerHardware::getPWMOut() {
    return pwm_out_;
}

void FreeFlyerHardware::setPWMOut(float pwm_out) {
    pwm_out_ = pwm_out;
}

void FreeFlyerHardware::setPIDSetpoint(float pid_setpoint) {
    controller_->setSetPoint(pid_setpoint);
}

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

void FreeFlyerHardware::publishWheelMeas() {
    velocity_sns_msg_.data = wheel_encoder_->getSpeed()*COUNTS2SHAFT;   // Publish measured shaft speed in RPM
    velocity_sns_pub_->publish(&velocity_sns_msg_);
}

void FreeFlyerHardware::publishPIDParam() {
    pid_param_msg_.data[0] = controller_->getPParam();
    pid_param_msg_.data[1] = controller_->getPParam()/controller_->getIParam();
    pid_param_msg_.data[2] = controller_->getPParam()*controller_->getDParam();
    pid_param_msg_.data[3] = controller_->getAccLimit();
    
    pid_param_pub_->publish(&pid_param_msg_);
}