mirror of
https://github.com/team2059/Dent
synced 2024-12-18 20:52:29 -05:00
Continued working on auto. Works, but not well
This commit is contained in:
parent
d0bb53a84c
commit
d66278a902
@ -15,6 +15,7 @@ void AutoDrive::Initialize(){
|
||||
void AutoDrive::Execute(){
|
||||
//X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
|
||||
printf("z: %f\n", z);
|
||||
printf("(x, y, z): (%f, %f, %f)\n", x, y, z);
|
||||
DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, gyro);
|
||||
}
|
||||
bool AutoDrive::IsFinished(){
|
||||
|
@ -16,7 +16,13 @@ Autonomous::Autonomous(int seq){
|
||||
case 0:
|
||||
// Just for testing
|
||||
// Turn testing
|
||||
AddSequential(new Turn(3.8));
|
||||
//AddSequential(new AutoDrive(2.0, -0.75, 0.0, true));
|
||||
//AddSequential(new CollectTote());
|
||||
//AddSequential(new CollectTote());
|
||||
//AddSequential(new CollectTote());
|
||||
//AddSequential(new CollectTote());
|
||||
//AddSequential(new Turn(3.8));
|
||||
AddSequential(new Turn(SmartDashboard::GetNumber("180Turn"), 0.8));
|
||||
break;
|
||||
case 1:
|
||||
// Drive to Auto Zone (TM)
|
||||
@ -44,8 +50,10 @@ Autonomous::Autonomous(int seq){
|
||||
// Collect one, two, or three totes, drive to Auto Zone (TM), release totes
|
||||
AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
|
||||
if(SmartDashboard::GetBoolean("Two totes")){
|
||||
AddParallel(new Turn(0.81));
|
||||
AddParallel(new Turn(SmartDashboard::GetNumber("TwoToteTurn")));
|
||||
AddParallel(new RollIn(3.5));
|
||||
AddSequential(new Raise(3.5));
|
||||
AddParallel(new RollIn(SmartDashboard::GetNumber("Two Tote Distance")));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75));
|
||||
AddSequential(new CollectTote());
|
||||
AddSequential(new Lower(3.0));
|
||||
@ -74,6 +82,14 @@ Autonomous::Autonomous(int seq){
|
||||
// Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM), release totes
|
||||
//TODO: Implement this
|
||||
break;
|
||||
case 8:
|
||||
AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
|
||||
AddSequential(new Turn(SmartDashboard::GetNumber("180Turn"), 0.8));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75));
|
||||
AddSequential(new CollectTote());
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Three Tote Distance"), 0.0, 0.75));
|
||||
AddSequential(new CollectTote());
|
||||
break;
|
||||
default:
|
||||
printf("Invalid seq: %d\n", seq);
|
||||
break;
|
||||
|
@ -1,14 +1,15 @@
|
||||
#include "Turn.h"
|
||||
#include "../../DentRobot.h"
|
||||
Turn::Turn(double timeout): Command("Turn"){
|
||||
Turn::Turn(double timeout, double pwr): Command("Turn"){
|
||||
Requires(DentRobot::drivetrain);
|
||||
SetTimeout(timeout);
|
||||
power=pwr;
|
||||
}
|
||||
void Turn::Initialize(){
|
||||
}
|
||||
void Turn::Execute(){
|
||||
//X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
|
||||
DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9);
|
||||
DentRobot::drivetrain->DriveMecanum(0.0, 0.0, power, 0.9);
|
||||
}
|
||||
bool Turn::IsFinished(){
|
||||
return IsTimedOut();
|
||||
|
@ -13,13 +13,14 @@
|
||||
*/
|
||||
class Turn: public Command{
|
||||
private:
|
||||
double power; //<! Power to turn (default: 0.6)
|
||||
public:
|
||||
/**
|
||||
* @brief Constructs Turn
|
||||
*
|
||||
* @param timeout Timeout in seconds
|
||||
*/
|
||||
Turn(double timeout);
|
||||
Turn(double timeout, double pwr=0.6);
|
||||
/**
|
||||
* @brief Initializes the class
|
||||
*/
|
||||
|
@ -26,6 +26,8 @@ void DentRobot::RobotInit(){
|
||||
// Calibration
|
||||
// Amount to turn while collecting the initial tote in auto 4
|
||||
SmartDashboard::PutNumber("CollectToteTurn", 0.25);
|
||||
SmartDashboard::PutNumber("180Turn", 1.8);
|
||||
SmartDashboard::PutNumber("TwoToteTurn", 0.81);
|
||||
// Amount of time to collect a tote
|
||||
SmartDashboard::PutNumber("DriveTime", 1.3);
|
||||
// Sequence of autonomous command
|
||||
@ -41,7 +43,6 @@ void DentRobot::RobotInit(){
|
||||
SmartDashboard::PutNumber("Three Tote Distance", 2.5);
|
||||
SmartDashboard::PutNumber("Auto Bin Distance", 0.25);
|
||||
SmartDashboard::PutNumber("TurnAmount", 1.8);
|
||||
|
||||
// Elevators
|
||||
SmartDashboard::PutBoolean("Bin Elevator Bottom", false);
|
||||
SmartDashboard::PutBoolean("Bin Elevator Top", false);
|
||||
@ -63,7 +64,6 @@ void DentRobot::AutonomousInit(){
|
||||
}
|
||||
}
|
||||
void DentRobot::AutonomousPeriodic(){
|
||||
printf("Running auto.\n");
|
||||
Scheduler::GetInstance()->Run();
|
||||
}
|
||||
void DentRobot::TeleopInit(){
|
||||
|
@ -27,9 +27,9 @@ void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity,
|
||||
if(DentRobot::oi->GetLeftStick()->GetRawButton(9)){
|
||||
correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
|
||||
}
|
||||
rightFront->Set((-correctX + correctY - correctZ));
|
||||
rightFront->Set(-correctX + correctY - correctZ);
|
||||
leftFront->Set((correctX + correctY + correctZ)*-1);
|
||||
rightRear->Set((correctX + correctY - correctZ));
|
||||
rightRear->Set(correctX + correctY - correctZ);
|
||||
leftRear->Set((-correctX + correctY + correctZ)*-1);
|
||||
}
|
||||
//Used in pretest
|
||||
|
Loading…
Reference in New Issue
Block a user