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"
|
2015-02-08 16:51:35 -05:00
|
|
|
#include "../Elevator/Lower.h"
|
2015-02-27 06:47:10 -05:00
|
|
|
#include "../BinElevator/BinRaise.h"
|
|
|
|
#include "../BinElevator/BinLower.h"
|
2015-02-08 16:51:35 -05:00
|
|
|
#include "AutoDrive.h"
|
|
|
|
#include "Turn.h"
|
2015-02-13 19:40:23 -05:00
|
|
|
#include "../Collector/RollIn.h"
|
2015-02-24 12:02:30 -05:00
|
|
|
#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){
|
2015-02-13 22:30:15 -05:00
|
|
|
//SmartDashboard::GetNumber("Auto Wait Time");
|
2015-02-13 17:34:38 -05:00
|
|
|
switch(seq){
|
|
|
|
case 0:
|
|
|
|
// Just for testing
|
2015-02-27 15:44:18 -05:00
|
|
|
// Strafe at .25 power
|
|
|
|
AddSequential(new AutoDrive(0.5, 0.25, 0.0));
|
2015-02-13 17:34:38 -05:00
|
|
|
break;
|
|
|
|
case 1:
|
2015-02-27 15:44:18 -05:00
|
|
|
// Drive to Auto Zone (TM)
|
2015-02-24 12:02:30 -05:00
|
|
|
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
2015-02-27 16:42:11 -05:00
|
|
|
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8));
|
2015-02-13 17:34:38 -05:00
|
|
|
break;
|
|
|
|
case 2:
|
2015-03-05 20:07:52 -05:00
|
|
|
// Lower BinElevator, collect bin, turn, drive to AutoZone (TM)
|
|
|
|
AddSequential(new BinLower(0.5));
|
|
|
|
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Bin Distance"), 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)
|
2015-02-27 06:47:10 -05:00
|
|
|
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
2015-02-27 09:51:02 -05:00
|
|
|
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
2015-02-27 07:46:14 -05:00
|
|
|
AddSequential(new BinRaise(1.2));
|
2015-02-27 16:42:11 -05:00
|
|
|
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
|
2015-02-27 09:51:02 -05:00
|
|
|
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;
|
2015-03-05 20:07:52 -05:00
|
|
|
case 4:
|
2015-03-05 20:33:18 -05:00
|
|
|
// Collect one, two, or three totes, drive to Auto Zone (TM), release totes
|
2015-02-27 15:44:18 -05:00
|
|
|
printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
|
2015-02-24 12:02:30 -05:00
|
|
|
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
|
|
|
printf("Done");
|
|
|
|
AddSequential(new CollectTote());
|
2015-03-05 20:07:52 -05:00
|
|
|
if(SmartDashboard::GetBoolean("Two totes")){
|
|
|
|
AddSequential(new Raise());
|
2015-02-27 16:42:11 -05:00
|
|
|
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
|
2015-02-24 12:02:30 -05:00
|
|
|
AddSequential(new CollectTote());
|
2015-02-26 07:45:35 -05:00
|
|
|
AddSequential(new Lower());
|
|
|
|
AddSequential(new Raise());
|
2015-03-05 20:07:52 -05:00
|
|
|
if(SmartDashboard::GetBoolean("Three totes")){
|
|
|
|
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
|
|
|
|
AddSequential(new CollectTote());
|
|
|
|
AddSequential(new Lower());
|
|
|
|
AddSequential(new Raise());
|
|
|
|
}
|
2015-02-24 12:02:30 -05:00
|
|
|
}
|
2015-02-28 03:23:54 -05:00
|
|
|
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
2015-02-27 16:42:11 -05:00
|
|
|
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;
|
2015-03-05 20:07:52 -05:00
|
|
|
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:
|
2015-03-05 20:33:18 -05:00
|
|
|
// Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM), release totes
|
2015-03-05 20:07:52 -05:00
|
|
|
//TODO: Implement this
|
|
|
|
break;
|
2015-02-13 17:34:38 -05:00
|
|
|
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
|