diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index e13fac2..236a6b8 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -12,6 +12,7 @@ Autonomous::Autonomous(int seq){ case 0: // Just for testing AddSequential(new CollectTote()); + AddSequential(new Turn()); //AddSequential(new Raise()); //AddSequential(new Lower()); //AddSequential(new Turn()); diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp index b42bd40..eea1450 100644 --- a/Commands/Autonomous/Turn.cpp +++ b/Commands/Autonomous/Turn.cpp @@ -3,13 +3,13 @@ // Drive for a short while then stop. Just for testing Turn::Turn() : Command("Turn"){ Requires(DentRobot::drivetrain); - SetTimeout(0.25); + SetTimeout(0.85); } void Turn::Initialize(){ } void Turn::Execute(){ //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro - DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.5,0.9,0.0); + DentRobot::drivetrain->DriveMecanum(0.0,0.0,1.0,0.9,0.0); } bool Turn::IsFinished(){ return IsTimedOut(); diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp index 39e134a..6b728f3 100644 --- a/Commands/Collector/CollectTote.cpp +++ b/Commands/Collector/CollectTote.cpp @@ -3,11 +3,7 @@ #include "../Autonomous/AutoDrive.h" #include "RollIn.h" CollectTote::CollectTote(){ - AddParallel(new AutoDrive(0.5, -1.0)); - AddSequential(new RollIn()); - AddParallel(new AutoDrive(0.5, 1.0)); - AddSequential(new RollIn()); - AddParallel(new AutoDrive(0.5, -1.0)); + AddParallel(new AutoDrive(1.0, -0.75)); AddSequential(new RollIn()); } // vim: ts=2:sw=2:et diff --git a/Commands/Collector/RollIn.cpp b/Commands/Collector/RollIn.cpp index d7addc7..e4384f2 100644 --- a/Commands/Collector/RollIn.cpp +++ b/Commands/Collector/RollIn.cpp @@ -7,7 +7,7 @@ void RollIn::Initialize(){ } void RollIn::Execute(){ double throttle=DentRobot::oi->GetLeftThrottle(); - double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4/5; + double cvt=throttle*DentRobot::collector->GetSonarDistance()/0.4; if(cvt>=1.0){ DentRobot::collector->MoveRollers(1.0); }else{ diff --git a/DentRobot.cpp b/DentRobot.cpp index 875799e..42a8201 100644 --- a/DentRobot.cpp +++ b/DentRobot.cpp @@ -18,7 +18,7 @@ DentRobot::DentRobot(){ CameraServer::GetInstance()->StartAutomaticCapture("cam0"); //SmartDashboard::PutNumber("Auto Wait Time", 1.0); //SmartDashboard::PutNumber("Auto Sequence", 0); - printf("Initialized"); + printf("Initialized\n"); } void DentRobot::RobotInit(){ //SmartDashboard::PutNumber("CodeVersion",0.001); diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp index a2322d4..f9b1f17 100644 --- a/Subsystems/Collector.cpp +++ b/Subsystems/Collector.cpp @@ -12,10 +12,10 @@ void Collector::InitDefaultCommand(){ } void Collector::MoveRollers(double a){ collectorMotorLeft->Set(a); - collectorMotorBottom->Set(a); + collectorMotorBottom->Set(-a); collectorMotorRamp->Set(a); collectorMotorRight->Set(-a); - printf("%f\n",GetSonarDistance()); + printf("Roller power: %f\n",a); } double Collector::GetSonarDistance(){ return sonarAnalog->GetAverageVoltage();