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

64 lines
2.4 KiB
C++
Raw Normal View History

2015-02-06 13:45:01 -05:00
#include "Autonomous.h"
#include "Commands/CommandGroup.h"
#include "../../DentRobot.h"
2015-02-07 12:50:36 -05:00
#include "../Elevator/Raise.h"
#include "../Elevator/Lower.h"
#include "../BinElevator/BinRaise.h"
#include "../BinElevator/BinLower.h"
#include "AutoDrive.h"
#include "Turn.h"
2015-02-13 19:40:23 -05:00
#include "../Collector/RollIn.h"
#include "CollectTote.h"
2015-02-24 15:03:59 -05:00
#include "ReleaseTote.h"
2015-02-13 17:34:38 -05:00
Autonomous::Autonomous(int seq){
//SmartDashboard::GetNumber("Auto Wait Time");
2015-02-13 17:34:38 -05:00
switch(seq){
case 0:
// Just for testing
// Strafe at .25 power
AddSequential(new AutoDrive(0.5, 0.25, 0.0));
2015-02-13 17:34:38 -05:00
break;
case 1:
// Drive to Auto Zone (TM)
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, 0.8));
2015-02-13 17:34:38 -05:00
break;
case 2:
// Collect a tote, turn, drive to Auto Zone (TM)
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
2015-02-27 07:46:14 -05:00
AddSequential(new BinRaise(1.2));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, 0.75));
AddSequential(new BinLower(1.0));
2015-02-27 10:56:29 -05:00
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
2015-02-13 17:34:38 -05:00
break;
case 3:
// Collect three totes, drive to Auto Zone (TM)
printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
printf("Done");
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, -0.75));
AddSequential(new CollectTote());
2015-02-26 07:45:35 -05:00
AddSequential(new Raise());
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, -0.75));
AddSequential(new CollectTote());
2015-02-26 07:45:35 -05:00
AddSequential(new Lower());
AddSequential(new Raise());
printf("Three totes?: %d\n", SmartDashboard::GetBoolean("Three totes"));
if(SmartDashboard::GetBoolean("Three totes")){
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, -0.75));
AddSequential(new CollectTote());
2015-02-26 07:45:35 -05:00
AddSequential(new Lower());
AddSequential(new Raise());
}
AddSequential(new Turn(90));
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, -0.75));
2015-02-24 15:03:59 -05:00
AddSequential(new ReleaseTote());
2015-02-13 17:34:38 -05:00
break;
default:
printf("Invalid seq: %d\n", seq);
break;
}
2015-02-06 13:45:01 -05:00
}
2015-02-08 12:26:15 -05:00
// vim: ts=2:sw=2:et