Alison Bartsch / 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@5:864709d3eb76, 2018-06-29 (annotated)

Committer:
ambyld
Date:
Fri Jun 29 17:49:40 2018 +0000
Revision:
5:864709d3eb76
Parent:
1:40bdbe1a93b7 
Moved isolated debug scripts to separate header file

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);
Knillinux 1:40bdbe1a93b7 48
Knillinux 0:dd126a1080d3 49 velocity_sns_pub_ = new ros::Publisher("wheel/sensors/velocity", &velocity_sns_msg_);
Knillinux 1:40bdbe1a93b7 50 setpoint_pub_ = new ros::Publisher("wheel/setpoint", &setpoint_msg_);
Knillinux 1:40bdbe1a93b7 51 acc_error_pub_ = new ros::Publisher("wheel/acc_error", &acc_error_msg_);
Knillinux 1:40bdbe1a93b7 52 pid_param_pub_ = new ros::Publisher("wheel/pid_params", &pid_param_msg_);
Knillinux 0:dd126a1080d3 53
Knillinux 0:dd126a1080d3 54 root_nh_->advertise(*velocity_sns_pub_);
Knillinux 0:dd126a1080d3 55 root_nh_->advertise(*setpoint_pub_);
Knillinux 1:40bdbe1a93b7 56 root_nh_->advertise(*acc_error_pub_);
Knillinux 1:40bdbe1a93b7 57 root_nh_->advertise(*pid_param_pub_);
Knillinux 0:dd126a1080d3 58
Knillinux 0:dd126a1080d3 59 wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
Knillinux 1:40bdbe1a93b7 60 wheel_duty_cycle_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/duty_cycle", &FreeFlyerHardware::wheelDutyCycleCmdCallback, this);
Knillinux 1:40bdbe1a93b7 61 wheel_pid_params_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("wheel/commands/pid_params", &FreeFlyerHardware::wheelPidParamsCmdCallback, this);
Knillinux 1:40bdbe1a93b7 62 thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("thruster/commands/duty_cycle", &FreeFlyerHardware::thrusterDutyCmdCallback, this);
Knillinux 1:40bdbe1a93b7 63
Knillinux 1:40bdbe1a93b7 64 root_nh_->subscribe(*wheel_vel_cmd_sub_);
Knillinux 1:40bdbe1a93b7 65 root_nh_->subscribe(*wheel_pid_params_cmd_sub_);
Knillinux 1:40bdbe1a93b7 66 root_nh_->subscribe(*thruster_duty_cmd_sub_);
Knillinux 1:40bdbe1a93b7 67 if (duty_cycle_command_mode_)
Knillinux 1:40bdbe1a93b7 68 root_nh_->subscribe(*wheel_duty_cycle_cmd_sub_);
Knillinux 0:dd126a1080d3 69 }
Knillinux 0:dd126a1080d3 70
Knillinux 0:dd126a1080d3 71 void FreeFlyerHardware::wheelVelCmdCallback(const std_msgs::Float32& msg) {
Knillinux 1:40bdbe1a93b7 72 controller_->setSetPoint(msg.data);
Knillinux 0:dd126a1080d3 73 }
Knillinux 0:dd126a1080d3 74
Knillinux 1:40bdbe1a93b7 75 void FreeFlyerHardware::wheelDutyCycleCmdCallback(const std_msgs::Float32& msg) {
Knillinux 1:40bdbe1a93b7 76 duty_cycle_cmd_ = msg.data;
Knillinux 1:40bdbe1a93b7 77 }
Knillinux 1:40bdbe1a93b7 78
Knillinux 1:40bdbe1a93b7 79 void FreeFlyerHardware::wheelPidParamsCmdCallback(const std_msgs::Float32MultiArray& msg) {
Knillinux 0:dd126a1080d3 80 float kp_new = msg.data[0];
Knillinux 0:dd126a1080d3 81 float ki_new = msg.data[1];
Knillinux 0:dd126a1080d3 82 float kd_new = msg.data[2];
Knillinux 0:dd126a1080d3 83
Knillinux 0:dd126a1080d3 84 controller_->setTunings(kp_new, kp_new/ki_new, kd_new/kp_new);
Knillinux 1:40bdbe1a93b7 85 controller_->setAccLimit(msg.data[3]);
Knillinux 0:dd126a1080d3 86 }
Knillinux 0:dd126a1080d3 87
Knillinux 0:dd126a1080d3 88 void FreeFlyerHardware::thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg) {
Knillinux 0:dd126a1080d3 89 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 0:dd126a1080d3 90 thruster_pwm_stop_[i] = ceil(msg.data[i]*THRUST_PWM_N);
Knillinux 0:dd126a1080d3 91 }
Knillinux 0:dd126a1080d3 92
Knillinux 0:dd126a1080d3 93 void FreeFlyerHardware::updatePID() {
Knillinux 1:40bdbe1a93b7 94 if (feed_forward_mode_) {
Knillinux 1:40bdbe1a93b7 95 // Apply measured feed-forward term and perform PID for small adjustments on top of that
ambyld 5:864709d3eb76 96 shaft_vel_meas_ = getWheelVelocity();
Knillinux 1:40bdbe1a93b7 97 controller_->setProcessValue(shaft_vel_meas_);
Knillinux 1:40bdbe1a93b7 98 pwm_out_ = utils::smooth(slope_ff_*controller_->getSetPoint() + inter_ff_ + controller_->compute(), SMOOTHING_VAL, pwm_out_);
Knillinux 1:40bdbe1a93b7 99
Knillinux 1:40bdbe1a93b7 100 } else {
Knillinux 1:40bdbe1a93b7 101 // Directly apply PID for all control of the reaction wheel speed
ambyld 5:864709d3eb76 102 shaft_vel_meas_ = getWheelVelocity();
Knillinux 1:40bdbe1a93b7 103 controller_->setProcessValue(shaft_vel_meas_);
Knillinux 1:40bdbe1a93b7 104 current_out_ = controller_->compute(); // Desired current
Knillinux 1:40bdbe1a93b7 105 voltage_out_ = utils::smooth(R*current_out_ + KE*shaft_vel_meas_*NGR, SMOOTHING_VAL, voltage_out_);
Knillinux 1:40bdbe1a93b7 106 pwm_out_ = voltage_out_/VOLTAGE_MAX;
Knillinux 1:40bdbe1a93b7 107 }
Knillinux 1:40bdbe1a93b7 108
Knillinux 1:40bdbe1a93b7 109 if (debug_flag_) {
Knillinux 1:40bdbe1a93b7 110 setpoint_msg_.data = controller_->getSetPoint(); // Speed setpoint
Knillinux 1:40bdbe1a93b7 111 acc_error_msg_.data = controller_->getAccError(); // Accumulated error
Knillinux 1:40bdbe1a93b7 112
Knillinux 1:40bdbe1a93b7 113 setpoint_pub_->publish(&setpoint_msg_);
Knillinux 1:40bdbe1a93b7 114 acc_error_pub_->publish(&acc_error_msg_);
Knillinux 1:40bdbe1a93b7 115 }
Knillinux 0:dd126a1080d3 116 }
Knillinux 0:dd126a1080d3 117
Knillinux 1:40bdbe1a93b7 118 float FreeFlyerHardware::getPWMOut() {
Knillinux 1:40bdbe1a93b7 119 return pwm_out_;
Knillinux 0:dd126a1080d3 120 }
Knillinux 0:dd126a1080d3 121
Knillinux 1:40bdbe1a93b7 122 void FreeFlyerHardware::setPWMOut(float pwm_out) {
Knillinux 1:40bdbe1a93b7 123 pwm_out_ = pwm_out;
Knillinux 1:40bdbe1a93b7 124 }
Knillinux 1:40bdbe1a93b7 125
Knillinux 1:40bdbe1a93b7 126 void FreeFlyerHardware::setPIDSetpoint(float pid_setpoint) {
Knillinux 1:40bdbe1a93b7 127 controller_->setSetPoint(pid_setpoint);
Knillinux 0:dd126a1080d3 128 }
Knillinux 0:dd126a1080d3 129
ambyld 5:864709d3eb76 130 float FreeFlyerHardware::getWheelVelocity() {
ambyld 5:864709d3eb76 131 return -wheel_encoder_->getSpeed()*COUNTS2SHAFT; // Wheel direction convention is opposite from shaft direction convention
ambyld 5:864709d3eb76 132 }
ambyld 5:864709d3eb76 133
Knillinux 0:dd126a1080d3 134 void FreeFlyerHardware::commandThrusters(int *thruster_on_off_cmd) {
Knillinux 0:dd126a1080d3 135 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 0:dd126a1080d3 136 if (thruster_on_off_cmd[i] == 1)
Knillinux 1:40bdbe1a93b7 137 thruster_pinouts_[i] = 1;
Knillinux 0:dd126a1080d3 138 else
Knillinux 1:40bdbe1a93b7 139 thruster_pinouts_[i] = 0;
Knillinux 0:dd126a1080d3 140 }
Knillinux 0:dd126a1080d3 141
Knillinux 0:dd126a1080d3 142 void FreeFlyerHardware::stepThrusterPWM() {
Knillinux 0:dd126a1080d3 143 thruster_pwm_clock_++;
Knillinux 0:dd126a1080d3 144
Knillinux 0:dd126a1080d3 145 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 1:40bdbe1a93b7 146 if (thruster_pwm_stop_[i] > thruster_pwm_clock_)
Knillinux 0:dd126a1080d3 147 thruster_pinouts_[i] = 1;
Knillinux 0:dd126a1080d3 148 else
Knillinux 0:dd126a1080d3 149 thruster_pinouts_[i] = 0;
Knillinux 0:dd126a1080d3 150
Knillinux 0:dd126a1080d3 151 if (thruster_pwm_clock_ == THRUST_PWM_N)
Knillinux 0:dd126a1080d3 152 thruster_pwm_clock_ = 0;
Knillinux 1:40bdbe1a93b7 153 }
Knillinux 1:40bdbe1a93b7 154
Knillinux 1:40bdbe1a93b7 155 void FreeFlyerHardware::publishWheelMeas() {
ambyld 5:864709d3eb76 156 velocity_sns_msg_.data = getWheelVelocity(); // Publish measured shaft speed in RPM
Knillinux 1:40bdbe1a93b7 157 velocity_sns_pub_->publish(&velocity_sns_msg_);
Knillinux 1:40bdbe1a93b7 158 }
Knillinux 1:40bdbe1a93b7 159
Knillinux 1:40bdbe1a93b7 160 void FreeFlyerHardware::publishPIDParam() {
Knillinux 1:40bdbe1a93b7 161 pid_param_msg_.data[0] = controller_->getPParam();
Knillinux 1:40bdbe1a93b7 162 pid_param_msg_.data[1] = controller_->getPParam()/controller_->getIParam();
Knillinux 1:40bdbe1a93b7 163 pid_param_msg_.data[2] = controller_->getPParam()*controller_->getDParam();
Knillinux 1:40bdbe1a93b7 164 pid_param_msg_.data[3] = controller_->getAccLimit();
Knillinux 1:40bdbe1a93b7 165
Knillinux 1:40bdbe1a93b7 166 pid_param_pub_->publish(&pid_param_msg_);
Knillinux 0:dd126a1080d3 167 }