diff --git a/jetson_setup/gs_usb.ko.4.4.38-tegra-patched b/jetson_setup/gs_usb.ko.4.4.38-tegra-patched new file mode 100644 index 00000000..805a3b4f Binary files /dev/null and b/jetson_setup/gs_usb.ko.4.4.38-tegra-patched differ diff --git a/zebROS_ws/src/ros_control_boilerplate/src/frcrobot_hw_interface.cpp b/zebROS_ws/src/ros_control_boilerplate/src/frcrobot_hw_interface.cpp index 25d9ed52..4da3ad5a 100644 --- a/zebROS_ws/src/ros_control_boilerplate/src/frcrobot_hw_interface.cpp +++ b/zebROS_ws/src/ros_control_boilerplate/src/frcrobot_hw_interface.cpp @@ -2174,6 +2174,15 @@ void FRCRobotHWInterface::write(ros::Duration &elapsed_time) ts.setDemand1Type(demand1_type_internal); ts.setDemand1Value(demand1_value); + // I think motionmagic calculates a path from currrent setpoint to new setpoint + // If we didn't actually make it to the previous setpoint, this will cause + // problems with random accumulating error. It looks like going from disabled + // back to motionmagic causes the talon to path from current encoder value + // to new setpoint. Temporarily disable the talon before sending a new setpoint + // in motionmagic mode to trigger this behavior. + if (b2 && (in_mode == hardware_interface::TalonMode_MotionMagic)) + talon->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, + command, demand1_type_phoenix, demand1_value); talon->Set(out_mode, command, demand1_type_phoenix, demand1_value); } }