Diff: FreeFlyerHardware.cpp

Revision:

5:864709d3eb76

Parent:

1:40bdbe1a93b7

diff -r cae255669971 -r 864709d3eb76 FreeFlyerHardware.cpp
--- a/FreeFlyerHardware.cpp	Fri Jun 29 02:30:38 2018 +0000
+++ b/FreeFlyerHardware.cpp	Fri Jun 29 17:49:40 2018 +0000
@@ -93,13 +93,13 @@
 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;
+        shaft_vel_meas_ = getWheelVelocity();
         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;
+        shaft_vel_meas_ = getWheelVelocity();
         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_);
@@ -127,6 +127,10 @@
     controller_->setSetPoint(pid_setpoint);
 }
 
+float FreeFlyerHardware::getWheelVelocity() {
+    return -wheel_encoder_->getSpeed()*COUNTS2SHAFT; // Wheel direction convention is opposite from shaft direction convention
+}
+
 void FreeFlyerHardware::commandThrusters(int *thruster_on_off_cmd) {
     for (int i = 0; i < NUM_THRUSTERS; i++)
         if (thruster_on_off_cmd[i] == 1)
@@ -149,7 +153,7 @@
 }
 
 void FreeFlyerHardware::publishWheelMeas() {
-    velocity_sns_msg_.data = wheel_encoder_->getSpeed()*COUNTS2SHAFT;   // Publish measured shaft speed in RPM
+    velocity_sns_msg_.data = getWheelVelocity();   // Publish measured shaft speed in RPM
     velocity_sns_pub_->publish(&velocity_sns_msg_);
 }