From 2a25a3c6c637bb9f7609f04e3912242c2fdd135e 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 2f25d55..73986d6 100644 --- a/RobotMap.h +++ b/RobotMap.h @@ -28,6 +28,7 @@ #define DRIVE_BACK_LEFT_CAN 3 #define DRIVE_FRONT_RIGHT_CAN 4 #define DRIVE_BACK_RIGHT_CAN 5 +#define DRIVE_GYRO_ANALOG 0 // Collector #define COLLECTOR_RAMP_CAN 7 diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp index fa546b7..ac93f2c 100644 --- a/Subsystems/Drivetrain.cpp +++ b/Subsystems/Drivetrain.cpp @@ -7,14 +7,22 @@ Drivetrain::Drivetrain(): Subsystem("Drivetrain"){ leftFront = new CANTalon(DRIVE_FRONT_LEFT_CAN); rightRear = new CANTalon(DRIVE_BACK_RIGHT_CAN); leftRear = new CANTalon(DRIVE_BACK_LEFT_CAN); + 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 e6ff9b9..6d8459e 100644 --- a/Subsystems/Drivetrain.h +++ b/Subsystems/Drivetrain.h @@ -13,6 +13,7 @@ class Drivetrain: public Subsystem{ *leftFront, //