diff --git a/DentRobot.cpp b/DentRobot.cpp index eb45120..bb790e8 100644 --- a/DentRobot.cpp +++ b/DentRobot.cpp @@ -35,7 +35,6 @@ void DentRobot::RobotInit(){ SmartDashboard::PutNumber("Auto Tote Distance", 0.5); SmartDashboard::PutNumber("Auto Bin Distance", 0.25); SmartDashboard::PutNumber("TurnAmount", 1.8); - // Elevators SmartDashboard::PutBoolean("Bin Elevator Bottom", false); SmartDashboard::PutBoolean("Bin Elevator Top", false); @@ -43,6 +42,8 @@ void DentRobot::RobotInit(){ SmartDashboard::PutBoolean("Elevator Top", false); //Drive speed SmartDashboard::PutNumber("DriveSpeedReductionThresh", 2.0); + //Gyro + SmartDashboard::PutNumber("Gyro kP", 0.02); } void DentRobot::DisabledPeriodic(){ Scheduler::GetInstance()->Run(); diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp index 4587e8c..4e368f7 100644 --- a/Subsystems/Drivetrain.cpp +++ b/Subsystems/Drivetrain.cpp @@ -14,7 +14,7 @@ void Drivetrain::InitDefaultCommand(){ } 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 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;