FreeFlyerHardware.cpp

00001 
00002 #include "FreeFlyerHardware.h"
00003 
00004 FreeFlyerHardware::FreeFlyerHardware(ros::NodeHandle &nh, I2C *i2c, DigitalOut *thruster_pinouts,
00005         DigitalOut *led_inv_out_en, bool debug_flag)
00006 : thruster_pwm_clock_(0), shaft_vel_meas_(0), pwm_out_(0), slope_ff_(SLOPE_FF_INIT), inter_ff_(INTER_FF_INIT),
00007     debug_flag_(debug_flag), duty_cycle_command_mode_(MODE_DUTY_CYCLE_CMD), feed_forward_mode_(MODE_FEED_FORWARD)
00008 {
00009     // Get params
00010     NUM_THRUSTERS = N_THRUSTERS;
00011     
00012     // Init thruster pinouts (set all to 0)
00013     thruster_pinouts_ = thruster_pinouts;
00014     for (int i = 0; i < NUM_THRUSTERS; i++) {
00015         thruster_pinouts_[i] = 0;
00016         thruster_pwm_stop_[i] = 0;
00017     }
00018     
00019     // Init PID controller
00020     if (feed_forward_mode_) 
00021         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
00022     else
00023         controller_ = new PID(KP_INIT, KP_INIT/KI_INIT, KD_INIT/KP_INIT, PID_PERIOD/1000.0);  //Kc, Ti, Td, interval
00024 
00025     controller_->setInterval(PID_PERIOD);
00026     controller_->setInputLimits(V_SMIN_ENF, V_SMAX_ENF);    // In terms of motor speed
00027     controller_->setOutputLimits(-CURRENT_MAX, CURRENT_MAX);
00028     controller_->setBias(0.0);
00029     controller_->setAccLimit(ACC_LIMIT_INIT);
00030     controller_->setSetPoint(INITIAL_SP);
00031     
00032     // Init encoder data processor
00033     wheel_encoder_ = new QEI(PIN_WENCA, PIN_WENCB, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
00034     
00035     // Init RGBA LED driver
00036     rgba_led_ = new RGBA_LED(i2c, led_inv_out_en, ADDR_RGB, MODE_AMBER);
00037     
00038     // ROS
00039     root_nh_ = &nh;
00040     
00041     // Init pub/sub
00042     pid_param_msg_.layout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension));
00043     pid_param_msg_.layout.dim[0].size = 4;
00044     pid_param_msg_.layout.dim[0].stride = 1*4;
00045     pid_param_msg_.layout.data_offset = 0;
00046     pid_param_msg_.data_length = 4;
00047     pid_param_msg_.data = (float *) malloc(sizeof(float) * 4);
00048     
00049     velocity_sns_pub_ = new ros::Publisher("wheel/sensors/velocity", &velocity_sns_msg_);
00050     setpoint_pub_ = new ros::Publisher("wheel/setpoint", &setpoint_msg_);
00051     acc_error_pub_ = new ros::Publisher("wheel/acc_error", &acc_error_msg_);
00052     pid_param_pub_ = new ros::Publisher("wheel/pid_params", &pid_param_msg_);
00053     
00054     root_nh_->advertise(*velocity_sns_pub_);
00055     root_nh_->advertise(*setpoint_pub_);
00056     root_nh_->advertise(*acc_error_pub_);
00057     root_nh_->advertise(*pid_param_pub_);
00058     
00059     wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
00060     wheel_duty_cycle_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/duty_cycle", &FreeFlyerHardware::wheelDutyCycleCmdCallback, this);
00061     wheel_pid_params_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("wheel/commands/pid_params", &FreeFlyerHardware::wheelPidParamsCmdCallback, this);
00062     thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("thruster/commands/duty_cycle", &FreeFlyerHardware::thrusterDutyCmdCallback, this);
00063     
00064     root_nh_->subscribe(*wheel_vel_cmd_sub_);
00065     root_nh_->subscribe(*wheel_pid_params_cmd_sub_);
00066     root_nh_->subscribe(*thruster_duty_cmd_sub_);
00067     if (duty_cycle_command_mode_)
00068         root_nh_->subscribe(*wheel_duty_cycle_cmd_sub_);
00069 }
00070 
00071 void FreeFlyerHardware::wheelVelCmdCallback(const std_msgs::Float32& msg) {
00072     controller_->setSetPoint(msg.data);
00073 }
00074 
00075 void FreeFlyerHardware::wheelDutyCycleCmdCallback(const std_msgs::Float32& msg) {
00076     duty_cycle_cmd_ = msg.data;
00077 }
00078 
00079 void FreeFlyerHardware::wheelPidParamsCmdCallback(const std_msgs::Float32MultiArray& msg) {
00080     float kp_new = msg.data[0];
00081     float ki_new = msg.data[1];
00082     float kd_new = msg.data[2];
00083     
00084     controller_->setTunings(kp_new, kp_new/ki_new, kd_new/kp_new);
00085     controller_->setAccLimit(msg.data[3]);
00086 }
00087 
00088 void FreeFlyerHardware::thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg) {
00089     for (int i = 0; i < NUM_THRUSTERS; i++)
00090         thruster_pwm_stop_[i] = ceil(msg.data[i]*THRUST_PWM_N);
00091 }
00092 
00093 void FreeFlyerHardware::updatePID() {
00094     if (feed_forward_mode_) {
00095         // Apply measured feed-forward term and perform PID for small adjustments on top of that
00096         shaft_vel_meas_ = getWheelVelocity();
00097         controller_->setProcessValue(shaft_vel_meas_);
00098         pwm_out_ = utils::smooth(slope_ff_*controller_->getSetPoint() + inter_ff_ + controller_->compute(), SMOOTHING_VAL, pwm_out_);
00099    
00100     } else {
00101         // Directly apply PID for all control of the reaction wheel speed
00102         shaft_vel_meas_ = getWheelVelocity();
00103         controller_->setProcessValue(shaft_vel_meas_);
00104         current_out_ = controller_->compute();              // Desired current
00105         voltage_out_ = utils::smooth(R*current_out_ + KE*shaft_vel_meas_*NGR, SMOOTHING_VAL, voltage_out_);
00106         pwm_out_ = voltage_out_/VOLTAGE_MAX;
00107     }
00108 
00109     if (debug_flag_) {
00110         setpoint_msg_.data = controller_->getSetPoint();    // Speed setpoint
00111         acc_error_msg_.data = controller_->getAccError();   // Accumulated error
00112                      
00113         setpoint_pub_->publish(&setpoint_msg_);
00114         acc_error_pub_->publish(&acc_error_msg_);
00115     }  
00116 }
00117 
00118 float FreeFlyerHardware::getPWMOut() {
00119     return pwm_out_;
00120 }
00121 
00122 void FreeFlyerHardware::setPWMOut(float pwm_out) {
00123     pwm_out_ = pwm_out;
00124 }
00125 
00126 void FreeFlyerHardware::setPIDSetpoint(float pid_setpoint) {
00127     controller_->setSetPoint(pid_setpoint);
00128 }
00129 
00130 float FreeFlyerHardware::getWheelVelocity() {
00131     return -wheel_encoder_->getSpeed()*COUNTS2SHAFT; // Wheel direction convention is opposite from shaft direction convention
00132 }
00133 
00134 void FreeFlyerHardware::commandThrusters(int *thruster_on_off_cmd) {
00135     for (int i = 0; i < NUM_THRUSTERS; i++)
00136         if (thruster_on_off_cmd[i] == 1)
00137             thruster_pinouts_[i] = 1;
00138         else
00139             thruster_pinouts_[i] = 0;
00140 }
00141 
00142 void FreeFlyerHardware::stepThrusterPWM() {
00143     thruster_pwm_clock_++;
00144     
00145     for (int i = 0; i < NUM_THRUSTERS; i++)
00146         if (thruster_pwm_stop_[i] > thruster_pwm_clock_)
00147             thruster_pinouts_[i] = 1;
00148         else
00149             thruster_pinouts_[i] = 0;
00150             
00151     if (thruster_pwm_clock_ == THRUST_PWM_N)
00152         thruster_pwm_clock_ = 0;
00153 }
00154 
00155 void FreeFlyerHardware::publishWheelMeas() {
00156     velocity_sns_msg_.data = getWheelVelocity();   // Publish measured shaft speed in RPM
00157     velocity_sns_pub_->publish(&velocity_sns_msg_);
00158 }
00159 
00160 void FreeFlyerHardware::publishPIDParam() {
00161     pid_param_msg_.data[0] = controller_->getPParam();
00162     pid_param_msg_.data[1] = controller_->getPParam()/controller_->getIParam();
00163     pid_param_msg_.data[2] = controller_->getPParam()*controller_->getDParam();
00164     pid_param_msg_.data[3] = controller_->getAccLimit();
00165     
00166     pid_param_pub_->publish(&pid_param_msg_);
00167 }