mirror of
https://github.com/team2059/Dent
synced 2024-12-18 20:52:29 -05:00
Made auto more modular
This commit is contained in:
parent
d515a11cd6
commit
d9c010aed8
@ -1,27 +1,17 @@
|
||||
#include "AutoDrive.h"
|
||||
#include "../../DentRobot.h"
|
||||
// Drive for a short while then stop. Just for testing
|
||||
AutoDrive::AutoDrive(double wait) : Command("AutoDrive"){
|
||||
AutoDrive::AutoDrive(double duration, double p=-0.75) : Command("AutoDrive"){
|
||||
Requires(DentRobot::drivetrain);
|
||||
SetTimeout(wait);
|
||||
power=5;
|
||||
}
|
||||
AutoDrive::AutoDrive(double wait, double p) : Command("AutoDrive"){
|
||||
Requires(DentRobot::drivetrain);
|
||||
SetTimeout(wait);
|
||||
SetTimeout(duration);
|
||||
power=p;
|
||||
}
|
||||
void AutoDrive::Initialize(){
|
||||
}
|
||||
void AutoDrive::Execute(){
|
||||
//X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
|
||||
//if(power==NULL){
|
||||
if(power==5){
|
||||
DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0);
|
||||
}else{
|
||||
DentRobot::drivetrain->DriveMecanum(0.0,power,0.0,0.9,0.0);
|
||||
}
|
||||
}
|
||||
bool AutoDrive::IsFinished(){
|
||||
return IsTimedOut();
|
||||
}
|
||||
|
@ -12,7 +12,7 @@ Autonomous::Autonomous(int seq){
|
||||
case 0:
|
||||
// Just for testing
|
||||
AddSequential(new CollectTote());
|
||||
AddSequential(new Turn());
|
||||
AddSequential(new Turn(90));
|
||||
//AddSequential(new Raise());
|
||||
//AddSequential(new Lower());
|
||||
//AddSequential(new Turn());
|
||||
@ -23,21 +23,18 @@ Autonomous::Autonomous(int seq){
|
||||
case 1:
|
||||
// Drive forward a bit, turn around, collect tote then bin
|
||||
AddSequential(new AutoDrive(0.5));
|
||||
AddSequential(new Turn());
|
||||
AddSequential(new Turn());
|
||||
AddSequential(new RollIn());
|
||||
AddSequential(new Turn(180));
|
||||
AddSequential(new RollIn(1.0));
|
||||
AddSequential(new Raise());
|
||||
break;
|
||||
case 2:
|
||||
// Drive forward a bit, turn around, collect tote then bin
|
||||
AddParallel(new Raise());
|
||||
AddParallel(new AutoDrive(0.5));
|
||||
AddSequential(new Turn());
|
||||
AddSequential(new Turn());
|
||||
AddSequential(new RollIn());
|
||||
AddSequential(new Turn(180));
|
||||
AddSequential(new RollIn(1.0));
|
||||
break;
|
||||
case 3:
|
||||
// Wait a desigated value, drive to Auto Zone (TM)
|
||||
//Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
||||
AddSequential(new AutoDrive(2.0));
|
||||
break;
|
||||
|
@ -1,8 +1,9 @@
|
||||
#include "Turn.h"
|
||||
#include "../../DentRobot.h"
|
||||
// Drive for a short while then stop. Just for testing
|
||||
Turn::Turn() : Command("Turn"){
|
||||
Turn::Turn(int k) : Command("Turn"){
|
||||
Requires(DentRobot::drivetrain);
|
||||
degrees=k;
|
||||
SetTimeout(0.85);
|
||||
}
|
||||
void Turn::Initialize(){
|
||||
|
@ -7,8 +7,10 @@
|
||||
#include "WPILib.h"
|
||||
|
||||
class Turn: public Command{
|
||||
private:
|
||||
int degrees;
|
||||
public:
|
||||
Turn();
|
||||
Turn(int);
|
||||
void Initialize();
|
||||
void Execute();
|
||||
bool IsFinished();
|
||||
|
@ -4,6 +4,6 @@
|
||||
#include "RollIn.h"
|
||||
CollectTote::CollectTote(){
|
||||
AddParallel(new AutoDrive(1.0, -0.75));
|
||||
AddSequential(new RollIn());
|
||||
AddSequential(new RollIn(1.0));
|
||||
}
|
||||
// vim: ts=2:sw=2:et
|
||||
|
@ -1,13 +1,13 @@
|
||||
#include "RollIn.h"
|
||||
RollIn::RollIn() : Command("RollIn"){
|
||||
RollIn::RollIn(double k) : Command("RollIn"){
|
||||
rawSpeed=k;
|
||||
}
|
||||
void RollIn::Initialize(){
|
||||
printf("Initialized RollIn\n");
|
||||
SetTimeout(2.0);
|
||||
}
|
||||
void RollIn::Execute(){
|
||||
double throttle=DentRobot::oi->GetLeftThrottle();
|
||||
double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4;
|
||||
double cvt=(rawSpeed)*DentRobot::collector->GetSonarDistance()/0.4;
|
||||
if(cvt>=1.0){
|
||||
DentRobot::collector->MoveRollers(1.0);
|
||||
}else{
|
||||
|
@ -7,8 +7,10 @@
|
||||
#include "WPILib.h"
|
||||
|
||||
class RollIn: public Command{
|
||||
private:
|
||||
double rawSpeed;
|
||||
public:
|
||||
RollIn();
|
||||
RollIn(double);
|
||||
void Initialize();
|
||||
void Execute();
|
||||
bool IsFinished();
|
||||
|
8
OI.cpp
8
OI.cpp
@ -5,6 +5,8 @@
|
||||
#include "Commands/Collector/RollOut.h"
|
||||
#include "Commands/BinElevator/BinLower.h"
|
||||
#include "Commands/BinElevator/BinRaise.h"
|
||||
#include "Commands/BinElevator/BinCloseArms.h"
|
||||
#include "Commands/BinElevator/BinOpenArms.h"
|
||||
|
||||
OI::OI() {
|
||||
// Joysticks
|
||||
@ -13,7 +15,7 @@ OI::OI() {
|
||||
// Collector
|
||||
JoystickButton *left1=new JoystickButton(leftStick, 1);
|
||||
JoystickButton *left2=new JoystickButton(leftStick, 2);
|
||||
left1->WhileHeld(new RollIn());
|
||||
left1->WhileHeld(new RollIn(GetLeftThrottle()));
|
||||
left2->WhileHeld(new RollOut());
|
||||
// Elevator
|
||||
raise=new Raise();
|
||||
@ -27,12 +29,16 @@ OI::OI() {
|
||||
// BinElevator
|
||||
JoystickButton *right3=new JoystickButton(rightStick, 3);
|
||||
JoystickButton *right5=new JoystickButton(rightStick, 5);
|
||||
JoystickButton *right7=new JoystickButton(rightStick, 7);
|
||||
JoystickButton *right8=new JoystickButton(rightStick, 8);
|
||||
binRaise=new BinRaise();
|
||||
binLower=new BinLower();
|
||||
right3->WhenPressed(binLower);
|
||||
right3->CancelWhenPressed(binRaise);
|
||||
right5->WhenPressed(binRaise);
|
||||
right5->CancelWhenPressed(binLower);
|
||||
right7->WhenPressed(new BinOpenArms());
|
||||
right8->WhenPressed(new BinCloseArms());
|
||||
// Cancel
|
||||
JoystickButton *right12=new JoystickButton(rightStick, 12);
|
||||
right12->CancelWhenPressed(raise);
|
||||
|
Loading…
Reference in New Issue
Block a user