2015-01-16 19:29:55 -05:00
|
|
|
#include "Drivetrain.h"
|
|
|
|
#include "../RobotMap.h"
|
2015-02-03 15:55:11 -05:00
|
|
|
#include "../Commands/Drivetrain/Drive.h"
|
2015-01-16 19:29:55 -05:00
|
|
|
|
2015-03-09 07:25:11 -04:00
|
|
|
Drivetrain::Drivetrain(): Subsystem("Drivetrain"){
|
2015-02-02 19:56:27 -05:00
|
|
|
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);
|
2015-03-10 20:14:38 -04:00
|
|
|
gyro = new Gyro(DRIVE_GYRO_ANALOG);
|
2015-01-16 19:29:55 -05:00
|
|
|
}
|
2015-01-17 12:21:16 -05:00
|
|
|
void Drivetrain::InitDefaultCommand(){
|
2015-01-17 12:55:51 -05:00
|
|
|
SetDefaultCommand(new Drive());
|
2015-01-16 19:29:55 -05:00
|
|
|
}
|
2015-03-10 20:14:38 -04:00
|
|
|
void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight){
|
2015-03-13 19:47:37 -04:00
|
|
|
double kP=SmartDashboard::GetNumber("Gyro kP");
|
2015-02-27 15:44:18 -05:00
|
|
|
double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
|
2015-02-27 16:42:11 -05:00
|
|
|
double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
|
2015-03-10 20:14:38 -04:00
|
|
|
double correctZ;
|
|
|
|
if(driveStraight){
|
2015-03-10 20:49:32 -04:00
|
|
|
printf("Driving straight at: %f\n", -gyro->GetAngle()*kP);
|
2015-03-10 20:14:38 -04:00
|
|
|
correctZ = -gyro->GetAngle()*kP;
|
|
|
|
}else{
|
|
|
|
correctZ = -z * 0.5;
|
|
|
|
}
|
2015-03-09 07:25:11 -04:00
|
|
|
if(DentRobot::oi->GetLeftStick()->GetRawButton(9)){
|
2015-02-28 02:22:43 -05:00
|
|
|
correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
|
|
|
|
}
|
2015-02-02 19:56:27 -05:00
|
|
|
rightFront->Set((-correctX + correctY - correctZ));
|
|
|
|
leftFront->Set((correctX + correctY + correctZ)*-1);
|
|
|
|
rightRear->Set((correctX + correctY - correctZ));
|
|
|
|
leftRear->Set((-correctX + correctY + correctZ)*-1);
|
2015-01-17 10:34:08 -05:00
|
|
|
}
|
2015-02-07 09:17:20 -05:00
|
|
|
//Used in pretest
|
2015-03-01 17:23:23 -05:00
|
|
|
void Drivetrain::TestMotor(e_motors motor, double power){
|
2015-02-07 09:17:20 -05:00
|
|
|
switch(motor){
|
|
|
|
case FRONTRIGHT:
|
|
|
|
rightFront->Set(power);
|
|
|
|
break;
|
|
|
|
case FRONTLEFT:
|
|
|
|
leftFront->Set(power);
|
|
|
|
break;
|
|
|
|
case BACKRIGHT:
|
|
|
|
rightRear->Set(power);
|
|
|
|
break;
|
|
|
|
case BACKLEFT:
|
|
|
|
leftRear->Set(power);
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2015-03-10 20:14:38 -04:00
|
|
|
void Drivetrain::ResetGyro(){
|
|
|
|
gyro->Reset();
|
|
|
|
}
|
2015-02-08 12:26:15 -05:00
|
|
|
// vim: ts=2:sw=2:et
|