Re-enable auton and implement basic inverse kinematics for the tank d…

…rive
msoe-vex · Feb 19, 2022 · f5dfda2 · f5dfda2
1 parent 5e6d447
commit f5dfda2
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 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
@@ -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);
 }
diff --git 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
@@ -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
@@ -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() {