/** This function is called periodically during operator control */
public void teleopPeriodic() {
// The order of the update methods is important. Besides making a nice slope of periods, the
// motors and sensors have to update first
controller.update(motorManagers);
sensors.update();
shoot.update(motorManagers, controller, sensors, SSS);
arm.update(motorManagers, controller);
// System.out.println("Encoder Raw: " + sensors.getEncoderValues());
// System.out.println("Encoder Count: " + sensors.getEncoderCount());
// System.out.println("Encoder Rate of Rotation: " + sensors.getEncoderRate());
// System.out.println("Encoder Distance: " + sensors.getEncoderDistance());
// System.out.println("Degrees per Second: " + sensors.getEncoderAngularSpeed());
// System.out.println("StringPot: " + sensors.getStringPot());
System.out.println("Ultrasonic Range Finder (Inches): " + sensors.getSonicRangeInches());
// System.out.println("Servo: " + shoot.pusher.getAngle());
sensors.resetGyroAngles(controller);
System.out.println("Gyro XY: " + sensors.getGyroXYAngle());
System.out.println("Gyro Z: " + sensors.getGyroZAngle());
System.out.println("wheel Angle: " + sensors.encoder.getEncoderAngle());
SmartDashboard.putNumber("Gyro XY", sensors.getGyroXYAngle());
SmartDashboard.putNumber("Gyro Z", sensors.getGyroZAngle());
SmartDashboard.putNumber("Distance", sensors.getSonicRangeInches());
}
@Override
public void update(SensorManager sensors) {
if (sensors.getGyroZAngle() <= 5 && !selectorCheck) caseSelector = 1;
switch (caseSelector) {
case 1: // Approach the defense ______
controller.forward(speed1); // | |
// V |
if (sensors.getSonicRangeInches() == 6) // How far away from the drawbridge to stop
{
controller.stop();
caseSelector = 2;
// time = System.currentTimeMillis();
}
break;
case 2: // Manage arm movement and prepare to cross - steps 3-7 on sheet
// Lower arm
// Move backward
// Lower arm further
// Lower shooter
// Raise arm
// Move forward
if (joy.getTiltArm() == basedegrees1) dr.tiltArm(0);
else if (joy.getTiltArm() < basedegrees1) dr.tiltArm(0.5); // plz test numbers
else if (joy.getTiltArm() > basedegrees1)
dr.tiltArm(-0.5); // plz test numbers, idk if positve or negative
if (joy.getTiltJoint() == jointdegrees1) dr.tiltJoint(0);
else if (joy.getTiltJoint() < jointdegrees1) dr.tiltJoint(0.5); // plz test numbers
else if (joy.getTiltJoint() > jointdegrees1)
dr.tiltJoint(-0.5); // plz test numbers, idk if positve or negative
if (joy.getTiltArm() == basedegrees1 && joy.getTiltJoint() == jointdegrees1) {
dr.tiltArm(0);
dr.tiltJoint(0);
time = System.currentTimeMillis();
caseSelector = 3;
}
break;
case 3:
controller.forward(-speed2);
if (joy.getTiltArm() == basedegrees2) dr.tiltArm(0);
else if (joy.getTiltArm() < basedegrees2) dr.tiltArm(0.5); // plz test numbers
else if (joy.getTiltArm() > basedegrees2)
dr.tiltArm(-0.5); // plz test numbers, idk if positve or negative
if (joy.getTiltJoint() == jointdegrees2) dr.tiltJoint(0);
else if (joy.getTiltJoint() < jointdegrees2) dr.tiltJoint(0.5); // plz test numbers
else if (joy.getTiltJoint() > jointdegrees2)
dr.tiltJoint(-0.5); // plz test numbers, idk if positve or negative
if (joy.getTiltArm() == basedegrees2
&& joy.getTiltJoint() == jointdegrees2
&& System.currentTimeMillis() - time >= 3000) {
dr.tiltArm(0);
dr.tiltJoint(0);
controller.stop();
caseSelector = 4;
}
break;
case 4:
if (joy.getTiltArm() == 180 - basedegrees2) dr.tiltArm(0);
else if (joy.getTiltArm() < 180 - basedegrees2) dr.tiltArm(0.5); // plz test numbers
else if (joy.getTiltArm() > 180 - basedegrees2)
dr.tiltArm(-0.5); // plz test numbers, idk if positve or negative
if (joy.getTiltJoint() == 180 - jointdegrees2) dr.tiltJoint(0);
else if (joy.getTiltJoint() < 180 - jointdegrees2) dr.tiltJoint(0.5); // plz test numbers
else if (joy.getTiltJoint() > 180 - jointdegrees2)
dr.tiltJoint(-0.5); // plz test numbers, idk if positve or negative
if (joy.getTiltArm() == 180 - basedegrees2 && joy.getTiltJoint() == 180 - jointdegrees2) {
dr.tiltArm(0);
dr.tiltJoint(0);
}
encoder.setShooterAngle(shooterangle);
if (joy.getTiltArm() == 180 - basedegrees2
&& joy.getTiltJoint() == 180 - jointdegrees2
&& encoder.getEncoderAngle() == shooterangle) {
caseSelector = 5;
}
break;
case 5:
controller.forward(speed3);
if (sensors.getGyroZAngle() > 5) {
caseSelector = 6;
}
case 6:
encoder.setShooterAngle(180 - shooterangle);
if (sensors.getGyroZAngle() < -5 && encoder.getEncoderAngle() == 180 - shooterangle) {
caseSelector = 7;
}
break;
case 7:
controller.forward(speed3);
if (sensors.getGyroZAngle() == 0) {
caseSelector = 8;
}
break;
case 8: // Stop
controller.stop();
System.out.println("Done!");
break;
}
}