From f5dfda28f6113b8c774971c56246d92b611cb595 Mon Sep 17 00:00:00 2001 From: sb-msoe Date: Fri, 18 Feb 2022 21:15:25 -0600 Subject: [PATCH] Re-enable auton and implement basic inverse kinematics for the tank drive --- .../firmware/src/auton/auton_routines/BasicAuton.cpp | 11 ++++++----- src/v5_hal/firmware/src/main.cpp | 6 +++--- src/v5_hal/firmware/src/nodes/TankDriveNode.cpp | 6 +++++- .../src/nodes/auton_nodes/AutonManagerNode.cpp | 2 +- 4 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/v5_hal/firmware/src/auton/auton_routines/BasicAuton.cpp b/src/v5_hal/firmware/src/auton/auton_routines/BasicAuton.cpp index b4c7ccb..b4a4628 100644 --- a/src/v5_hal/firmware/src/auton/auton_routines/BasicAuton.cpp +++ b/src/v5_hal/firmware/src/auton/auton_routines/BasicAuton.cpp @@ -8,13 +8,14 @@ BasicAuton::BasicAuton(IDriveNode* drive_node, IClawNode* claw_node) : } void BasicAuton::AddNodes() { - //m_forward_node = new AutonNode(10, new DriveStraightAction(m_drive_node, 1000, 10000, 10000)); + m_forward_node = new AutonNode(10, new DriveStraightAction(m_drive_node, 24, 10000, 10000)); + Auton::AddFirstNode(m_forward_node); - m_claw_close_node = new AutonNode(10, new UseClawAction(m_claw_node, false)); - Auton::AddFirstNode(m_claw_close_node); + //m_claw_close_node = new AutonNode(10, new UseClawAction(m_claw_node, false)); + //Auton::AddFirstNode(m_claw_close_node); //m_forward_node->AddNext(m_claw_close_node); - m_backward_node = new AutonNode(10, new DriveStraightAction(m_drive_node, 800, 10000, 10000)); - m_claw_close_node->AddNext(m_claw_close_node); + //m_backward_node = new AutonNode(10, new DriveStraightAction(m_drive_node, 800, 10000, 10000)); + //m_claw_close_node->AddNext(m_claw_close_node); } \ No newline at end of file diff --git a/src/v5_hal/firmware/src/main.cpp b/src/v5_hal/firmware/src/main.cpp index 3accf62..82469c6 100644 --- a/src/v5_hal/firmware/src/main.cpp +++ b/src/v5_hal/firmware/src/main.cpp @@ -155,9 +155,9 @@ void autonomous() { auton_manager_node->selected_auton->AutonInit(); // Execute autonomous code - //while (pros::competition::is_autonomous()) { - // node_manager->executeAuton(); - //} + while (pros::competition::is_autonomous()) { + node_manager->executeAuton(); + } } /** diff --git a/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp b/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp index a9ce702..b13696f 100644 --- a/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp +++ b/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp @@ -84,7 +84,11 @@ void TankDriveNode::setDriveVoltage(int x_voltage, int theta_voltage) { } void TankDriveNode::setDriveVelocity(float x_velocity, float theta_velocity) { //incoming values should be in m/s so we convert to rpm here - // + IDriveKinematics::FourMotorPercentages motor_percentages = + m_kinematics.inverseKinematics(x_velocity, 0, theta_velocity, MAX_VELOCITY); + + m_setLeftVelocity(motor_percentages.left_front_percent * MAX_VELOCITY); + m_setRightVelocity(motor_percentages.right_front_percent * MAX_VELOCITY); } void TankDriveNode::teleopPeriodic() { diff --git a/src/v5_hal/firmware/src/nodes/auton_nodes/AutonManagerNode.cpp b/src/v5_hal/firmware/src/nodes/auton_nodes/AutonManagerNode.cpp index e92b476..3ea1c33 100644 --- a/src/v5_hal/firmware/src/nodes/auton_nodes/AutonManagerNode.cpp +++ b/src/v5_hal/firmware/src/nodes/auton_nodes/AutonManagerNode.cpp @@ -5,7 +5,7 @@ AutonManagerNode::AutonManagerNode(NodeManager* node_manager, IDriveNode* drive_ m_drive_node(drive_node), m_claw_node(claw_node) { m_basic_auton = new BasicAuton(m_drive_node, m_claw_node); - //selected_auton = m_basic_auton; + selected_auton = m_basic_auton; } void AutonManagerNode::initialize() {