From 007fbc1a3cee70f7f536db7a82a613b1c2129519 Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Tue, 10 Mar 2015 20:14:38 -0400 Subject: [PATCH] Implemented gyro (untested) --- Commands/Autonomous/AutoDrive.cpp | 6 +++--- Commands/Autonomous/Turn.cpp | 6 +++--- Commands/Drivetrain/Drive.cpp | 4 ++-- RobotMap.h | 1 + Subsystems/Drivetrain.cpp | 16 +++++++++++++--- Subsystems/Drivetrain.h | 30 ++++++++++++++---------------- 6 files changed, 36 insertions(+), 27 deletions(-) diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp index 2525eb8..9832bf6 100644 --- a/Commands/Autonomous/AutoDrive.cpp +++ b/Commands/Autonomous/AutoDrive.cpp @@ -10,14 +10,14 @@ AutoDrive::AutoDrive(double duration, double xtmp, double ytmp): Command("AutoDr void AutoDrive::Initialize(){ } void AutoDrive::Execute(){ - //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro - DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, 0.0); + //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle) + DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9); } bool AutoDrive::IsFinished(){ return IsTimedOut(); } void AutoDrive::End(){ - DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9, 0.0); + DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9); } void AutoDrive::Interrupted(){ End(); diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp index b44dab9..2413e6c 100644 --- a/Commands/Autonomous/Turn.cpp +++ b/Commands/Autonomous/Turn.cpp @@ -7,15 +7,15 @@ Turn::Turn(double timeout): Command("Turn"){ 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.6, 0.9, 0.0); + //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle) + DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9); } bool Turn::IsFinished(){ return IsTimedOut(); } void Turn::End(){ // Stop driving - DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9, 0.0); + DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9); } void Turn::Interrupted(){ End(); diff --git a/Commands/Drivetrain/Drive.cpp b/Commands/Drivetrain/Drive.cpp index 71470eb..692753b 100644 --- a/Commands/Drivetrain/Drive.cpp +++ b/Commands/Drivetrain/Drive.cpp @@ -17,8 +17,8 @@ void Drive::Execute(){ //if(DentRobot::oi->GetLeftStick()->GetRawButton(2)){ // y=0; //} - //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro - DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, 0.0); + //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle) + DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9); } bool Drive::IsFinished(){ return IsTimedOut(); diff --git a/RobotMap.h b/RobotMap.h index 6e510c9..eeaed04 100644 --- a/RobotMap.h +++ b/RobotMap.h @@ -28,6 +28,7 @@ #define DRIVE_BACK_LEFT 3 #define DRIVE_FRONT_RIGHT 4 #define DRIVE_BACK_RIGHT 5 +#define DRIVE_GYRO_ANALOG 0 // Collector #define COLLECTOR_RAMP 7 diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp index 4f4dae9..c0d127f 100644 --- a/Subsystems/Drivetrain.cpp +++ b/Subsystems/Drivetrain.cpp @@ -7,14 +7,22 @@ Drivetrain::Drivetrain(): Subsystem("Drivetrain"){ leftFront = new Victor(DRIVE_FRONT_LEFT); rightRear = new Victor(DRIVE_BACK_RIGHT); leftRear = new Victor(DRIVE_BACK_LEFT); + gyro = new Gyro(DRIVE_GYRO_ANALOG); } void Drivetrain::InitDefaultCommand(){ SetDefaultCommand(new Drive()); } -void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, double gyro){ +void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight){ + //TODO: Find the correct value for kP + double kP=0.02; double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x); double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y); - double correctZ = -z * 0.5; + double correctZ; + if(driveStraight){ + correctZ = -gyro->GetAngle()*kP; + }else{ + correctZ = -z * 0.5; + } if(DentRobot::oi->GetLeftStick()->GetRawButton(9)){ correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh"); } @@ -23,7 +31,6 @@ void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, rightRear->Set((correctX + correctY - correctZ)); leftRear->Set((-correctX + correctY + correctZ)*-1); } - //Used in pretest void Drivetrain::TestMotor(e_motors motor, double power){ switch(motor){ @@ -43,4 +50,7 @@ void Drivetrain::TestMotor(e_motors motor, double power){ break; } } +void Drivetrain::ResetGyro(){ + gyro->Reset(); +} // vim: ts=2:sw=2:et diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h index 47a392e..4e1e454 100644 --- a/Subsystems/Drivetrain.h +++ b/Subsystems/Drivetrain.h @@ -13,6 +13,7 @@ class Drivetrain: public Subsystem{ *leftFront, //