team2059/dent
2
0
Fork 0
mirror of https://github.com/team2059/Dent synced 2024-12-18 20:52:29 -05:00
Code Issues Releases Wiki Activity

fixes for frankie

Browse Source
This commit is contained in:
Adam Long 2016-05-16 13:38:59 +00:00
parent 4dd144d377
commit 7df77f9314
5 changed files with 13 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Commands/Autonomous/AutoDrive.cpp
View File

@ -14,13 +14,11 @@ 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);
}
bool AutoDrive::IsFinished(){
return IsTimedOut();
}
void AutoDrive::End(){
DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9);
}
void AutoDrive::Interrupted(){
End();

2
Commands/Autonomous/Turn.cpp
View File

@ -8,14 +8,12 @@ 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);
}
bool Turn::IsFinished(){
return IsTimedOut();
}
void Turn::End(){
// Stop driving
DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.0, 0.9);
}
void Turn::Interrupted(){
End();

18
Commands/Drivetrain/Drive.cpp
View File

@ -10,23 +10,7 @@ void Drive::Execute(){
x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
y = -DentRobot::oi->GetLeftStick()->GetRawAxis(1);
z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
// Lock the x axis when not holding button 1
//if(DentRobot::oi->GetLeftStick()->GetRawButton(1)){
// x = 0;
//}
//if(DentRobot::oi->GetLeftStick()->GetRawButton(2)){
// y = 0;
//}
//X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
if(DentRobot::oi->GetLeftStick()->GetRawButton(11)){
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->DriveMecanum(x, y, z);
}
bool Drive::IsFinished(){
return IsTimedOut();

50
Subsystems/Drivetrain.cpp
View File

@ -3,52 +3,20 @@
#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);
rightFront = new Talon(DRIVE_FRONT_RIGHT_CAN);
leftFront = new Talon(DRIVE_FRONT_LEFT_CAN);
rightRear = new Talon(DRIVE_BACK_RIGHT_CAN);
leftRear = new Talon(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, 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;
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 /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
}
rightFront->Set((-correctX + correctY - correctZ));
leftFront->Set((correctX + correctY + correctZ)*-1);
rightRear->Set((correctX + correctY - correctZ));
leftRear->Set((-correctX + correctY + correctZ)*-1);
}
//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;
case BACKLEFT:
leftRear->Set(power);
break;
default:
break;
}
void Drivetrain::DriveMecanum(double x, double y, double z){
rightFront->Set(y+(x+z));
rightRear->Set(y+(x+z));
leftFront->Set(-y+(x+z));
leftRear->Set(-y+(x+z));
}
void Drivetrain::ResetGyro(){
gyro->Reset();

22
Subsystems/Drivetrain.h
View File

@ -4,12 +4,12 @@
#include "WPILib.h"
/**
* @brief Drives the robot
*
*
* 4 wheel mecanum drive
*/
class Drivetrain: public Subsystem{
private:
CANTalon *rightFront, //<! Front right motor
Talon *rightFront, //<! Front right motor
*leftFront, //<! Front left motor
*rightRear, //<! Back right motor
*leftRear; //<! Back left motor
@ -32,23 +32,7 @@ class Drivetrain: public Subsystem{
* @brief No action
*/
void InitDefaultCommand();
/**
* @brief Drives the robot with mecanum
*
* @param x Joystick x value (-1.0 to 1.0)
* @param y Joystick y value (-1.0 to 1.0)
* @param z Joystick z value (-1.0 to 1.0)
* @param sensitivity Sensitivity (0.0 to 1.0)
* @param driveStraight Overrides z value to correct for motor lag (default: false)
*/
void DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight = false);
/**
* @brief Tests one motor
*
* @param motor Motor to test
* @param power Power to set motor
*/
void TestMotor(e_motors motor, double power);
void DriveMecanum(double x, double y, double z);
/**
* @brief Sets the gyro value to 0.0
*/