2
0
mirror of https://github.com/team2059/Dent synced 2024-12-18 20:52:29 -05:00
dent/Commands/Autonomous/Autonomous.cpp

97 lines
3.7 KiB
C++

#include "Autonomous.h"
#include "Commands/CommandGroup.h"
#include "../../DentRobot.h"
#include "../Elevator/Raise.h"
#include "../Elevator/Lower.h"
#include "../BinElevator/BinRaise.h"
#include "../BinElevator/BinLower.h"
#include "AutoDrive.h"
#include "Turn.h"
#include "../Collector/RollIn.h"
#include "CollectTote.h"
#include "ReleaseTote.h"
Autonomous::Autonomous(int seq){
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
switch(seq){
case 0:
// Just for testing
// Turn testing
AddSequential(new Turn(3.8));
break;
case 1:
// Drive to Auto Zone (TM)
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8, 0.01));
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
break;
case 2:
// Lower BinElevator, collect bin, turn, drive to AutoZone (TM)
AddSequential(new BinLower(0.5));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Bin Distance"), 0.0, 0.75));
AddSequential(new BinRaise(1.0));
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
break;
case 3:
// Collect a tote with BinElevator, turn, drive to Auto Zone (TM)
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
AddSequential(new BinRaise(1.2));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
AddSequential(new BinLower(1.0));
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
break;
case 4:
// Collect one, two, or three totes, drive to Auto Zone (TM), release totes
AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
if(SmartDashboard::GetBoolean("Two totes")){
AddParallel(new Turn(0.81));
AddSequential(new Raise(3.5));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75));
AddSequential(new CollectTote());
AddSequential(new Lower(3.0));
AddSequential(new Raise(3.5));
if(SmartDashboard::GetBoolean("Three totes")){
AddSequential(new Turn(3.8));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Three Tote Distance"), 0.0, 0.75));
AddSequential(new CollectTote());
AddSequential(new Lower(3.0));
AddSequential(new Raise(3.5));
}
}
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, 0.75));
AddSequential(new ReleaseTote());
break;
case 5:
// Same as auto 4, but navigate around bins
//TODO: Implement this
break;
case 6:
// Collect 1 bin and 1 tote
//TODO: Implement this
break;
case 7:
// Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM), release totes
//TODO: Implement this
break;
case 8:
//Use rear elevator to move tote
AddSequential(new BinRaise(1));
AddSequential(new Turn(2.1));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"),0.0,0.75));
AddSequential(new Turn(2.1));
break;
case 9:
//Use rear elevator to move tote
AddSequential(new BinLower(1.5));
AddSequential(new Turn(2.1));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"),0.0,-0.75));
AddSequential(new Turn(2.1));
break;
default:
printf("Invalid seq: %d\n", seq);
break;
}
}
// vim: ts=2:sw=2:et