2015-01-16 19:49:16 -05:00
|
|
|
#include "DentRobot.h"
|
2015-02-12 21:18:48 -05:00
|
|
|
#include "OI.h"
|
2015-02-19 12:33:23 -05:00
|
|
|
#include "RobotMap.h"
|
2015-02-07 12:32:46 -05:00
|
|
|
#include "Commands/Autonomous/Autonomous.h"
|
2015-03-23 10:52:02 -04:00
|
|
|
OI* DentRobot::oi = NULL;
|
|
|
|
Collector* DentRobot::collector = NULL;
|
|
|
|
Drivetrain* DentRobot::drivetrain = NULL;
|
|
|
|
Elevator* DentRobot::elevator = NULL;
|
|
|
|
BinElevator* DentRobot::binElevator = NULL;
|
|
|
|
CommandGroup* DentRobot::aut = NULL;
|
|
|
|
Pneumatics* DentRobot::pneumatics = NULL;
|
2015-04-06 20:36:35 -04:00
|
|
|
BinCollector* DentRobot::binCollector = NULL;
|
2015-07-31 12:52:15 -04:00
|
|
|
DentRobot::DentRobot() {
|
2015-03-23 10:52:02 -04:00
|
|
|
oi = new OI();
|
|
|
|
collector = new Collector();
|
|
|
|
drivetrain = new Drivetrain();
|
|
|
|
elevator = new Elevator();
|
|
|
|
binElevator = new BinElevator();
|
|
|
|
pneumatics = new Pneumatics();
|
2015-04-06 20:36:35 -04:00
|
|
|
binCollector = new BinCollector();
|
2015-02-26 12:18:23 -05:00
|
|
|
//CameraServer::GetInstance()->SetQuality(25);
|
|
|
|
//CameraServer::GetInstance()->StartAutomaticCapture("cam0");
|
2015-02-20 08:28:49 -05:00
|
|
|
printf("The robot is on\n");
|
2015-01-16 19:49:16 -05:00
|
|
|
}
|
2015-07-31 12:52:15 -04:00
|
|
|
void DentRobot::RobotInit() {
|
2015-02-27 15:44:18 -05:00
|
|
|
SmartDashboard::PutNumber("CodeVersion", CODE_VERSION);
|
2015-02-24 12:02:30 -05:00
|
|
|
// Autonomous
|
2015-03-17 16:33:25 -04:00
|
|
|
// Calibration
|
|
|
|
// Amount to turn while collecting the initial tote in auto 4
|
|
|
|
SmartDashboard::PutNumber("CollectToteTurn", 0.25);
|
|
|
|
// Amount of time to collect a tote
|
2015-10-23 15:15:06 -04:00
|
|
|
SmartDashboard::PutNumber("DriveTime1", 1.3);
|
|
|
|
SmartDashboard::PutNumber("DriveTime2", 1.3);
|
2015-02-24 12:02:30 -05:00
|
|
|
// Sequence of autonomous command
|
2015-10-10 03:08:21 -04:00
|
|
|
SmartDashboard::PutNumber("Auto Sequence", -1.0);
|
2015-03-17 16:33:25 -04:00
|
|
|
SmartDashboard::PutNumber("Auto Wait Time", 0.5);
|
2015-02-24 12:02:30 -05:00
|
|
|
// If the robot will be picking up three totes in sequence 3
|
2015-03-17 16:33:25 -04:00
|
|
|
SmartDashboard::PutBoolean("Two totes", false);
|
2015-03-05 20:07:52 -05:00
|
|
|
SmartDashboard::PutBoolean("Three totes", false);
|
2015-02-24 12:02:30 -05:00
|
|
|
// Distance (in time) to auto zone
|
2015-03-05 15:43:07 -05:00
|
|
|
SmartDashboard::PutNumber("Auto Zone Distance", 2.1);
|
2015-02-24 12:02:30 -05:00
|
|
|
// Distance (in time) to auto tote (used in sequence 3)
|
2015-03-17 16:33:25 -04:00
|
|
|
SmartDashboard::PutNumber("Two Tote Distance", 1.0);
|
|
|
|
SmartDashboard::PutNumber("Three Tote Distance", 2.5);
|
2015-03-19 15:48:47 -04:00
|
|
|
SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
|
|
|
|
SmartDashboard::PutNumber("TurnAmount", 2.6);
|
2015-02-24 12:02:30 -05:00
|
|
|
// Elevators
|
|
|
|
SmartDashboard::PutBoolean("Elevator Bottom", false);
|
|
|
|
SmartDashboard::PutBoolean("Elevator Top", false);
|
2015-03-13 19:47:37 -04:00
|
|
|
//Gyro
|
2015-03-17 16:33:25 -04:00
|
|
|
SmartDashboard::PutNumber("Gyro kP", -0.02);
|
2015-09-18 16:34:20 -04:00
|
|
|
printf("Starting compressor\n");
|
|
|
|
pneumatics->SetCompressorEnabled(true);
|
2015-10-23 14:45:39 -04:00
|
|
|
pneumatics->SetClawOpen(true);
|
2015-01-16 19:49:16 -05:00
|
|
|
}
|
2015-07-31 12:52:15 -04:00
|
|
|
void DentRobot::DisabledPeriodic() {
|
2015-01-16 19:49:16 -05:00
|
|
|
Scheduler::GetInstance()->Run();
|
|
|
|
}
|
2015-07-31 12:52:15 -04:00
|
|
|
void DentRobot::AutonomousInit() {
|
2015-03-23 10:52:02 -04:00
|
|
|
aut = new Autonomous(SmartDashboard::GetNumber("Auto Sequence"));
|
2015-02-27 15:44:18 -05:00
|
|
|
printf("Enabling Auto Sequence %f\n", SmartDashboard::GetNumber("Auto Sequence"));
|
2015-07-31 12:52:15 -04:00
|
|
|
if(aut != NULL) {
|
2015-02-27 03:32:21 -05:00
|
|
|
aut->Start();
|
|
|
|
}
|
2015-01-16 19:49:16 -05:00
|
|
|
}
|
2015-07-31 12:52:15 -04:00
|
|
|
void DentRobot::AutonomousPeriodic() {
|
2015-02-20 08:43:31 -05:00
|
|
|
printf("Running auto.\n");
|
2015-01-16 19:49:16 -05:00
|
|
|
Scheduler::GetInstance()->Run();
|
|
|
|
}
|
2015-07-31 12:52:15 -04:00
|
|
|
void DentRobot::TeleopInit() {
|
|
|
|
if(aut != NULL) {
|
2015-02-12 21:18:48 -05:00
|
|
|
aut->Cancel();
|
|
|
|
}
|
2015-01-16 19:49:16 -05:00
|
|
|
}
|
2015-07-31 12:52:15 -04:00
|
|
|
void DentRobot::TeleopPeriodic() {
|
2015-01-16 19:49:16 -05:00
|
|
|
Scheduler::GetInstance()->Run();
|
2015-03-22 14:29:07 -04:00
|
|
|
SmartDashboard::PutNumber("CollectorThrottle", oi->GetLeftThrottle());
|
2015-01-16 19:49:16 -05:00
|
|
|
}
|
2015-09-19 17:57:42 -04:00
|
|
|
void DentRobot::TestPeriodic() {}
|
2015-01-11 16:02:49 -05:00
|
|
|
START_ROBOT_CLASS(DentRobot);
|
2015-02-08 12:26:15 -05:00
|
|
|
// vim: ts=2:sw=2:et
|