mirror of
https://github.com/team2059/Dent
synced 2024-12-18 20:52:29 -05:00
Merged develop with final dashboard values from Palmetto
This commit is contained in:
commit
a8ab3b239f
@ -25,7 +25,16 @@ Autonomous::Autonomous(int seq){
|
|||||||
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
// Collect a tote, turn, drive to Auto Zone (TM)
|
// 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)
|
||||||
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
||||||
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
||||||
AddSequential(new BinRaise(1.2));
|
AddSequential(new BinRaise(1.2));
|
||||||
@ -33,29 +42,41 @@ Autonomous::Autonomous(int seq){
|
|||||||
AddSequential(new BinLower(1.0));
|
AddSequential(new BinLower(1.0));
|
||||||
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
||||||
break;
|
break;
|
||||||
case 3:
|
case 4:
|
||||||
// Collect three totes, drive to Auto Zone (TM)
|
// Collect one, two, or three totes, drive to Auto Zone (TM), release totes
|
||||||
printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
|
printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
|
||||||
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
||||||
printf("Done");
|
printf("Done");
|
||||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
|
|
||||||
AddSequential(new CollectTote());
|
AddSequential(new CollectTote());
|
||||||
|
if(SmartDashboard::GetBoolean("Two totes")){
|
||||||
AddSequential(new Raise());
|
AddSequential(new Raise());
|
||||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
|
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
|
||||||
AddSequential(new CollectTote());
|
AddSequential(new CollectTote());
|
||||||
AddSequential(new Lower());
|
AddSequential(new Lower());
|
||||||
AddSequential(new Raise());
|
AddSequential(new Raise());
|
||||||
printf("Three totes?: %d\n", SmartDashboard::GetBoolean("Three totes"));
|
|
||||||
if(SmartDashboard::GetBoolean("Three totes")){
|
if(SmartDashboard::GetBoolean("Three totes")){
|
||||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
|
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75));
|
||||||
AddSequential(new CollectTote());
|
AddSequential(new CollectTote());
|
||||||
AddSequential(new Lower());
|
AddSequential(new Lower());
|
||||||
AddSequential(new Raise());
|
AddSequential(new Raise());
|
||||||
}
|
}
|
||||||
|
}
|
||||||
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
||||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, 0.75));
|
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, 0.75));
|
||||||
AddSequential(new ReleaseTote());
|
AddSequential(new ReleaseTote());
|
||||||
break;
|
break;
|
||||||
|
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:
|
||||||
|
// Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM), release totes
|
||||||
|
//TODO: Implement this
|
||||||
|
break;
|
||||||
default:
|
default:
|
||||||
printf("Invalid seq: %d\n", seq);
|
printf("Invalid seq: %d\n", seq);
|
||||||
break;
|
break;
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#include "Turn.h"
|
#include "Turn.h"
|
||||||
#include "../../DentRobot.h"
|
#include "../../DentRobot.h"
|
||||||
// Drive for a short while then stop. Just for testing
|
// Drive for a short while then stop. Just for testing
|
||||||
Turn::Turn(int k) : Command("Turn"){
|
Turn::Turn(double k) : Command("Turn"){
|
||||||
Requires(DentRobot::drivetrain);
|
Requires(DentRobot::drivetrain);
|
||||||
SetTimeout(k);
|
SetTimeout(k);
|
||||||
}
|
}
|
||||||
|
@ -10,7 +10,7 @@ class Turn: public Command{
|
|||||||
private:
|
private:
|
||||||
int degrees;
|
int degrees;
|
||||||
public:
|
public:
|
||||||
Turn(int);
|
Turn(double);
|
||||||
void Initialize();
|
void Initialize();
|
||||||
void Execute();
|
void Execute();
|
||||||
bool IsFinished();
|
bool IsFinished();
|
||||||
|
@ -4,11 +4,10 @@
|
|||||||
BinCloseArms::BinCloseArms() : Command("BinCloseArms"){
|
BinCloseArms::BinCloseArms() : Command("BinCloseArms"){
|
||||||
}
|
}
|
||||||
void BinCloseArms::Initialize(){
|
void BinCloseArms::Initialize(){
|
||||||
//Should never need to use this
|
|
||||||
SetTimeout(0.5);
|
SetTimeout(0.5);
|
||||||
}
|
}
|
||||||
void BinCloseArms::Execute(){
|
void BinCloseArms::Execute(){
|
||||||
DentRobot::pneumatics->SetOpen(true);
|
DentRobot::pneumatics->SetOpen(false);
|
||||||
}
|
}
|
||||||
bool BinCloseArms::IsFinished(){
|
bool BinCloseArms::IsFinished(){
|
||||||
return true;
|
return true;
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#include "BinLower.h"
|
#include "BinLower.h"
|
||||||
#include "../../DentRobot.h"
|
#include "../../DentRobot.h"
|
||||||
#include "../../OI.h"
|
#include "../../OI.h"
|
||||||
BinLower::BinLower(float t) : Command("BinLower"){
|
BinLower::BinLower(double t) : Command("BinLower"){
|
||||||
timeout=t;
|
timeout=t;
|
||||||
}
|
}
|
||||||
void BinLower::Initialize(){
|
void BinLower::Initialize(){
|
||||||
|
@ -6,9 +6,9 @@
|
|||||||
|
|
||||||
class BinLower: public Command{
|
class BinLower: public Command{
|
||||||
private:
|
private:
|
||||||
float timeout;
|
double timeout;
|
||||||
public:
|
public:
|
||||||
BinLower(float);
|
BinLower(double);
|
||||||
void Initialize();
|
void Initialize();
|
||||||
void Execute();
|
void Execute();
|
||||||
bool IsFinished();
|
bool IsFinished();
|
||||||
|
@ -4,7 +4,6 @@
|
|||||||
BinOpenArms::BinOpenArms() : Command("BinOpenArms"){
|
BinOpenArms::BinOpenArms() : Command("BinOpenArms"){
|
||||||
}
|
}
|
||||||
void BinOpenArms::Initialize(){
|
void BinOpenArms::Initialize(){
|
||||||
//Should never need to use this
|
|
||||||
SetTimeout(0.5);
|
SetTimeout(0.5);
|
||||||
}
|
}
|
||||||
void BinOpenArms::Execute(){
|
void BinOpenArms::Execute(){
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#include "BinRaise.h"
|
#include "BinRaise.h"
|
||||||
#include "../../DentRobot.h"
|
#include "../../DentRobot.h"
|
||||||
#include "../../OI.h"
|
#include "../../OI.h"
|
||||||
BinRaise::BinRaise(float t) : Command("BinRaise"){
|
BinRaise::BinRaise(double t) : Command("BinRaise"){
|
||||||
timeout=t;
|
timeout=t;
|
||||||
}
|
}
|
||||||
void BinRaise::Initialize(){
|
void BinRaise::Initialize(){
|
||||||
|
@ -6,9 +6,9 @@
|
|||||||
|
|
||||||
class BinRaise: public Command{
|
class BinRaise: public Command{
|
||||||
private:
|
private:
|
||||||
float timeout;
|
double timeout;
|
||||||
public:
|
public:
|
||||||
BinRaise(float);
|
BinRaise(double);
|
||||||
void Initialize();
|
void Initialize();
|
||||||
void Execute();
|
void Execute();
|
||||||
bool IsFinished();
|
bool IsFinished();
|
||||||
|
@ -25,14 +25,16 @@ void DentRobot::RobotInit(){
|
|||||||
// Autonomous
|
// Autonomous
|
||||||
// Sequence of autonomous command
|
// Sequence of autonomous command
|
||||||
SmartDashboard::PutNumber("Auto Sequence", 1.0);
|
SmartDashboard::PutNumber("Auto Sequence", 1.0);
|
||||||
SmartDashboard::PutNumber("Auto Wait Time", 3.0);
|
SmartDashboard::PutNumber("Auto Wait Time", 2.0);
|
||||||
// If the robot will be picking up three totes in sequence 3
|
// If the robot will be picking up three totes in sequence 3
|
||||||
SmartDashboard::PutBoolean("Three totes", true);
|
SmartDashboard::PutBoolean("Two totes", true);
|
||||||
|
SmartDashboard::PutBoolean("Three totes", false);
|
||||||
// TODO: Calibrate the following two values
|
// TODO: Calibrate the following two values
|
||||||
// Distance (in time) to auto zone
|
// Distance (in time) to auto zone
|
||||||
SmartDashboard::PutNumber("Auto Zone Distance", 2.1);
|
SmartDashboard::PutNumber("Auto Zone Distance", 2.1);
|
||||||
// Distance (in time) to auto tote (used in sequence 3)
|
// Distance (in time) to auto tote (used in sequence 3)
|
||||||
SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
|
SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
|
||||||
|
SmartDashboard::PutNumber("Auto Bin Distance", 0.25);
|
||||||
SmartDashboard::PutNumber("TurnAmount", 1.8);
|
SmartDashboard::PutNumber("TurnAmount", 1.8);
|
||||||
|
|
||||||
// Elevators
|
// Elevators
|
||||||
|
@ -11,7 +11,7 @@ Drivetrain::Drivetrain() : Subsystem("Drivetrain"){
|
|||||||
void Drivetrain::InitDefaultCommand(){
|
void Drivetrain::InitDefaultCommand(){
|
||||||
SetDefaultCommand(new Drive());
|
SetDefaultCommand(new Drive());
|
||||||
}
|
}
|
||||||
void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){
|
void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, double gyro){
|
||||||
double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
|
double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
|
||||||
double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
|
double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
|
||||||
double correctZ = -z * 0.5;
|
double correctZ = -z * 0.5;
|
||||||
@ -25,7 +25,7 @@ void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, floa
|
|||||||
}
|
}
|
||||||
|
|
||||||
//Used in pretest
|
//Used in pretest
|
||||||
void Drivetrain::TestMotor(e_motors motor, float power){
|
void Drivetrain::TestMotor(e_motors motor, double power){
|
||||||
switch(motor){
|
switch(motor){
|
||||||
case FRONTRIGHT:
|
case FRONTRIGHT:
|
||||||
rightFront->Set(power);
|
rightFront->Set(power);
|
||||||
|
@ -15,9 +15,9 @@ class Drivetrain: public Subsystem{
|
|||||||
BACKLEFT
|
BACKLEFT
|
||||||
};
|
};
|
||||||
void InitDefaultCommand();
|
void InitDefaultCommand();
|
||||||
void DriveMecanum(float, float, float, float, float);
|
void DriveMecanum(double, double, double, double, double);
|
||||||
void DriveArcade(float, float);
|
void DriveArcade(double, double);
|
||||||
void TestMotor(e_motors, float);
|
void TestMotor(e_motors, double);
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
// vim: ts=2:sw=2:et
|
// vim: ts=2:sw=2:et
|
||||||
|
Loading…
Reference in New Issue
Block a user