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

Implemented gyro (untested)

Browse Source
This commit is contained in:
Austen Adler 2015-03-10 20:14:38 -04:00
parent 996338fb54
commit 007fbc1a3c
6 changed files with 36 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Commands/Autonomous/AutoDrive.cpp
View File

@ -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();

6
Commands/Autonomous/Turn.cpp
View File

@ -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();

4
Commands/Drivetrain/Drive.cpp
View File

@ -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();

1
RobotMap.h
View File

@ -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

16
Subsystems/Drivetrain.cpp
View File

@ -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

30
Subsystems/Drivetrain.h
View File

@ -13,6 +13,7 @@ class Drivetrain: public Subsystem{
*leftFront, //<! Front left motor
*rightRear, //<! Back right motor
*leftRear; //<! Back left motor
Gyro *gyro; //<! Gyro
public:
/**
* @brief Constructs Drivetrain
@ -34,27 +35,24 @@ class Drivetrain: public Subsystem{
/**
* @brief Drives the robot with mecanum
*
* @param double Joystick x value (-1.0 to 1.0)
* @param double Joystick y value (-1.0 to 1.0)
* @param double Joystick z value (-1.0 to 1.0)
* @param double Sensitivity (0.0 to 1.0)
* @param double Gyro value (unused)
* @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
*/
void DriveMecanum(double, double, double, double, double);
/**
* @brief Drives the robot with arcade drive
*
* @param double Joystick x value (-1.0 to 1.0)
* @param double Joystick y value (-1.0 to 1.0)
*/
void DriveArcade(double, double);
void DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight=false);
/**
* @brief Tests one motor
*
* @param e_motors Motor to test
* @param double Power to set motor
* @param motor Motor to test
* @param power Power to set motor
*/
void TestMotor(e_motors, double);
void TestMotor(e_motors motor, double power);
/**
* @brief Sets the gyro value to 0.0
*/
void ResetGyro();
};
#endif
// vim: ts=2:sw=2:et