Progskills ii

src/v5_hal/firmware/include/Constants.h

@@ -0,0 +1,6 @@
+#ifndef _LOCAL_CONSTANTS_H_
+#define _LOCAL_CONSTANTS_H_
+
+#define DRIVE_CONFIG {900., 1., M_PI * 4.}
+
+#endif

src/v5_hal/firmware/include/auton/auton_actions/SetGoalRotatorPosition.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "api.h"
+#include "lib-rr/auton/Auton.h"
+#include "lib-rr/util/Timer.h"
+#include "lib-rr/util/Constants.h"
+#include "nodes/GoalSpinnerNode.h"
+
+class SetGoalRotatorPosition : public AutonAction {
+private:
+    GoalSpinnerNode* m_goalSpinner;
+    int m_position;
+
+public:
+    SetGoalRotatorPosition(GoalSpinnerNode* goalSpinner, int position);
+
+    void ActionInit();
+
+    actionStatus Action();
+
+    void ActionEnd();
+};

src/v5_hal/firmware/include/auton/auton_routines/BasicAuton.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "Constants.h"
 #include "lib-rr/auton/Auton.h"
 #include "lib-rr/nodes/subsystems/IDriveNode.h"
 #include "lib-rr/nodes/odometry_nodes/OdometryNode.h"

src/v5_hal/firmware/include/auton/auton_routines/MatchAuton.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "Constants.h"
 #include "lib-rr/auton/Auton.h"
 #include "lib-rr/nodes/subsystems/IDriveNode.h"
 #include "lib-rr/nodes/odometry_nodes/OdometryNode.h"

src/v5_hal/firmware/include/auton/auton_routines/ProgrammingSkillzAuton.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include "lib-rr/auton/Auton.h"
+#include "lib-rr/nodes/subsystems/IDriveNode.h"
+#include "lib-rr/nodes/odometry_nodes/OdometryNode.h"
+#include "lib-rr/nodes/sensor_nodes/InertialSensorNode.h"
+#include "lib-rr/auton/auton_actions/DriveStraightAction.h"
+#include "auton/auton_actions/PlatformBalance.h"
+#include "lib-rr/auton/auton_actions/UseClawAction.h"
+#include "lib-rr/auton/auton_actions/MoveLiftToPositionAction.h"
+#include "lib-rr/auton/auton_actions/RollerIntakeAction.h"
+#include "lib-rr/auton/auton_actions/FollowPathAction.h"
+#include "auton/auton_actions/SetBackClawStateAction.h"
+#include "lib-rr/pursuit/path_pursuit/TankPathPursuit.h"
+#include "lib-rr/nodes/subsystems/ILiftNode.h"
+#include "lib-rr/nodes/subsystems/IClawNode.h"
+#include "lib-rr/nodes/subsystems/IRollerIntakeNode.h"
+#include "auton/auton_actions/DeployAction.h"
+#include "auton/auton_actions/SetLiftStateAction.h"
+#include "lib-rr/pathing/PathManager.h"
+#include "lib-rr/math/Pose.h"
+#include "lib-rr/util/Constants.h"
+#include "lib-rr/eigen/Eigen/Dense"
+#include "nodes/BackClawNode.h"
+#include "nodes/LiftNode.h"
+#include "nodes/HighRungLiftNode.h"
+
+
+class ProgrammingSkillzAuton : public Auton {
+public:
+    ProgrammingSkillzAuton(IDriveNode* driveNode, OdometryNode* odomNode, IRollerIntakeNode* intakeNode, 
+        IRollerIntakeNode* conveyorNode, IRollerIntakeNode* flapConveyorNode, IClawNode* frontClawNode, 
+        BackClawNode* backClaw, IClawNode* wingArms, IClawNode* buddyClimb, LiftNode* liftNode, 
+        HighRungLiftNode* highRungLiftNode);
+
+    void AddNodes();
+
+private:
+    IDriveNode* m_driveNode;
+    OdometryNode* m_odomNode;
+    IRollerIntakeNode* m_intakeNode;
+    IRollerIntakeNode* m_conveyorNode;
+    IRollerIntakeNode* m_flapConveyorNode;
+    IClawNode* m_frontClawNode;
+    BackClawNode* m_backClaw;
+    IClawNode* m_wingArms;
+    IClawNode* m_buddyClimb;
+    LiftNode* m_liftNode;
+    HighRungLiftNode* m_highRungLiftNode;
+
+};

src/v5_hal/firmware/include/main.h

@@ -80,7 +80,11 @@
 #include "auton/auton_routines/MatchAuton.h"
 #include "auton/auton_routines/LeftAuton.h"
 #include "auton/auton_routines/BasicAuton.h"
+<<<<<<< HEAD
+=======
 #include "auton/auton_routines/BackClawTestAuton.h"
+>>>>>>> 3064be4f7582e28e6ab2080e22cbcfc963b61d93
+#include "auton/auton_routines/ProgrammingSkillzAuton.h"
 
    /**
     * You should add more #includes here

src/v5_hal/firmware/src/auton/auton_actions/SetGoalRotatorPosition.cpp

@@ -0,0 +1,20 @@
+#include "auton/auton_actions/SetGoalRotatorPosition.h"
+
+SetGoalRotatorPosition::SetGoalRotatorPosition(GoalSpinnerNode* goalSpinner, int position) : 
+        m_goalSpinner(goalSpinner), 
+        m_position(position)  {
+
+}
+
+void SetGoalRotatorPosition::ActionInit() {
+
+}
+
+AutonAction::actionStatus SetGoalRotatorPosition::Action() {
+    m_goalSpinner->setSpinnerPosition(m_position);
+    return END;
+}
+
+void SetGoalRotatorPosition::ActionEnd() {
+
+}

src/v5_hal/firmware/src/auton/auton_routines/BasicAuton.cpp

@@ -9,7 +9,7 @@ BasicAuton::BasicAuton(IDriveNode* drive_node, OdometryNode* odom_node, HighRung
 }
 
 void BasicAuton::AddNodes() {
-    AutonNode* forward_node = new AutonNode(10, new SetHighRungLiftStateAction(m_high_rung_lift_node, HighRungLiftNode::HighRungLiftState::SCORING));
+    AutonNode* forward_node = new AutonNode(10, new DriveStraightAction(m_drive_node, m_odom_node, DRIVE_CONFIG, 60, 50, 10));
     Auton::AddFirstNode(forward_node);
 
     //AutonNode* platformBalance_Node = new AutonNode(10, new PlatformBalance(m_drive_node, m_y_odom_encoder, m_inertial_node));

src/v5_hal/firmware/src/auton/auton_routines/LeftAuton.cpp

@@ -3,7 +3,7 @@
 LeftAuton::LeftAuton(IDriveNode* drive_node, OdometryNode* odom_node, 
     IClawNode* front_claw_node, LiftNode* liftNode, HighRungLiftNode* highRingLiftNode, BackClawNode* backClawNode,
     IRollerIntakeNode* intakeNode, IRollerIntakeNode* conveyorNode, IRollerIntakeNode* flapConveyorNode) : 
-        Auton("Default Left Auton"), 
+        Auton("Left Match Auton", "/usd/paths/24inLeft.json"), 
         m_drive_node(drive_node),
         m_odom_node(odom_node),
         m_front_claw_node(front_claw_node),
@@ -34,7 +34,7 @@ void LeftAuton::AddNodes() {
     deploy->AddNext(lowerLift);
     deploy->AddNext(clawCloseDelay);
 
-    AutonNode* clawClose = new AutonNode(0.5, new UseClawAction(m_front_claw_node, true));
+    AutonNode* clawClose = new AutonNode(0.5, new UseClawAction(m_front_claw_node, false));
 
     clawCloseDelay->AddNext(clawClose);
 
@@ -52,7 +52,7 @@ void LeftAuton::AddNodes() {
 
     clawClose->AddNext(firstBackingNode);
 
-    AutonNode* clawOpen = new AutonNode(0.5, new UseClawAction(m_front_claw_node, false));
+    AutonNode* clawOpen = new AutonNode(0.5, new UseClawAction(m_front_claw_node, true));
     firstBackingNode->AddNext(clawOpen);
 
     AutonNode* backClawOpen = new AutonNode(0.5, new SetBackClawStateAction(m_backClawNode, BackClawNode::BackClawState::PIVOT_DOWN_CLAW_OPEN));
@@ -62,7 +62,7 @@ void LeftAuton::AddNodes() {
     AutonNode* secondGrabYellowNode = new AutonNode(4, new FollowPathAction(m_drive_node, m_odom_node, new TankPathPursuit(secondGrabYellow), secondGrabYellow, false));
     firstBackingNode->AddNext(secondGrabYellowNode);
 
-    AutonNode* clawClose2 = new AutonNode(0.5, new UseClawAction(m_front_claw_node, true));
+    AutonNode* clawClose2 = new AutonNode(0.5, new UseClawAction(m_front_claw_node, false));
     secondGrabYellowNode->AddNext(clawClose2);
 
     AutonNode* wait = new AutonNode(0.5, new WaitAction(0.5));
@@ -81,7 +81,7 @@ void LeftAuton::AddNodes() {
     AutonNode* backClawClosed = new AutonNode(0.5, new SetBackClawStateAction(m_backClawNode, BackClawNode::BackClawState::PIVOT_BACK));
     backingGrabBlueNode->AddNext(backClawClosed);
 
-    AutonNode* clawOpen2 = new AutonNode(0.5, new UseClawAction(m_front_claw_node, false));
+    AutonNode* clawOpen2 = new AutonNode(0.5, new UseClawAction(m_front_claw_node, true));
 
     backClawClosed->AddNext(clawOpen2);
 
@@ -131,15 +131,15 @@ void LeftAuton::AddNodes() {
 
     AutonNode* lowerLiftForTallGoal = new AutonNode(2, new SetLiftStateAction(m_liftNode, LiftNode::LiftState::DOWN));
 
-    AutonNode* clawOpenForTallGoal = new AutonNode(0.1, new UseClawAction(m_front_claw_node, false));
+    AutonNode* clawOpenForTallGoal = new AutonNode(0.1, new UseClawAction(m_front_claw_node, true));
 
     AutonNode* waitForLift = new AutonNode(1., new WaitAction(1.));
 
     fifthReverseForTallGoal->AddNext(lowerLiftForTallGoal);
     fifthReverseForTallGoal->AddNext(clawOpenForTallGoal);
     fifthReverseForTallGoal->AddNext(waitForLift);
 
-    AutonNode* clawCloseForTallGoal = new AutonNode(0.1, new UseClawAction(m_front_claw_node, true));
+    AutonNode* clawCloseForTallGoal = new AutonNode(0.1, new UseClawAction(m_front_claw_node, false));
 
     waitForLift->AddNext(clawCloseForTallGoal);
 
@@ -161,7 +161,7 @@ void LeftAuton::AddNodes() {
 
     waitForPullTallGoalClose->AddNext(backwardsTallGoalGrab);
 
-    AutonNode* clawOpenFinalForTallGoal = new AutonNode(0.1, new UseClawAction(m_front_claw_node, false));
+    AutonNode* clawOpenFinalForTallGoal = new AutonNode(0.1, new UseClawAction(m_front_claw_node, true));
 
     backwardsTallGoalGrab->AddNext(clawOpenFinalForTallGoal);
 
@@ -179,7 +179,7 @@ void LeftAuton::AddNodes() {
 
     clawOpenFinalForTallGoal->AddNext(sixthMovetoTallGoal);
 
-    AutonNode* clawCloseFinalForTallGoal = new AutonNode(0.1, new UseClawAction(m_front_claw_node, true));
+    AutonNode* clawCloseFinalForTallGoal = new AutonNode(0.1, new UseClawAction(m_front_claw_node, false));
 
     sixthMovetoTallGoal->AddNext(clawCloseFinalForTallGoal);
 

src/v5_hal/firmware/src/auton/auton_routines/MatchAuton.cpp

@@ -16,7 +16,7 @@ void MatchAuton::AddNodes() {
     AutonNode* deploy = new AutonNode(0.1, new DeployAction());
     Auton::AddFirstNode(deploy);
 
-    AutonNode* forward = new AutonNode(2, new DriveStraightAction(m_driveNode, m_odomNode, DRIVE_PARAMS, 38., 70., 80.));
+    AutonNode* forward = new AutonNode(2, new DriveStraightAction(m_driveNode, m_odomNode, DRIVE_CONFIG, 38, 70, 80));
 
     AutonNode* clawCloseDelay = new AutonNode(1.2, new WaitAction(1.2));