Diff: FreeFlyerHardware.cpp

Revision:

1:40bdbe1a93b7

Parent:

0:dd126a1080d3

Child:

5:864709d3eb76

--- a/FreeFlyerHardware.cpp	Tue Feb 14 05:12:54 2017 +0000
+++ b/FreeFlyerHardware.cpp	Fri Jun 22 02:09:50 2018 +0000
@@ -1,8 +1,10 @@
 
 #include "FreeFlyerHardware.h"
 
-FreeFlyerHardware::FreeFlyerHardware(ros::NodeHandle &nh, DigitalOut *thruster_pinouts)
-: thruster_pwm_clock_(0)
+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;
@@ -15,61 +17,72 @@
     }
     
     // Init PID controller
-    controller_ = new PID(KP, KP/KI, KD/KP, PID_PERIOD/1000.0); //Kc, Ti, Td, interval
+    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(0, SCALED_MAX);
+    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);
+    controller_->setAccLimit(ACC_LIMIT_INIT);
     controller_->setSetPoint(INITIAL_SP);
     
     // Init encoder data processor
-    wheel_encoder_ = new QEI(p17, p18, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
+    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/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_);
+    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(*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_);
+    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_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);
+    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) {
-    
-    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);
+    controller_->setSetPoint(msg.data);
 }
 
-void FreeFlyerHardware::wheelPidGainsCmdCallback(const std_msgs::Float32MultiArray& msg) {
-    
+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) {
@@ -78,49 +91,73 @@
 }
 
 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_);
+    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_);
+    }  
 }
 
-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::getPWMOut() {
+    return pwm_out_;
 }
 
-float FreeFlyerHardware::getVoltageOut() {
-    return voltage_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;
+            thruster_pinouts_[i] = 1;
         else
-            thruster_pinouts_[i] == 0;
+            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_)
+        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_);
 }
\ No newline at end of file