diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp index c3921f4..cb30c46 100644 --- a/Commands/Autonomous/AutoDrive.cpp +++ b/Commands/Autonomous/AutoDrive.cpp @@ -14,13 +14,13 @@ void AutoDrive::Initialize() { } void AutoDrive::Execute() { //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle) - DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, gyro); + DentRobot::drivetrain->DriveArcade(x, y, z, 0.9, gyro); } bool AutoDrive::IsFinished() { return IsTimedOut(); } void AutoDrive::End() { - DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9); + DentRobot::drivetrain->DriveArcade(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 24d9890..2ca2e4b 100644 --- a/Commands/Autonomous/Turn.cpp +++ b/Commands/Autonomous/Turn.cpp @@ -8,14 +8,14 @@ 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->DriveArcade(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); + DentRobot::drivetrain->DriveArcade(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 e5056e1..00f7559 100644 --- a/Commands/Drivetrain/Drive.cpp +++ b/Commands/Drivetrain/Drive.cpp @@ -22,11 +22,7 @@ void Drive::Execute() { x = -x; y = -y; } - if(DentRobot::oi->GetLeftStick()->GetRawButton(7)) { - DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9); - } else { - DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9); - } + DentRobot::drivetrain->DriveArcade(x, y, z, 0.9); } bool Drive::IsFinished() { return IsTimedOut(); diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp index 205da60..566ead0 100644 --- a/Subsystems/Drivetrain.cpp +++ b/Subsystems/Drivetrain.cpp @@ -3,8 +3,6 @@ #include "../Commands/Drivetrain/Drive.h" Drivetrain::Drivetrain(): Subsystem("Drivetrain") { - rightFront = new CANTalon(DRIVE_FRONT_RIGHT_CAN); - 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); @@ -12,34 +10,31 @@ Drivetrain::Drivetrain(): Subsystem("Drivetrain") { void Drivetrain::InitDefaultCommand() { SetDefaultCommand(new Drive()); } -void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight) { +void Drivetrain::DriveArcade(double x, double y, double z, double sensitivity, bool driveStraight) { double kP = SmartDashboard::GetNumber("Gyro kP"); double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x); double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y); - double correctZ; + double correctZ = z; if(driveStraight) { printf("Driving straight at: %f\n", -gyro->GetAngle()*kP); correctZ = -gyro->GetAngle()*kP; } else { correctZ = -z * 0.5; } + if(DentRobot::oi->GetLeftStick()->GetRawButton(9)) { correctY /= 2.0; } - rightFront->Set((-correctX + correctY - correctZ)); - leftFront->Set((correctX + correctY + correctZ)*-1); - rightRear->Set((correctX + correctY - correctZ)); - leftRear->Set((-correctX + correctY + correctZ)*-1); + int leftPower=((correctY+(correctX+correctZ))/2+1)*127+1; + int rightPower=((correctY-(correctX+correctZ))/2+1)*127+1; + printf("rightPower:%d\n",rightPower); + printf("leftPower:%d\n",leftPower); + leftRear->Set(leftPower); + rightRear->Set(rightPower); } //Used in pretest void Drivetrain::TestMotor(e_motors motor, double power) { switch(motor) { - case FRONTRIGHT: - rightFront->Set(power); - break; - case FRONTLEFT: - leftFront->Set(power); - break; case BACKRIGHT: rightRear->Set(power); break; diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h index 3c20530..5e6b3c6 100644 --- a/Subsystems/Drivetrain.h +++ b/Subsystems/Drivetrain.h @@ -9,9 +9,7 @@ */ class Drivetrain: public Subsystem { private: - CANTalon *rightFront, //