Revision 7:794bd40830c1, committed 2021-03-02

Comitter:

thomasjlew

Date:

Tue Mar 02 22:49:24 2021 +0000

Parent:

6:7520bf98d660

Commit message:

Embedded code for RSS 2021 Safe Active Learning submission - increased control frequency / reduced PWM duty cycle.

Changed in this revision

--- a/FreeFlyerHardware.cpp	Mon Jul 02 23:38:21 2018 +0000
+++ b/FreeFlyerHardware.cpp	Tue Mar 02 22:49:24 2021 +0000
@@ -45,21 +45,29 @@
     pid_param_msg_.layout.data_offset = 0;
     pid_param_msg_.data_length = 4;
     pid_param_msg_.data = (float *) malloc(sizeof(float) * 4);
+   // 
+//    thruster_pwm_msg_.layout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension));
+//    thruster_pwm_msg_.layout.dim[0].size = 8;
+//    thruster_pwm_msg_.layout.dim[0].stride = 1*8;
+//    thruster_pwm_msg_.layout.data_offset = 0;
+//    thruster_pwm_msg_.data_length = 8;
+//    thruster_pwm_msg_.data = (float *) malloc(sizeof(float) * 8);
     
-    velocity_sns_pub_ = new ros::Publisher("wheel/sensors/velocity", &velocity_sns_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_);
+    velocity_sns_pub_ = new ros::Publisher("/mbed/wheel/sensors/velocity", &velocity_sns_msg_);
+    setpoint_pub_ = new ros::Publisher("/mbed/wheel/setpoint", &setpoint_msg_);
+    acc_error_pub_ = new ros::Publisher("/mbed/wheel/acc_error", &acc_error_msg_);
+    pid_param_pub_ = new ros::Publisher("/mbed/wheel/pid_params", &pid_param_msg_);
+//    thrusters_pub_ = new ros::Publisher("/mbed/thruster/commands/duty_cycle", &thruster_pwm_msg_);
     
     root_nh_->advertise(*velocity_sns_pub_);
     root_nh_->advertise(*setpoint_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_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);
+    wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
+    wheel_duty_cycle_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/duty_cycle", &FreeFlyerHardware::wheelDutyCycleCmdCallback, this);
+    wheel_pid_params_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/pid_params", &FreeFlyerHardware::wheelPidParamsCmdCallback, this);
+    thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("/free_flyer/enterprise/thruster/commands/duty_cycle", &FreeFlyerHardware::thrusterDutyCmdCallback, this);
     
     root_nh_->subscribe(*wheel_vel_cmd_sub_);
     root_nh_->subscribe(*wheel_pid_params_cmd_sub_);
@@ -88,6 +96,11 @@
 void FreeFlyerHardware::thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg) {
     for (int i = 0; i < NUM_THRUSTERS; i++)
         thruster_pwm_stop_[i] = ceil(msg.data[i]*THRUST_PWM_N);
+        
+    // debug republish thruster//s commands
+//    for (int i = 0; i < NUM_THRUSTERS; i++)
+//        thruster_pwm_msg_.data[i] = msg.data[i];
+//    thrusters_pub_->publish(&thruster_pwm_msg_);
 }
 
 void FreeFlyerHardware::updatePID() {

--- a/FreeFlyerHardware.h	Mon Jul 02 23:38:21 2018 +0000
+++ b/FreeFlyerHardware.h	Tue Mar 02 22:49:24 2021 +0000
@@ -58,10 +58,10 @@
     ros::NodeHandle *root_nh_;
     
     ros::Publisher *velocity_sns_pub_, *setpoint_pub_, *acc_error_pub_;
-    ros::Publisher *pid_param_pub_;
+    ros::Publisher *pid_param_pub_, *thrusters_pub_;
     
     std_msgs::Float32 velocity_sns_msg_, setpoint_msg_, acc_error_msg_;
-    std_msgs::Float32MultiArray pid_param_msg_;
+    std_msgs::Float32MultiArray pid_param_msg_;//, thruster_pwm_msg_;
     
     ros::Subscriber<std_msgs::Float32, FreeFlyerHardware> *wheel_vel_cmd_sub_,
                                                           *wheel_duty_cycle_cmd_sub_;

--- a/defines.h	Mon Jul 02 23:38:21 2018 +0000
+++ b/defines.h	Tue Mar 02 22:49:24 2021 +0000
@@ -14,8 +14,8 @@
 #define LED_PERIOD      50.0        // [ms] Period of the blinking LED
 #define MEAS_PERIOD     50.0        // [ms] Period at which to publish new velocity measurement
 #define PID_DEBUG_PERIOD 2500.0     // [ms] Period at which to publish PID parameters
-#define THRUST_PERIOD   500.0       // [ms] PWM period of thrusters
-#define THRUST_PWM_N    20.0        // Number of PWM steps per PID period
+#define THRUST_PERIOD   100.0       // [ms] PWM period of thrusters
+#define THRUST_PWM_N    1000.0        // Number of PWM steps per PID period
 #define MOTOR_PWM_PERIOD    0.00005 // 20 kHz
 
 // Parameters for motor encoder