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@0:dd126a1080d3, 2017-02-14 (annotated)

Committer:
Knillinux
Date:
Tue Feb 14 05:12:54 2017 +0000
Revision:
0:dd126a1080d3
Child:
1:40bdbe1a93b7
test

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 0:dd126a1080d3 4 FreeFlyerHardware::FreeFlyerHardware(ros::NodeHandle &nh, DigitalOut *thruster_pinouts)
Knillinux 0:dd126a1080d3 5 : thruster_pwm_clock_(0)
Knillinux 0:dd126a1080d3 6 {
Knillinux 0:dd126a1080d3 7 // Get params
Knillinux 0:dd126a1080d3 8 NUM_THRUSTERS = N_THRUSTERS;
Knillinux 0:dd126a1080d3 9
Knillinux 0:dd126a1080d3 10 // Init thruster pinouts (set all to 0)
Knillinux 0:dd126a1080d3 11 thruster_pinouts_ = thruster_pinouts;
Knillinux 0:dd126a1080d3 12 for (int i = 0; i < NUM_THRUSTERS; i++) {
Knillinux 0:dd126a1080d3 13 thruster_pinouts_[i] = 0;
Knillinux 0:dd126a1080d3 14 thruster_pwm_stop_[i] = 0;
Knillinux 0:dd126a1080d3 15 }
Knillinux 0:dd126a1080d3 16
Knillinux 0:dd126a1080d3 17 // Init PID controller
Knillinux 0:dd126a1080d3 18 controller_ = new PID(KP, KP/KI, KD/KP, PID_PERIOD/1000.0); //Kc, Ti, Td, interval
Knillinux 0:dd126a1080d3 19 controller_->setInterval(PID_PERIOD);
Knillinux 0:dd126a1080d3 20 controller_->setInputLimits(0, SCALED_MAX);
Knillinux 0:dd126a1080d3 21 controller_->setOutputLimits(-CURRENT_MAX, CURRENT_MAX);
Knillinux 0:dd126a1080d3 22 controller_->setBias(0.0);
Knillinux 0:dd126a1080d3 23 controller_->setAccLimit(ACC_LIMIT);
Knillinux 0:dd126a1080d3 24 controller_->setSetPoint(INITIAL_SP);
Knillinux 0:dd126a1080d3 25
Knillinux 0:dd126a1080d3 26 // Init encoder data processor
Knillinux 0:dd126a1080d3 27 wheel_encoder_ = new QEI(p17, p18, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
Knillinux 0:dd126a1080d3 28
Knillinux 0:dd126a1080d3 29 // ROS
Knillinux 0:dd126a1080d3 30 root_nh_ = &nh;
Knillinux 0:dd126a1080d3 31
Knillinux 0:dd126a1080d3 32 // Init pub/sub
Knillinux 0:dd126a1080d3 33 velocity_sns_pub_ = new ros::Publisher("wheel/sensors/velocity", &velocity_sns_msg_);
Knillinux 0:dd126a1080d3 34 setpoint_pub_ = new ros::Publisher("wheel/debug/setpoint", &setpoint_msg_);
Knillinux 0:dd126a1080d3 35 estimate_pub_ = new ros::Publisher("wheel/debug/estimate", &estimate_msg_);
Knillinux 0:dd126a1080d3 36 error_pub_ = new ros::Publisher("wheel/debug/error", &error_msg_);
Knillinux 0:dd126a1080d3 37 acc_error_pub_ = new ros::Publisher("wheel/debug/acc_error", &acc_error_msg_);
Knillinux 0:dd126a1080d3 38 current_cmd_pub_ = new ros::Publisher("wheel/debug/current_cmd", &current_cmd_msg_);
Knillinux 0:dd126a1080d3 39 duty_cycle_pub_ = new ros::Publisher("wheel/debug/duty_cycle", &duty_cycle_msg_);
Knillinux 0:dd126a1080d3 40
Knillinux 0:dd126a1080d3 41 root_nh_->advertise(*velocity_sns_pub_);
Knillinux 0:dd126a1080d3 42 root_nh_->advertise(*setpoint_pub_);
Knillinux 0:dd126a1080d3 43 root_nh_->advertise(*estimate_pub_);
Knillinux 0:dd126a1080d3 44 root_nh_->advertise(*error_pub_);
Knillinux 0:dd126a1080d3 45 root_nh_->advertise(*acc_error_pub_ );
Knillinux 0:dd126a1080d3 46 root_nh_->advertise(*current_cmd_pub_);
Knillinux 0:dd126a1080d3 47 root_nh_->advertise(*duty_cycle_pub_);
Knillinux 0:dd126a1080d3 48
Knillinux 0:dd126a1080d3 49 wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
Knillinux 0:dd126a1080d3 50 wheel_pid_gains_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("wheel/commands/pid_gains", &FreeFlyerHardware::wheelPidGainsCmdCallback, this);
Knillinux 0:dd126a1080d3 51 thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("wheel/commands/pid_gains", &FreeFlyerHardware::wheelPidGainsCmdCallback, this);
Knillinux 0:dd126a1080d3 52 }
Knillinux 0:dd126a1080d3 53
Knillinux 0:dd126a1080d3 54 void FreeFlyerHardware::wheelVelCmdCallback(const std_msgs::Float32& msg) {
Knillinux 0:dd126a1080d3 55
Knillinux 0:dd126a1080d3 56 float motor_command = msg.data*SCALE_VSHAFT;
Knillinux 0:dd126a1080d3 57
Knillinux 0:dd126a1080d3 58 if ((motor_command >= SCALED_SMIN_ENF) && (motor_command <= SCALED_SMAX_ENF))
Knillinux 0:dd126a1080d3 59 controller_->setSetPoint(motor_command);
Knillinux 0:dd126a1080d3 60 else if (motor_command <= SCALED_SMIN_ENF)
Knillinux 0:dd126a1080d3 61 controller_->setSetPoint(SCALED_SMIN_ENF);
Knillinux 0:dd126a1080d3 62 else if (motor_command >= SCALED_SMAX_ENF)
Knillinux 0:dd126a1080d3 63 controller_->setSetPoint(SCALED_SMAX_ENF);
Knillinux 0:dd126a1080d3 64 }
Knillinux 0:dd126a1080d3 65
Knillinux 0:dd126a1080d3 66 void FreeFlyerHardware::wheelPidGainsCmdCallback(const std_msgs::Float32MultiArray& msg) {
Knillinux 0:dd126a1080d3 67
Knillinux 0:dd126a1080d3 68 float kp_new = msg.data[0];
Knillinux 0:dd126a1080d3 69 float ki_new = msg.data[1];
Knillinux 0:dd126a1080d3 70 float kd_new = msg.data[2];
Knillinux 0:dd126a1080d3 71
Knillinux 0:dd126a1080d3 72 controller_->setTunings(kp_new, kp_new/ki_new, kd_new/kp_new);
Knillinux 0:dd126a1080d3 73 }
Knillinux 0:dd126a1080d3 74
Knillinux 0:dd126a1080d3 75 void FreeFlyerHardware::thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg) {
Knillinux 0:dd126a1080d3 76 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 0:dd126a1080d3 77 thruster_pwm_stop_[i] = ceil(msg.data[i]*THRUST_PWM_N);
Knillinux 0:dd126a1080d3 78 }
Knillinux 0:dd126a1080d3 79
Knillinux 0:dd126a1080d3 80 void FreeFlyerHardware::updatePID() {
Knillinux 0:dd126a1080d3 81 scaled_meas_ = wheel_encoder_->getSpeed()*SCALE_COUNTS;
Knillinux 0:dd126a1080d3 82 controller_->setProcessValue(scaled_meas_);
Knillinux 0:dd126a1080d3 83 current_out_ = controller_->compute(); // Desired current
Knillinux 0:dd126a1080d3 84 voltage_out_ = utils::smooth(R*current_out_ + KE_SCALED*scaled_meas_, SMOOTHING_VAL, voltage_out_);
Knillinux 0:dd126a1080d3 85 }
Knillinux 0:dd126a1080d3 86
Knillinux 0:dd126a1080d3 87 void FreeFlyerHardware::publishDebug() {
Knillinux 0:dd126a1080d3 88 setpoint_msg_.data = controller_->setPoint_; // Speed setpoint
Knillinux 0:dd126a1080d3 89 estimate_msg_.data = scaled_meas_; // Speed estimate
Knillinux 0:dd126a1080d3 90 error_msg_.data = controller_->setPoint_ - scaled_meas_; // Error
Knillinux 0:dd126a1080d3 91 acc_error_msg_.data = controller_->accError_; // AccError
Knillinux 0:dd126a1080d3 92 current_cmd_msg_.data = current_out_; // Current out
Knillinux 0:dd126a1080d3 93 duty_cycle_msg_.data = voltage_out_/VOLTAGE_MAX; // Duty cycle out
Knillinux 0:dd126a1080d3 94
Knillinux 0:dd126a1080d3 95 setpoint_pub_->publish(&setpoint_msg_);
Knillinux 0:dd126a1080d3 96 estimate_pub_->publish(&estimate_msg_);
Knillinux 0:dd126a1080d3 97 error_pub_->publish(&error_msg_);
Knillinux 0:dd126a1080d3 98 acc_error_pub_->publish(&acc_error_msg_);
Knillinux 0:dd126a1080d3 99 current_cmd_pub_->publish(&current_cmd_msg_);
Knillinux 0:dd126a1080d3 100 duty_cycle_pub_->publish(&duty_cycle_msg_);
Knillinux 0:dd126a1080d3 101 }
Knillinux 0:dd126a1080d3 102
Knillinux 0:dd126a1080d3 103 float FreeFlyerHardware::getVoltageOut() {
Knillinux 0:dd126a1080d3 104 return voltage_out_;
Knillinux 0:dd126a1080d3 105 }
Knillinux 0:dd126a1080d3 106
Knillinux 0:dd126a1080d3 107 void FreeFlyerHardware::commandThrusters(int *thruster_on_off_cmd) {
Knillinux 0:dd126a1080d3 108 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 0:dd126a1080d3 109 if (thruster_on_off_cmd[i] == 1)
Knillinux 0:dd126a1080d3 110 thruster_pinouts_[i] == 1;
Knillinux 0:dd126a1080d3 111 else
Knillinux 0:dd126a1080d3 112 thruster_pinouts_[i] == 0;
Knillinux 0:dd126a1080d3 113 }
Knillinux 0:dd126a1080d3 114
Knillinux 0:dd126a1080d3 115 void FreeFlyerHardware::stepThrusterPWM() {
Knillinux 0:dd126a1080d3 116 thruster_pwm_clock_++;
Knillinux 0:dd126a1080d3 117
Knillinux 0:dd126a1080d3 118 for (int i = 0; i < NUM_THRUSTERS; i++)
Knillinux 0:dd126a1080d3 119 if (thruster_pwm_stop_[i] <= thruster_pwm_clock_)
Knillinux 0:dd126a1080d3 120 thruster_pinouts_[i] = 1;
Knillinux 0:dd126a1080d3 121 else
Knillinux 0:dd126a1080d3 122 thruster_pinouts_[i] = 0;
Knillinux 0:dd126a1080d3 123
Knillinux 0:dd126a1080d3 124 if (thruster_pwm_clock_ == THRUST_PWM_N)
Knillinux 0:dd126a1080d3 125 thruster_pwm_clock_ = 0;
Knillinux 0:dd126a1080d3 126 }