package org.usfirst.frc.team3501.robot.commands.driving;
+import org.usfirst.frc.team3501.robot.MathLib;
import org.usfirst.frc.team3501.robot.Robot;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
import org.usfirst.frc.team3501.robot.utils.PIDController;
this.maxTimeOut = maxTimeOut;
this.target = distance;
+ private double target;
+ private double gyroP;
+ private double gyroI;
+ private double gyroD;
+
+ private double driveP;
+ private double driveI;
+ private double driveD;
+
+ public DriveDistance(double distance, double motorVal) {
+ requires(driveTrain);
this.driveP = driveTrain.driveP;
this.driveI = driveTrain.driveI;
this.driveD = driveTrain.driveD;
this.driveTrain.setMotorValues(leftDrive, rightDrive);
driveTrain.printEncoderOutput();
+ // System.out.println("turn: " + xVal);
+ double leftDrive = MathLib.calcLeftTankDrive(-xVal, yVal);
+ double rightDrive = MathLib.calcRightTankDrive(xVal, -yVal);
+
+ this.driveTrain.setMotorValues(leftDrive, rightDrive);
+
+ System.out.println(driveTrain.getAvgEncoderDistance());
+ // System.out.println("motorval: " + yVal);
}
@Override
import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
+import org.usfirst.frc.team3501.robot.utils.BNO055;
import com.ctre.CANTalon;
public static double driveP = 0.006, driveI = 0.001, driveD = -0.002;
public static double defaultGyroP = 0.004, defaultGyroI = 0.0013,
defaultGyroD = -0.005;
+ public static double driveP = 0.008, driveI = 0.001, driveD = -0.002;
+ public static double defaultGyroP = 0.009, defaultGyroI = 0.00000,
+ defaultGyroD = -0.000;
private double gyroZero = 0;
public static final double WHEEL_DIAMETER = 6; // inches
private final DoubleSolenoid leftGearPiston, rightGearPiston;
private ADXRS450_Gyro imu;
+ private BNO055 imu;
private DriveTrain() {
// MOTOR CONTROLLERS
rightGearPiston.set(gear);
}
+ public double getAngle() {
+ if (!this.imu.isInitialized())
+ return -1;
+ return this.imu.getHeading() - this.gyroZero;
+ }
+
+ public void resetGyro() {
+ this.gyroZero = this.getAngle();
+
+ }
+
+ public double getZeroAngle() {
+ return this.gyroZero;
+ }
+
@Override
protected void initDefaultCommand() {
setDefaultCommand(new JoystickDrive());