package org.usfirst.frc.team3501.robot;
+<<<<<<< HEAD
+=======
+import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
+>>>>>>> fix bugs
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() {
+<<<<<<< HEAD
+=======
+ Scheduler.getInstance().add(new DriveDistance(25, 10));
+>>>>>>> fix bugs
}
@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;
public DriveDistance(double distance, double motorVal) {
requires(driveTrain);
+ this.maxTimeOut = maxTimeOut;
+ this.target = distance;
+
this.driveP = driveTrain.driveP;
this.driveI = driveTrain.driveI;
this.driveD = driveTrain.driveD;
driveTrain.printEncoderOutput();
// 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 edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
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;
Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE);
rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER,
Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE);
+
+ 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() {
// System.out.println("left: " + getLeftEncoderDistance());
// System.out.println("right: " + getRightEncoderDistance());
System.out.println(getAvgEncoderDistance());
+ System.out.println("left: " + getLeftEncoderDistance());
+ System.out.println("right: " + getRightEncoderDistance());
}
public double getAvgEncoderDistance() {