From 041ff7bded1a262016afbabdc73e4a8c6384ea5d Mon Sep 17 00:00:00 2001 From: Jacob Gloudemans Date: Wed, 16 Jan 2019 19:53:21 -0600 Subject: [PATCH] WIP #3: Test to drive one wheel --- motor_control/src/can_bus_interface.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/motor_control/src/can_bus_interface.cpp b/motor_control/src/can_bus_interface.cpp index c26f446..b93fe18 100644 --- a/motor_control/src/can_bus_interface.cpp +++ b/motor_control/src/can_bus_interface.cpp @@ -30,6 +30,14 @@ int main(int argc, char **argv) { // Create a Talon object ctre::phoenix::motorcontrol::can::TalonSRX fl(1); + // Wait for CAN bus set up to complete + ros::Duration setup_time(2); + setup_time.sleep(); + + // Drive wheel + fl.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 20); + + // Rate to loop at (in Hz) ros::Rate loop_rate(1); // Loop