package org.usfirst.frc.team3501.robot;
-import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
+import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
import org.usfirst.frc.team3501.robot.subsystems.Intake;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;
@Override
public void autonomousInit() {
- Scheduler.getInstance().add(new TimeDrive(1.5, 0.4));
+ Scheduler.getInstance().add(new DriveDistance(25, 10));
}
@Override
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;
private double driveI;
private double driveD;
- public DriveDistance(double distance, double motorVal) {
+ public DriveDistance(double distance, double maxTimeOut) {
requires(driveTrain);
+ this.maxTimeOut = maxTimeOut;
+ this.target = distance;
+
this.driveP = driveTrain.driveP;
this.driveI = driveTrain.driveI;
this.driveD = driveTrain.driveD;
.calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
}
// System.out.println("turn: " + xVal);
- double leftDrive = MathLib.calcLeftTankDrive(-xVal, yVal);
- double rightDrive = MathLib.calcRightTankDrive(xVal, -yVal);
+ double leftDrive = yVal - xVal;
+ double rightDrive = yVal + xVal;
this.driveTrain.setMotorValues(leftDrive, rightDrive);
- System.out.println(driveTrain.getAvgEncoderDistance());
+ driveTrain.printEncoderOutput();
// System.out.println("motorval: " + yVal);
}
import com.ctre.CANTalon;
import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
// ROBOT DRIVE
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+ this.imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
+ BNO055.vector_type_t.VECTOR_EULER, Port.kOnboard, (byte) 0x28);
+ gyroZero = imu.getHeading();
}
public static DriveTrain getDriveTrain() {
return rightEncoder.getDistance();
}
+ public void printEncoderOutput() {
+ System.out.println("left: " + getLeftEncoderDistance());
+ System.out.println("right: " + getRightEncoderDistance());
+ }
+
public double getAvgEncoderDistance() {
return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
}