public void driveToBridge() {
System.out.println("Val: " + potentiometer.getVoltage());
if (Autonomousflag) {
if (potentiometer.getVoltage() < 5) { // line
shoot();
Timer.delay(7);
shoot.set(0);
collect.set(0);
driveTrain.tankDrive(-speed, speed);
Timer.delay(1);
driveTrain.tankDrive(speed, speed);
Timer.delay(1.2);
driveTrain.tankDrive(0, 0);
Timer.delay(0.9);
bridge.set(-1);
} else if (potentiometer.getVoltage() > 5) { // left
shoot();
}
Autonomousflag = false;
} else {
driveTrain.tankDrive(0, 0);
}
}
/** This function is called periodically during test mode */
public void testPeriodic() {
jag1.set(0);
jag2.set(0);
jag3.set(0);
jag4.set(0);
System.out.println(compressor.getPressureSwitchValue());
}
/**
* Method for closing the control loop.
*
* @param cmd Reference signal (setpoint)
* @param act Feedback signal (measured response)
*/
public void DoControl(double cmd, double act) {
// Handle relay version first
if (relay != null) {
if (cmd - act > tolerance) {
relay.set(Relay.Value.kForward);
} else if (cmd - act < -tolerance) {
relay.set(Relay.Value.kReverse);
} else {
relay.set(Relay.Value.kOff);
}
} else {
if (cmd - act > tolerance) {
if (reverseCmd) {
jag.set(-speed);
} else {
jag.set(speed);
}
} else if (cmd - act < -tolerance) {
if (reverseCmd) {
jag.set(speed);
} else {
jag.set(-speed);
}
} else {
jag.set(0.0);
}
}
}
public void autoDrive() {
double speed = 0;
if (distPID.isEnable()) {
speed = distPID.get();
}
double turn = 0;
if (turnPID.isEnable()) {
turn = turnPID.get();
}
if (speed > 0) {
drive.drive(speed, turn);
} else {
if (turn > 0) {
// turn right
tankDrive(turn, -turn);
} else if (turn < 0) {
tankDrive(-turn, turn);
}
}
SmartDashboard.putDouble("left pwm", leftJag.get());
SmartDashboard.putDouble("right pwm", rightJag.get());
SmartDashboard.putDouble("speed", RobotMap.roundtoTwo(speed));
SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(turn));
SmartDashboard.putDouble("turn error", turnPID.getError());
// System.out.println("driving " + speed + " " + turn);
}
/** Handles periodic operations. Should be called periodically. */
public void loop() {
ds.setDigitalOut(8, limitSwitch.get());
final int tolerance =
10; // the number of degrees, in either direction, of tolerance for motor movement
if (!isAccepting) {
if (topTarget > topEncoder.get() && Math.abs(topTarget - topEncoder.get()) > tolerance) {
int error = Math.abs(topTarget - topEncoder.get());
topMotor.set(-limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Backwards", "Gripper Top Motor");
} else if (topTarget < topEncoder.get()
&& Math.abs(topTarget - topEncoder.get()) > tolerance) {
double error = Math.abs(topTarget - topEncoder.get());
topMotor.set(limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Forward", "Gripper Top Motor");
} else {
topMotor.set(0);
}
if (bottomTarget > bottomEncoder.get()
&& Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
double error = Math.abs(bottomTarget - bottomEncoder.get());
bottomMotor.set(-limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Backwards", "Gripper Bottom Motor");
} else if (bottomTarget < bottomEncoder.get()
&& Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
double error = Math.abs(bottomTarget - bottomEncoder.get());
bottomMotor.set(limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Forward", "Gripper Bottom Motor");
} else {
bottomMotor.set(0);
// SmartDashboard.log("Stopped", "Gripper Bottom Motor");
}
}
// if the gripper is in accept mode, it needs to stop when the limit switch is pressed
if (isAccepting && !limitSwitch.get()) {
// and also by simply stopping them
topMotor.set(0);
bottomMotor.set(0);
isAccepting = false;
// stop the motors by making the target positions equal to the current positions
topTarget = topEncoder.get();
bottomTarget = bottomEncoder.get();
} else if (isAccepting && limitSwitch.get()) {
// if it is accepting and the limit switch is not pressed
topMotor.set(1);
bottomMotor.set(-1);
}
}
public void stopMotor() {
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon.set(0.0);
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar.set(0.0);
}
}
/*
* requires a magnitude between -1 and 1 inclusive:
* assumes that the angle is in degrees
* calculates and sets the motor speeds for a given polar vector
* allows for rotation while driving [-1,1]
*/
private void driveMecanum() {
/*
* Does the computation of each motor speed value ever exceed the
* range of -1.0, 1.0? Just curious.
*
* - Mr. Ward
*/
// RRLogger.logDebug(this.getClass(),"driveMecanum()", "driveMecanum()" );
frontLeftMotor.set(-(l_magnitude + rotation) * Math.cos(Math.toRadians((l_angle + 45))));
frontRightMotor.set((l_magnitude - rotation) * Math.sin(Math.toRadians(l_angle + 45)));
backLeftMotor.set(-(l_magnitude + rotation) * Math.sin(Math.toRadians(l_angle + 45)));
backRightMotor.set((l_magnitude - rotation) * Math.cos(Math.toRadians(l_angle + 45)));
}
/*
*
* @param direction -1 to draw the arm back, +1 to rotate motor forward away from arm.
*
*/
public void reload(int direction, double backSpeed, double frontSpeed) {
final int REVERSE = -1;
if (direction == DRAW_KICKER_BACK) {
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon.set(
REVERSE
* (backSpeed)); // TODO: Check to make sure the motors will draw back when recieving
// a positive voltage.
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar.set(REVERSE * backSpeed);
}
} else if (direction == ROTATE_KICKER_FORWARD) {
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon.set(frontSpeed);
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar.set(frontSpeed);
}
}
}
protected void usePIDOutput(double output) {
double speed = shootingMotor.get() + output;
if (speed > .75 && this.getSetpoint() < 2600) {
speed = .7;
} else if (speed > .85 && this.getSetpoint() < 3600) {
speed = .8;
} else if (speed < 0) {
speed = 0;
}
shooterSet(speed);
}
private void driveCartesianTank() {
// sensitivity
double tuneForward = 1 * sensitivity;
double tuneRight = 1 * sensitivity;
double tuneClockwise = .75 * sensitivity;
double Yf = (forward + right_forward) / 2;
double Yt = (forward - right_forward) / 2;
// can change this to also use the average of X sticks
// currently does not use right x value
double front_left = tuneForward * Yf + tuneClockwise * Yt + tuneRight * right;
double front_right = tuneForward * Yf - tuneClockwise * Yt - tuneRight * right;
double rear_left = tuneForward * Yf + tuneClockwise * Yt - tuneRight * right;
double rear_right = tuneForward * Yf - tuneClockwise * Yt + tuneRight * right;
// Normalize
double max = Math.abs(front_left);
if (Math.abs(front_right) > max) {
max = Math.abs(front_right);
}
if (Math.abs(rear_left) > max) {
max = Math.abs(rear_left);
}
if (Math.abs(rear_right) > max) {
max = Math.abs(rear_right);
}
if (max > 1) {
front_left /= max;
front_right /= max;
rear_left /= max;
rear_right /= max;
}
frontLeftMotor.set(front_left);
frontRightMotor.set(-front_right);
backLeftMotor.set(rear_left);
backRightMotor.set(-rear_right);
}
public ShooterSubsystem() {
kickerLatch = new DoubleSolenoid(RobotMap.KICKER_LATCH_RELEASE, RobotMap.KICKER_LATCH_RELOAD);
kickerLatch.set(DoubleSolenoid.Value.kForward);
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon = new Talon(RobotMap.KICKER_MOTOR);
kickerTalon.set(0.0);
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar = new Jaguar(RobotMap.KICKER_MOTOR);
kickerJaguar.set(0.0);
}
kickerLimitSwitch = new DigitalInput(RobotMap.KICKER_RETRACTED_SWITCH);
kickerMotorReturnSwitch = new DigitalInput(RobotMap.KICKER_MOTOR_RETURN_SWITCH);
}
/*
* Uses a different or modified computation to resolve motor speeds
* without trigonometric functions
* Hopefully this will provide increased efficiency as well as
* more precise driving
*/
private void driveCartesianTest() {
double front_left = forward + clockwise + right;
double front_right = forward - clockwise - right;
double rear_left = forward + clockwise - right;
double rear_right = forward - clockwise + right;
double max = Math.abs(front_left);
max = Math.max(Math.abs(front_right), max);
max = Math.max(Math.abs(rear_left), max);
max = Math.max(Math.abs(rear_right), max);
if (max > 1) {
front_left /= max;
front_right /= max;
rear_left /= max;
rear_right /= max;
}
frontLeftMotor.set(front_left);
frontRightMotor.set(-front_right);
backLeftMotor.set(rear_left);
backRightMotor.set(-rear_right);
}
public void teleopPeriodic() {
watchdog.feed(); // feed the watchdog
// Toggle drive mode
if (!driveToggle && left.getRawButton(2)) {
driveMode = (driveMode + 1) % 3;
driveToggle = true;
} else if (driveToggle && !left.getRawButton(2)) driveToggle = false;
// Print drive mode to DS & send values to Jaguars
switch (driveMode) {
case 0:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Tank ");
red.set(left.getY() + gamePad.getY());
black.set(-right.getY() - gamePad.getThrottle());
break;
case 1:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Arcade");
red.set(right.getX() - right.getY());
black.set(right.getX() + right.getY());
break;
case 2:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Kaj ");
red.set(left.getX() - right.getY());
black.set(left.getX() + right.getY());
break;
} /**/
dsLCD.println(
DriverStationLCD.Line.kUser3,
1,
"1" + photoreceptorL.get() + "2" + photoreceptorM.get() + "3" + photoreceptorR.get());
dsLCD.println(DriverStationLCD.Line.kUser4, 1, "Encoder in 4,5: " + leftEncoder.get() + " ");
dsLCD.println(
DriverStationLCD.Line.kUser5, 1, "Encoder in 6,7: " + rightEncoder.get() + " ");
dsLCD.updateLCD();
}
public void adjustHood(double value) {
hood.set(value);
}
public void stop() {
shooter.stopMotor();
}
/*
* Drives the left and right wheels separately, with its repsective xbox stick
* Allows for more precise rotation
*/
private void driveTank() {
frontLeftMotor.set(-(l_magnitude + rotation) * Math.cos(Math.toRadians((l_angle + 45))));
frontRightMotor.set((r_magnitude - rotation) * Math.sin(Math.toRadians(r_angle + 45)));
backLeftMotor.set(-(l_magnitude + rotation) * Math.sin(Math.toRadians(l_angle + 45)));
backRightMotor.set((r_magnitude - rotation) * Math.cos(Math.toRadians(r_angle + 45)));
}
/**
* Sets the motor to move backwards at a specific speed.
*
* @param speed the desired to move at
*/
public void reverse(int speed) {
jag.set(-speed);
currentState = Component.ComponentState.COMPONENT_REVERSE;
}
public void shooterSet(double speed) {
shootingMotor.set(speed);
shootingMotor2.set(speed);
}
public void stopShooter() {
shootingMotor.set(0);
shootingMotor2.set(0);
}
public void stop() {
frontLeftMotor.set(0);
frontRightMotor.set(0);
backLeftMotor.set(0);
backRightMotor.set(0);
}
/**
* Sets the motor to move forward at a specific speed.
*
* @param speed the desired speed to move at
*/
public void forward(int speed) {
jag.set(speed);
currentState = Component.ComponentState.COMPONENT_FORWARD;
}
/**
* Sets the PWM value of the motor component.
*
* @param speed the motor speed between -1.0 and 1.0
*/
public void set(double speed) {
jag.set(speed);
}
/**
* Gets the current PWM value of the motor component.
*
* @return the current PWM value of the component
*/
public double get() {
return jag.get();
}
/**
* Directly specify motor speed.
*
* @param cmd speed command to issue to the jaguars.
*/
private void SetMotors(double cmd) {
motor1.set(-cmd);
motor2.set(-cmd);
}
public void destroy() {
jag.free();
}
public void stop() {
jag1.set(0);
jag2.set(0);
jag3.set(0);
jag4.set(0);
}
public void shoot() {
shoot.set(1); // Full power
Timer.delay(6);
collect.set(-1); // FIXED mechanical
}
/** This function is called periodically during autonomous */
public void autonomousPeriodic() {
red.set(1);
black.set(1);
}
public void autoForward() {
jag1.set(-AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
jag2.set(-AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
jag3.set(AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
jag4.set(AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
}
public void run(double value) {
shooter.set(value);
}
