mirror of
https://github.com/team2059/Dent
synced 2024-12-18 20:52:29 -05:00
Formatted commas, swapped AutoDrive x and y (untested)
This commit is contained in:
parent
2a3ddd6041
commit
1494fdceba
@ -1,17 +1,17 @@
|
||||
#include "AutoDrive.h"
|
||||
#include "../../DentRobot.h"
|
||||
// Drive for a short while then stop. Just for testing
|
||||
AutoDrive::AutoDrive(double duration, double xtmp=-0.75, double ytmp=0) : Command("AutoDrive"){
|
||||
AutoDrive::AutoDrive(double duration, double xtmp=0, double ytmp=-0.75) : Command("AutoDrive"){
|
||||
Requires(DentRobot::drivetrain);
|
||||
SetTimeout(duration);
|
||||
speed=xtmp;
|
||||
strafe=ytmp;
|
||||
x=xtmp;
|
||||
y=ytmp;
|
||||
}
|
||||
void AutoDrive::Initialize(){
|
||||
}
|
||||
void AutoDrive::Execute(){
|
||||
//X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
|
||||
DentRobot::drivetrain->DriveMecanum(strafe,speed,0.0,0.9,0.0);
|
||||
DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, 0.0);
|
||||
}
|
||||
bool AutoDrive::IsFinished(){
|
||||
return IsTimedOut();
|
||||
|
@ -8,7 +8,7 @@
|
||||
|
||||
class AutoDrive: public Command{
|
||||
private:
|
||||
double speed, strafe;
|
||||
double x, y;
|
||||
public:
|
||||
AutoDrive(double);
|
||||
AutoDrive(double, double, double);
|
||||
|
@ -15,20 +15,20 @@ Autonomous::Autonomous(int seq){
|
||||
switch(seq){
|
||||
case 0:
|
||||
// Just for testing
|
||||
AddSequential(new AutoDrive(.5,0,.25));
|
||||
// Strafe at .25 power
|
||||
AddSequential(new AutoDrive(0.5, 0.25, 0.0));
|
||||
break;
|
||||
case 1:
|
||||
// Wait a desigated value, drive to Auto Zone (TM)
|
||||
// Drive to Auto Zone (TM)
|
||||
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.8,0.0));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, 0.8));
|
||||
break;
|
||||
case 2:
|
||||
// Wait a desigated value, drive to Auto Zone (TM)
|
||||
// Collect a tote, turn, drive to Auto Zone (TM)
|
||||
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
||||
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
||||
AddSequential(new BinRaise(1.2));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"),0.75,0.0));
|
||||
//AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0,-1.0));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, 0.75));
|
||||
AddSequential(new BinLower(1.0));
|
||||
AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
|
||||
break;
|
||||
@ -37,22 +37,22 @@ Autonomous::Autonomous(int seq){
|
||||
printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
|
||||
Wait(SmartDashboard::GetNumber("Auto Wait Time"));
|
||||
printf("Done");
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, -0.75));
|
||||
AddSequential(new CollectTote());
|
||||
AddSequential(new Raise());
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, -0.75));
|
||||
AddSequential(new CollectTote());
|
||||
AddSequential(new Lower());
|
||||
AddSequential(new Raise());
|
||||
printf("Three totes?: %d\n", SmartDashboard::GetBoolean("Three totes"));
|
||||
if(SmartDashboard::GetBoolean("Three totes")){
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, -0.75));
|
||||
AddSequential(new CollectTote());
|
||||
AddSequential(new Lower());
|
||||
AddSequential(new Raise());
|
||||
}
|
||||
AddSequential(new Turn(90));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75,0));
|
||||
AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, -0.75));
|
||||
AddSequential(new ReleaseTote());
|
||||
break;
|
||||
default:
|
||||
|
@ -3,7 +3,7 @@
|
||||
#include "AutoDrive.h"
|
||||
#include "../Collector/RollIn.h"
|
||||
CollectTote::CollectTote(){
|
||||
AddParallel(new AutoDrive(1.0, -0.75,0));
|
||||
AddParallel(new AutoDrive(1.0, -0.75, 0.0));
|
||||
AddSequential(new RollIn(1.0));
|
||||
}
|
||||
// vim: ts=2:sw=2:et
|
||||
|
@ -4,6 +4,6 @@
|
||||
#include "../Collector/RollOut.h"
|
||||
ReleaseTote::ReleaseTote(){
|
||||
AddParallel(new RollOut());
|
||||
AddParallel(new AutoDrive(0.5, 0.75,0));
|
||||
AddParallel(new AutoDrive(0.5, 0, 0.75));
|
||||
}
|
||||
// vim: ts=2:sw=2:et
|
||||
|
@ -8,7 +8,7 @@ void RollOut::Initialize(){
|
||||
void RollOut::Execute(){
|
||||
//TODO check this value to move the motors in the right direction
|
||||
// Devide by 2 twice because this speed should be half the collector speed
|
||||
DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle()*.8);
|
||||
DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle() * 0.8);
|
||||
}
|
||||
bool RollOut::IsFinished(){
|
||||
return IsTimedOut();
|
||||
|
@ -7,10 +7,10 @@ void Drive::Initialize(){
|
||||
}
|
||||
void Drive::Execute(){
|
||||
double x, y, z;
|
||||
//Code to lock the x axis when not holding button 1
|
||||
x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
|
||||
z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
|
||||
y = DentRobot::oi->GetLeftStick()->GetRawAxis(1);
|
||||
//Code to lock the x axis when not holding button 1
|
||||
//if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
|
||||
// x=0;
|
||||
//}
|
||||
@ -18,7 +18,7 @@ void Drive::Execute(){
|
||||
// y=0;
|
||||
//}
|
||||
//X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
|
||||
DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
|
||||
DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, 0.0);
|
||||
}
|
||||
bool Drive::IsFinished(){
|
||||
return IsTimedOut();
|
||||
|
@ -14,7 +14,7 @@ void Drivetrain::InitDefaultCommand(){
|
||||
void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){
|
||||
double correctY = (sensitivity*(pow(y, 3))+(1-sensitivity)*y);
|
||||
double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
|
||||
double correctZ = -z *.5;
|
||||
double correctZ = -z * 0.5;
|
||||
rightFront->Set((-correctX + correctY - correctZ));
|
||||
leftFront->Set((correctX + correctY + correctZ)*-1);
|
||||
rightRear->Set((correctX + correctY - correctZ));
|
||||
|
Loading…
Reference in New Issue
Block a user