Thomas Lew / Mbed 2 deprecated FreeFlyerROS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_kinetic

FreeFlyerHardware.cpp@7:794bd40830c1, 2021-03-02 (annotated)

Committer:
thomasjlew
Date:
Tue Mar 02 22:49:24 2021 +0000
Revision:
7:794bd40830c1
Parent:
5:864709d3eb76 
Embedded code for RSS 2021 Safe Active Learning submission - increased control frequency / reduced PWM duty cycle.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Knillinux 0:dd126a1080d3 1
Knillinux 0:dd126a1080d3 2 #include "FreeFlyerHardware.h"
Knillinux 0:dd126a1080d3 3
Knillinux 1:40bdbe1a93b7 4 FreeFlyerHardware::FreeFlyerHardware(ros::NodeHandle &nh, I2C *i2c, DigitalOut *thruster_pinouts,
Knillinux 1:40bdbe1a93b7 5 DigitalOut *led_inv_out_en, bool debug_flag)
Knillinux 1:40bdbe1a93b7 6 : thruster_pwm_clock_(0), shaft_vel_meas_(0), pwm_out_(0), slope_ff_(SLOPE_FF_INIT), inter_ff_(INTER_FF_INIT),
Knillinux 1:40bdbe1a93b7 7 debug_flag_(debug_flag), duty_cycle_command_mode_(MODE_DUTY_CYCLE_CMD), feed_forward_mode_(MODE_FEED_FORWARD)
Knillinux 0:dd126a1080d3 8 {
Knillinux 0:dd126a1080d3 9 // Get params
Knillinux 0:dd126a1080d3 10 NUM_THRUSTERS = N_THRUSTERS;
Knillinux 0:dd126a1080d3 11
Knillinux 0:dd126a1080d3 12 // Init thruster pinouts (set all to 0)
Knillinux 0:dd126a1080d3 13 thruster_pinouts_ = thruster_pinouts;
Knillinux 0:dd126a1080d3 14 for (int i = 0; i < NUM_THRUSTERS; i++) {
Knillinux 0:dd126a1080d3 15 thruster_pinouts_[i] = 0;
Knillinux 0:dd126a1080d3 16 thruster_pwm_stop_[i] = 0;
Knillinux 0:dd126a1080d3 17 }
Knillinux 0:dd126a1080d3 18
Knillinux 0:dd126a1080d3 19 // Init PID controller
Knillinux 1:40bdbe1a93b7 20 if (feed_forward_mode_)
Knillinux 1:40bdbe1a93b7 21 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
Knillinux 1:40bdbe1a93b7 22 else
Knillinux 1:40bdbe1a93b7 23 controller_ = new PID(KP_INIT, KP_INIT/KI_INIT, KD_INIT/KP_INIT, PID_PERIOD/1000.0); //Kc, Ti, Td, interval
Knillinux 1:40bdbe1a93b7 24
Knillinux 0:dd126a1080d3 25 controller_->setInterval(PID_PERIOD);
Knillinux 1:40bdbe1a93b7 26 controller_->setInputLimits(V_SMIN_ENF, V_SMAX_ENF); // In terms of motor speed
Knillinux 0:dd126a1080d3 27 controller_->setOutputLimits(-CURRENT_MAX, CURRENT_MAX);
Knillinux 0:dd126a1080d3 28 controller_->setBias(0.0);
Knillinux 1:40bdbe1a93b7 29 controller_->setAccLimit(ACC_LIMIT_INIT);
Knillinux 0:dd126a1080d3 30 controller_->setSetPoint(INITIAL_SP);
Knillinux 0:dd126a1080d3 31
Knillinux 0:dd126a1080d3 32 // Init encoder data processor
Knillinux 1:40bdbe1a93b7 33 wheel_encoder_ = new QEI(PIN_WENCA, PIN_WENCB, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
Knillinux 1:40bdbe1a93b7 34
Knillinux 1:40bdbe1a93b7 35 // Init RGBA LED driver
Knillinux 1:40bdbe1a93b7 36 rgba_led_ = new RGBA_LED(i2c, led_inv_out_en, ADDR_RGB, MODE_AMBER);
Knillinux 0:dd126a1080d3 37
Knillinux 0:dd126a1080d3 38 // ROS
Knillinux 0:dd126a1080d3 39 root_nh_ = &nh;
Knillinux 0:dd126a1080d3 40
Knillinux 0:dd126a1080d3 41 // Init pub/sub
Knillinux 1:40bdbe1a93b7 42 pid_param_msg_.layout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension));
Knillinux 1:40bdbe1a93b7 43 pid_param_msg_.layout.dim[0].size = 4;
Knillinux 1:40bdbe1a93b7 44 pid_param_msg_.layout.dim[0].stride = 1*4;
Knillinux 1:40bdbe1a93b7 45 pid_param_msg_.layout.data_offset = 0;
Knillinux 1:40bdbe1a93b7 46 pid_param_msg_.data_length = 4;
Knillinux 1:40bdbe1a93b7 47 pid_param_msg_.data = (float *) malloc(sizeof(float) * 4);
thomasjlew 7:794bd40830c1 48 //
thomasjlew 7:794bd40830c1 49 // thruster_pwm_msg_.layout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension));
thomasjlew 7:794bd40830c1 50 // thruster_pwm_msg_.layout.dim[0].size = 8;
thomasjlew 7:794bd40830c1 51 // thruster_pwm_msg_.layout.dim[0].stride = 1*8;
thomasjlew 7:794bd40830c1 52 // thruster_pwm_msg_.layout.data_offset = 0;
thomasjlew 7:794bd40830c1 53 // thruster_pwm_msg_.data_length = 8;
thomasjlew 7:794bd40830c1 54 // thruster_pwm_msg_.data = (float *) malloc(sizeof(float) * 8);
Knillinux 1:40bdbe1a93b7 55
thomasjlew 7:794bd40830c1 56 velocity_sns_pub_ = new ros::Publisher("/mbed/wheel/sensors/velocity", &velocity_sns_msg_);
thomasjlew 7:794bd40830c1 57 setpoint_pub_ = new ros::Publisher("/mbed/wheel/setpoint", &setpoint_msg_);
thomasjlew 7:794bd40830c1 58 acc_error_pub_ = new ros::Publisher("/mbed/wheel/acc_error", &acc_error_msg_);
thomasjlew 7:794bd40830c1 59 pid_param_pub_ = new ros::Publisher("/mbed/wheel/pid_params", &pid_param_msg_);
thomasjlew 7:794bd40830c1 60 // thrusters_pub_ = new ros::Publisher("/mbed/thruster/commands/duty_cycle", &thruster_pwm_msg_);
Knillinux 0:dd126a1080d3 61
Knillinux 0:dd126a1080d3 62 root_nh_->advertise(*velocity_sns_pub_);
Knillinux 0:dd126a1080d3 63 root_nh_->advertise(*setpoint_pub_);
Knillinux 1:40bdbe1a93b7 64 root_nh_->advertise(*acc_error_pub_);
Knillinux 1:40bdbe1a93b7 65 root_nh_->advertise(*pid_param_pub_);
Knillinux 0:dd126a1080d3 66
thomasjlew 7:794bd40830c1 67 wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
thomasjlew 7:794bd40830c1 68 wheel_duty_cycle_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/duty_cycle", &FreeFlyerHardware::wheelDutyCycleCmdCallback, this);
thomasjlew 7:794bd40830c1 69 wheel_pid_params_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/pid_params", &FreeFlyerHardware::wheelPidParamsCmdCallback, this);
thomasjlew 7:794bd40830c1 70 thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("/free_flyer/enterprise/thruster/commands/duty_cycle", &FreeFlyerHardware::thrusterDutyCmdCallback, this);
Knillinux 1:40bdbe1a93b7 71
Knillinux 1:40bdbe1a93b7 72 root_nh_->subscribe(*wheel_vel_cmd_sub_);
Knillinux 1:40bdbe1a93b7 73 root_nh_->subscribe(*wheel_pid_params_cmd_sub_);
Knillinux 1:40bdbe1a93b7 74 root_nh_->subscribe(*thruster_duty_cmd_sub_);
Knillinux 1:40bdbe1a93b7 75 if (duty_cycle_command_mode_)
Knillinux 1:40bdbe1a93b7 76 root_nh_->subscribe(*wheel_duty_cycle_cmd_sub_);
Knillinux 0:dd126a1080d3 77 }
Knillinux 0:dd126a1080d3 78
Knillinux 0:dd126a1080d3 79 void FreeFlyerHardware::wheelVelCmdCallback(const std_msgs::Float32& msg) {
Knillinux 1:40bdbe1a93b7 80 controller_->setSetPoint(msg.data);
Knillinux 0:dd126a1080d3 81 }
Knillinux 0:dd126a1080d3 82
Knillinux 1:40bdbe1a93b7 83 void FreeFlyerHardware::wheelDutyCycleCmdCallback(const std_msgs::Float32& msg) {
Knillinux 1:40bdbe1a93b7 84 duty_cycle_cmd_ = msg.data;
Knillinux 1:40bdbe1a93b7 85 }
Knillinux 1:40bdbe1a93b7 86
Knillinux 1:40bdbe1a93b7 87 void FreeFlyerHardware::wheelPidParamsCmdCallback(const std_msgs::Float32MultiArray& msg) {
Knillinux 0:dd126a1080d3 88 float kp_new = msg.data[0];
Knillinux 0:dd126a1080d3 89 float ki_new = msg.data[1];
Knillinux 0:dd126a1080d3 90 float kd_new = msg.data[2];
Knillinux 0:dd126a1080d3 91
Knillinux 0:dd126a1080d3 92 controller_->setTunings(kp_new, kp_new/ki_new, kd_new/kp_new);
Knillinux 1:40bdbe1a93b7 93 controller_->setAccLimit(msg.data[3]);
Knillinux 0:dd126a1080d3 94 }
Knillinux 0:dd126a1080d3 95
Knillinux 0:dd126a1080d3 96 void FreeFlyerHardware::thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg) {
Knillinux 0:dd126a1080d3 97 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 0:dd126a1080d3 98 thruster_pwm_stop_[i] = ceil(msg.data[i]*THRUST_PWM_N);
thomasjlew 7:794bd40830c1 99
thomasjlew 7:794bd40830c1 100 // debug republish thruster//s commands
thomasjlew 7:794bd40830c1 101 // for (int i = 0; i < NUM_THRUSTERS; i++)
thomasjlew 7:794bd40830c1 102 // thruster_pwm_msg_.data[i] = msg.data[i];
thomasjlew 7:794bd40830c1 103 // thrusters_pub_->publish(&thruster_pwm_msg_);
Knillinux 0:dd126a1080d3 104 }
Knillinux 0:dd126a1080d3 105
Knillinux 0:dd126a1080d3 106 void FreeFlyerHardware::updatePID() {
Knillinux 1:40bdbe1a93b7 107 if (feed_forward_mode_) {
Knillinux 1:40bdbe1a93b7 108 // Apply measured feed-forward term and perform PID for small adjustments on top of that
ambyld 5:864709d3eb76 109 shaft_vel_meas_ = getWheelVelocity();
Knillinux 1:40bdbe1a93b7 110 controller_->setProcessValue(shaft_vel_meas_);
Knillinux 1:40bdbe1a93b7 111 pwm_out_ = utils::smooth(slope_ff_*controller_->getSetPoint() + inter_ff_ + controller_->compute(), SMOOTHING_VAL, pwm_out_);
Knillinux 1:40bdbe1a93b7 112
Knillinux 1:40bdbe1a93b7 113 } else {
Knillinux 1:40bdbe1a93b7 114 // Directly apply PID for all control of the reaction wheel speed
ambyld 5:864709d3eb76 115 shaft_vel_meas_ = getWheelVelocity();
Knillinux 1:40bdbe1a93b7 116 controller_->setProcessValue(shaft_vel_meas_);
Knillinux 1:40bdbe1a93b7 117 current_out_ = controller_->compute(); // Desired current
Knillinux 1:40bdbe1a93b7 118 voltage_out_ = utils::smooth(R*current_out_ + KE*shaft_vel_meas_*NGR, SMOOTHING_VAL, voltage_out_);
Knillinux 1:40bdbe1a93b7 119 pwm_out_ = voltage_out_/VOLTAGE_MAX;
Knillinux 1:40bdbe1a93b7 120 }
Knillinux 1:40bdbe1a93b7 121
Knillinux 1:40bdbe1a93b7 122 if (debug_flag_) {
Knillinux 1:40bdbe1a93b7 123 setpoint_msg_.data = controller_->getSetPoint(); // Speed setpoint
Knillinux 1:40bdbe1a93b7 124 acc_error_msg_.data = controller_->getAccError(); // Accumulated error
Knillinux 1:40bdbe1a93b7 125
Knillinux 1:40bdbe1a93b7 126 setpoint_pub_->publish(&setpoint_msg_);
Knillinux 1:40bdbe1a93b7 127 acc_error_pub_->publish(&acc_error_msg_);
Knillinux 1:40bdbe1a93b7 128 }
Knillinux 0:dd126a1080d3 129 }
Knillinux 0:dd126a1080d3 130
Knillinux 1:40bdbe1a93b7 131 float FreeFlyerHardware::getPWMOut() {
Knillinux 1:40bdbe1a93b7 132 return pwm_out_;
Knillinux 0:dd126a1080d3 133 }
Knillinux 0:dd126a1080d3 134
Knillinux 1:40bdbe1a93b7 135 void FreeFlyerHardware::setPWMOut(float pwm_out) {
Knillinux 1:40bdbe1a93b7 136 pwm_out_ = pwm_out;
Knillinux 1:40bdbe1a93b7 137 }
Knillinux 1:40bdbe1a93b7 138
Knillinux 1:40bdbe1a93b7 139 void FreeFlyerHardware::setPIDSetpoint(float pid_setpoint) {
Knillinux 1:40bdbe1a93b7 140 controller_->setSetPoint(pid_setpoint);
Knillinux 0:dd126a1080d3 141 }
Knillinux 0:dd126a1080d3 142
ambyld 5:864709d3eb76 143 float FreeFlyerHardware::getWheelVelocity() {
ambyld 5:864709d3eb76 144 return -wheel_encoder_->getSpeed()*COUNTS2SHAFT; // Wheel direction convention is opposite from shaft direction convention
ambyld 5:864709d3eb76 145 }
ambyld 5:864709d3eb76 146
Knillinux 0:dd126a1080d3 147 void FreeFlyerHardware::commandThrusters(int *thruster_on_off_cmd) {
Knillinux 0:dd126a1080d3 148 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 0:dd126a1080d3 149 if (thruster_on_off_cmd[i] == 1)
Knillinux 1:40bdbe1a93b7 150 thruster_pinouts_[i] = 1;
Knillinux 0:dd126a1080d3 151 else
Knillinux 1:40bdbe1a93b7 152 thruster_pinouts_[i] = 0;
Knillinux 0:dd126a1080d3 153 }
Knillinux 0:dd126a1080d3 154
Knillinux 0:dd126a1080d3 155 void FreeFlyerHardware::stepThrusterPWM() {
Knillinux 0:dd126a1080d3 156 thruster_pwm_clock_++;
Knillinux 0:dd126a1080d3 157
Knillinux 0:dd126a1080d3 158 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 1:40bdbe1a93b7 159 if (thruster_pwm_stop_[i] > thruster_pwm_clock_)
Knillinux 0:dd126a1080d3 160 thruster_pinouts_[i] = 1;
Knillinux 0:dd126a1080d3 161 else
Knillinux 0:dd126a1080d3 162 thruster_pinouts_[i] = 0;
Knillinux 0:dd126a1080d3 163
Knillinux 0:dd126a1080d3 164 if (thruster_pwm_clock_ == THRUST_PWM_N)
Knillinux 0:dd126a1080d3 165 thruster_pwm_clock_ = 0;
Knillinux 1:40bdbe1a93b7 166 }
Knillinux 1:40bdbe1a93b7 167
Knillinux 1:40bdbe1a93b7 168 void FreeFlyerHardware::publishWheelMeas() {
ambyld 5:864709d3eb76 169 velocity_sns_msg_.data = getWheelVelocity(); // Publish measured shaft speed in RPM
Knillinux 1:40bdbe1a93b7 170 velocity_sns_pub_->publish(&velocity_sns_msg_);
Knillinux 1:40bdbe1a93b7 171 }
Knillinux 1:40bdbe1a93b7 172
Knillinux 1:40bdbe1a93b7 173 void FreeFlyerHardware::publishPIDParam() {
Knillinux 1:40bdbe1a93b7 174 pid_param_msg_.data[0] = controller_->getPParam();
Knillinux 1:40bdbe1a93b7 175 pid_param_msg_.data[1] = controller_->getPParam()/controller_->getIParam();
Knillinux 1:40bdbe1a93b7 176 pid_param_msg_.data[2] = controller_->getPParam()*controller_->getDParam();
Knillinux 1:40bdbe1a93b7 177 pid_param_msg_.data[3] = controller_->getAccLimit();
Knillinux 1:40bdbe1a93b7 178
Knillinux 1:40bdbe1a93b7 179 pid_param_pub_->publish(&pid_param_msg_);
Knillinux 0:dd126a1080d3 180 }