From a44be9ff158ff09c11412c6d39f2d7146bbce70b Mon Sep 17 00:00:00 2001 From: NathanDuPont Date: Sat, 6 Feb 2021 17:13:38 -0600 Subject: [PATCH 1/5] Fixed launch file and changed port for V5 --- .../launch/15_inch_robot.launch | 4 ++-- .../firmware/src/DataNodes/DriverControlNode.cpp | 12 +++++++----- src/v5_hal/firmware/src/main.cpp | 2 +- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/change_up_driver_control/launch/15_inch_robot.launch b/src/change_up_driver_control/launch/15_inch_robot.launch index a1a5d1b2e..add4d43ea 100644 --- a/src/change_up_driver_control/launch/15_inch_robot.launch +++ b/src/change_up_driver_control/launch/15_inch_robot.launch @@ -1,6 +1,6 @@ - + @@ -9,7 +9,7 @@ - + diff --git a/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp b/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp index 543ec406f..e228026af 100644 --- a/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp +++ b/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp @@ -21,8 +21,10 @@ DriverControlNode::DriverControlNode(NodeManager* node_manager, MotorNode* left_ rear_module_location(0) = rear_module_location_x; rear_module_location(1) = rear_module_location_y; - // m_navx_sub = new ros::Subscriber - // ("/navx/rpy", &DriverControlNode::m_navxDataCallback, this); + robot_angle = Eigen::Rotation2Dd(0); + + m_navx_sub = new ros::Subscriber + ("/navx/rpy", &DriverControlNode::m_navxDataCallback, this); } void DriverControlNode::initialize() { @@ -30,7 +32,7 @@ void DriverControlNode::initialize() { } void DriverControlNode::m_navxDataCallback(const v5_hal::RollPitchYaw& msg) { - //robot_angle = Eigen::Rotation2Dd(msg.yaw); + robot_angle = Eigen::Rotation2Dd(msg.yaw); Node::m_handle->loginfo("Message Recieved"); } @@ -65,8 +67,8 @@ void DriverControlNode::m_spinEjectionRollerVoltage(int voltage) { void DriverControlNode::periodic() { // printf("Inertial Sensor Yaw: %f\n", inertial_sensor->getYaw()); - double sensor_yaw = inertial_sensor->getYaw(); - robot_angle = Eigen::Rotation2Dd(-sensor_yaw); + // double sensor_yaw = inertial_sensor->getYaw(); + // robot_angle = Eigen::Rotation2Dd(-sensor_yaw); controller_target_velocity(0) = (controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_X) / 127.0) * max_velocity; controller_target_velocity(1) = (controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y) / 127.0) * max_velocity; diff --git a/src/v5_hal/firmware/src/main.cpp b/src/v5_hal/firmware/src/main.cpp index 193626b8b..5ca811c80 100644 --- a/src/v5_hal/firmware/src/main.cpp +++ b/src/v5_hal/firmware/src/main.cpp @@ -59,7 +59,7 @@ void initialize() { ejection_roller = new MotorNode(node_manager, 15, "ejectionRoller", false); top_rollers = new MotorNode(node_manager, 7, "topRollers", true); - inertial_sensor = new InertialSensorNode(node_manager, 9, "inertialSensor"); + //inertial_sensor = new InertialSensorNode(node_manager, 9, "inertialSensor"); x_odometry_encoder = new ADIEncoderNode(node_manager, 1, 2, "xOdometryEncoder", false); y_odometry_encoder = new ADIEncoderNode(node_manager, 3, 4, "yOdometryEncoder", false); From 440693624486cfc605770df59eeeb2bbb9dd0949 Mon Sep 17 00:00:00 2001 From: NathanDuPont Date: Sat, 6 Feb 2021 17:48:50 -0600 Subject: [PATCH 2/5] Changed source for RPY data --- src/navx_publisher/src/navx_publisher.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/navx_publisher/src/navx_publisher.cpp b/src/navx_publisher/src/navx_publisher.cpp index f26bf3bae..71226714f 100644 --- a/src/navx_publisher/src/navx_publisher.cpp +++ b/src/navx_publisher/src/navx_publisher.cpp @@ -239,11 +239,8 @@ int main(int argc, char **argv) tf::Matrix3x3(rot).getRPY(roll, pitch, yaw); const double dTime = odom.header.stamp.toSec() - last_time.toSec(); imu_msg.angular_velocity.x = roll / dTime; - imu_rpy_msg.roll = roll; imu_msg.angular_velocity.y = pitch / dTime; - imu_rpy_msg.pitch = pitch; imu_msg.angular_velocity.z = -yaw / dTime; - imu_rpy_msg.yaw = yaw; imu_msg_raw.angular_velocity = imu_msg.angular_velocity; last_rot = pose; last_time = odom.header.stamp; @@ -262,6 +259,11 @@ int main(int argc, char **argv) odom.twist.twist.angular = imu_msg.angular_velocity; + // Gather RPY into custom message + imu_rpy_msg.roll = nx.GetRoll(); + imu_rpy_msg.pitch = nx.GetPitch(); + imu_rpy_msg.yaw = nx.GetYaw(); + //publish to ROS topics time_pub.publish(timestamp); imu_pub.publish(imu_msg); From 060232fd5646c1d66b0c460c0eb0f6bcd38408c6 Mon Sep 17 00:00:00 2001 From: NathanDuPont Date: Sat, 6 Feb 2021 20:41:41 -0600 Subject: [PATCH 3/5] Added code to initialize NavX subscriber --- src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp b/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp index e228026af..7f73aea86 100644 --- a/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp +++ b/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp @@ -28,12 +28,11 @@ DriverControlNode::DriverControlNode(NodeManager* node_manager, MotorNode* left_ } void DriverControlNode::initialize() { - + Node::m_handle->subscribe(*m_navx_sub); } void DriverControlNode::m_navxDataCallback(const v5_hal::RollPitchYaw& msg) { - robot_angle = Eigen::Rotation2Dd(msg.yaw); - Node::m_handle->loginfo("Message Recieved"); + robot_angle = Eigen::Rotation2Dd((msg.yaw * (M_PI/180))); } /** From 981826843f1567e4dcb0ebe0675ddc3214d840f1 Mon Sep 17 00:00:00 2001 From: msoevex Date: Sun, 7 Feb 2021 02:39:50 +0000 Subject: [PATCH 4/5] Added startup file to be run on a cron job on the co-processor (using crontab -e) --- startup/start-ros.sh | 5 +++++ 1 file changed, 5 insertions(+) create mode 100755 startup/start-ros.sh diff --git a/startup/start-ros.sh b/startup/start-ros.sh new file mode 100755 index 000000000..b8a3ccb7c --- /dev/null +++ b/startup/start-ros.sh @@ -0,0 +1,5 @@ +#!/bin/bash +echo "Starting ROS..." +cd /home/ubuntu/ChangeUp +source ./devel/setup.bash +roslaunch change_up_driver_control 15_inch_robot.launch From 7fc40121fa12f3d4ab6432e492d373e04add60aa Mon Sep 17 00:00:00 2001 From: NathanDuPont Date: Sun, 28 Feb 2021 00:29:47 -0600 Subject: [PATCH 5/5] Removing commented out lines --- .../firmware/src/DataNodes/DriverControlNode.cpp | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp b/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp index 7f73aea86..e0dfc5398 100644 --- a/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp +++ b/src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp @@ -64,19 +64,13 @@ void DriverControlNode::m_spinEjectionRollerVoltage(int voltage) { ejection_roller->moveVoltage(voltage); } -void DriverControlNode::periodic() { - // printf("Inertial Sensor Yaw: %f\n", inertial_sensor->getYaw()); - // double sensor_yaw = inertial_sensor->getYaw(); - // robot_angle = Eigen::Rotation2Dd(-sensor_yaw); - +void DriverControlNode::periodic() { controller_target_velocity(0) = (controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_X) / 127.0) * max_velocity; controller_target_velocity(1) = (controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y) / 127.0) * max_velocity; rotation_velocity = -(controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X) / 127.0) * max_rotation_velocity; field_target_velocity = robot_angle.inverse() * controller_target_velocity; - // printf("Field Target Velocity: %f\n", field_target_velocity); - swerveController.assignActualAngle(left_swerve_pot->getValue(), right_swerve_pot->getValue(), rear_swerve_pot->getValue()); MotorPowers left_motor_powers = swerveController.calculateLeftModule(field_target_velocity, rotation_velocity); @@ -90,13 +84,6 @@ void DriverControlNode::periodic() { rear_swerve_1->move(rear_motor_powers.left_motor_power); rear_swerve_2->move(rear_motor_powers.right_motor_power); - // printf("Left Motor 1 Power: %d\n", left_motor_powers.left_motor_power); - // printf("Left Motor 2 Power: %d\n", left_motor_powers.right_motor_power); - // printf("Right Motor 1 Power: %d\n", right_motor_powers.left_motor_power); - // printf("Right Motor 2 Power: %d\n", right_motor_powers.right_motor_power); - // printf("Rear Motor 1 Power: %d\n", rear_motor_powers.left_motor_power); - // printf("Rear Motor 2 Power: %d\n", rear_motor_powers.right_motor_power); - int intake_voltage = 0; int main_rollers_voltage = 0; int ejection_roller_voltage = 0;