mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-15 08:42:35 +00:00
Pizza bot is now fieldcentric and arm has been commented out.
This commit is contained in:
NerdyBirdy460
@@ -19,7 +19,8 @@ public class SodiPizzaMinibotObject extends Robot {
|
||||
|
||||
public HardwareMap hardwareMap;
|
||||
public DcMotor leftFront, rightFront, leftBack, rightBack;
|
||||
public Servo shoulder, gripper, launcher;
|
||||
// public Servo shoulder, gripper;
|
||||
public Servo launcher;
|
||||
public IMU imu;
|
||||
public Rev2mDistanceSensor distSensor;
|
||||
private String string;
|
||||
@@ -70,8 +71,8 @@ public class SodiPizzaMinibotObject extends Robot {
|
||||
rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
//Servo Defining
|
||||
shoulder = engine.hardwareMap.servo.get("arm");
|
||||
gripper = engine.hardwareMap.servo.get("gripper");
|
||||
// shoulder = engine.hardwareMap.servo.get("arm");
|
||||
// gripper = engine.hardwareMap.servo.get("gripper");
|
||||
launcher = engine.hardwareMap.servo.get("launcher");
|
||||
|
||||
//Distance Sensor
|
||||
|
||||
@@ -26,7 +26,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
|
||||
public final double minInput = 0.1 /* <- Minimum input from stick to send command */;
|
||||
public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative;
|
||||
public float approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3;
|
||||
private int armPos, objectPos;
|
||||
private double lfPower, rfPower, lbPower, rbPower;
|
||||
private float yTransitPercent, xTransitPercent, rotPercent, percentDenom;
|
||||
private int objectPos;
|
||||
private boolean droneLaunched;
|
||||
private char buttonPressed;
|
||||
private GamepadChecker gp1checker, gp2checker;
|
||||
@@ -48,12 +50,6 @@ public class SodiPizzaTeleOPState extends CyberarmState {
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Arm should be at Position ", armPos);
|
||||
engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition());
|
||||
|
||||
engine.telemetry.addData("Button Pressed = ", buttonPressed);
|
||||
|
||||
engine.telemetry.addLine();
|
||||
engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition());
|
||||
engine.telemetry.addData("Drone Launched?", droneLaunched);
|
||||
}
|
||||
@@ -77,15 +73,18 @@ public class SodiPizzaTeleOPState extends CyberarmState {
|
||||
gp2checker = new GamepadChecker(engine, engine.gamepad2);
|
||||
|
||||
lastMoveTime = System.currentTimeMillis();
|
||||
armPos = 0;
|
||||
lastDistRead = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
|
||||
|
||||
|
||||
if (Math.abs(engine.gamepad1.left_stick_y) < minInput &&
|
||||
Math.abs(engine.gamepad1.left_stick_x) < minInput &&
|
||||
Math.abs(engine.gamepad1.right_stick_x) < minInput) /* <- input from ONLY left stick y means to move forward or backward */{
|
||||
Math.abs(engine.gamepad1.right_stick_x) < minInput){
|
||||
|
||||
drivePower = 0;
|
||||
robot.leftFront.setPower(drivePower);
|
||||
@@ -94,69 +93,37 @@ public class SodiPizzaTeleOPState extends CyberarmState {
|
||||
robot.rightBack.setPower(drivePower);
|
||||
}
|
||||
|
||||
if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle + 0.5) {
|
||||
if (Math.abs(engine.gamepad1.right_stick_x) > minInput &&
|
||||
Math.abs(engine.gamepad1.left_stick_y) > minInput) {
|
||||
robot.rightFront.setPower(robot.leftFront.getPower() * 0.8);
|
||||
robot.rightBack.setPower(robot.leftBack.getPower() * 0.8);
|
||||
}
|
||||
|
||||
} else if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle - 0.5 &&
|
||||
Math.abs(engine.gamepad1.left_stick_y) > minInput) {
|
||||
|
||||
robot.leftFront.setPower(robot.rightFront.getPower() * 0.8);
|
||||
robot.leftBack.setPower(robot.rightBack.getPower() * 0.8);
|
||||
if (Math.abs(yTransitPercent) > 0.01) {
|
||||
|
||||
percentDenom = 100;
|
||||
} else {
|
||||
|
||||
robot.leftFront.setPower(drivePower);
|
||||
robot.rightFront.setPower(drivePower);
|
||||
robot.leftBack.setPower(drivePower);
|
||||
robot.rightBack.setPower(drivePower);
|
||||
percentDenom = 0;
|
||||
}
|
||||
|
||||
if (engine.gamepad1.start && !engine.gamepad1.a) /*<-- reset everything: encoders, imu, and armPos int*/ {
|
||||
if (Math.abs(xTransitPercent) > 0.01) {
|
||||
|
||||
robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.imu.resetYaw();
|
||||
percentDenom = percentDenom + 100;
|
||||
}
|
||||
|
||||
if (Math.abs(engine.gamepad1.left_stick_y) > minInput &&
|
||||
Math.abs(engine.gamepad1.left_stick_x) < minInput &&
|
||||
armPos == 0/* &&
|
||||
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle - 0.5 &&
|
||||
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle + 0.5*/) {
|
||||
if (Math.abs(rotPercent) > 0.01) {
|
||||
|
||||
drivePower = engine.gamepad1.left_stick_y;
|
||||
robot.leftFront.setPower(drivePower);
|
||||
robot.rightFront.setPower(drivePower);
|
||||
robot.leftBack.setPower(drivePower);
|
||||
robot.rightBack.setPower(drivePower);
|
||||
percentDenom = percentDenom + 100;
|
||||
}
|
||||
yTransitPercent = engine.gamepad1.left_stick_y * 100;
|
||||
xTransitPercent = engine.gamepad1.left_stick_x * 100;
|
||||
rotPercent = engine.gamepad1.right_stick_x * -100;
|
||||
|
||||
if (Math.abs(engine.gamepad1.left_stick_x) > minInput &&
|
||||
armPos == 0) {
|
||||
lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom);
|
||||
robot.leftFront.setPower(lfPower);
|
||||
|
||||
drivePower = engine.gamepad1.left_stick_x;
|
||||
robot.leftFront.setPower(-drivePower);
|
||||
robot.rightFront.setPower(drivePower);
|
||||
robot.leftBack.setPower(drivePower);
|
||||
robot.rightBack.setPower(-drivePower);
|
||||
}
|
||||
rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom);
|
||||
robot.rightFront.setPower(rfPower);
|
||||
|
||||
if (Math.abs(engine.gamepad1.right_stick_x) > minInput &&
|
||||
armPos == 0) {
|
||||
lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom);
|
||||
robot.leftBack.setPower(lbPower);
|
||||
|
||||
drivePower = engine.gamepad1.right_stick_x;
|
||||
robot.leftFront.setPower(-drivePower);
|
||||
robot.rightFront.setPower(drivePower);
|
||||
robot.leftBack.setPower(-drivePower);
|
||||
robot.rightBack.setPower(drivePower);
|
||||
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
|
||||
}
|
||||
rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom);
|
||||
robot.rightBack.setPower(rbPower);
|
||||
|
||||
if (engine.gamepad2.left_stick_button) {
|
||||
if (System.currentTimeMillis() - lastMoveTime >= 200) {
|
||||
@@ -182,140 +149,140 @@ public class SodiPizzaTeleOPState extends CyberarmState {
|
||||
}
|
||||
}
|
||||
|
||||
// This moves the arm to Collect position, which is at servo position 0.00.
|
||||
if (engine.gamepad2.a && !engine.gamepad2.start) {
|
||||
armPos = 1;
|
||||
}
|
||||
|
||||
if (armPos == 1) {
|
||||
|
||||
buttonPressed = 'a';
|
||||
|
||||
if (Math.abs(drivePower) > 0.25) {
|
||||
drivePower = 0.15;
|
||||
robot.leftFront.setPower(drivePower);
|
||||
robot.leftBack.setPower(drivePower);
|
||||
robot.rightFront.setPower(drivePower);
|
||||
robot.rightBack.setPower(drivePower);
|
||||
} else {
|
||||
//if servo's position is greater than Collect position with a run-to tolerance of 0.05,
|
||||
//decrement position at a rate of 0.05 per 150 milliseconds.
|
||||
if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
|
||||
lastMoveTime = System.currentTimeMillis();
|
||||
} else if (System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(ARM_COLLECT);
|
||||
armPos = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
//End of code for armPos = 1
|
||||
|
||||
// This moves the arm to Precollect position, which is at servo position 0.05.
|
||||
if (engine.gamepad2.x) {
|
||||
armPos = 2;
|
||||
}
|
||||
|
||||
if (armPos == 2) {
|
||||
|
||||
buttonPressed = 'x';
|
||||
|
||||
if (Math.abs(drivePower) > 0.25) {
|
||||
drivePower = 0.15;
|
||||
robot.leftFront.setPower(drivePower);
|
||||
robot.leftBack.setPower(drivePower);
|
||||
robot.rightFront.setPower(drivePower);
|
||||
robot.rightBack.setPower(drivePower);
|
||||
} else {
|
||||
//if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
|
||||
//decrement position at a rate of 0.05 per 150 milliseconds.
|
||||
if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
|
||||
lastMoveTime = System.currentTimeMillis();
|
||||
}//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
|
||||
else if (System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(ARM_PRECOLLECT);
|
||||
armPos = 0;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
//End of code for armPos = 2
|
||||
|
||||
// This moves the arm to Deliver position, which is at servo position 0.28.
|
||||
if (engine.gamepad2.b && !engine.gamepad2.start) {
|
||||
armPos = 3;
|
||||
}
|
||||
|
||||
if (armPos == 3) {
|
||||
|
||||
buttonPressed = 'b';
|
||||
|
||||
if (Math.abs(drivePower) > 0.25) {
|
||||
drivePower = 0.15;
|
||||
robot.leftFront.setPower(drivePower);
|
||||
robot.leftBack.setPower(drivePower);
|
||||
robot.rightFront.setPower(drivePower);
|
||||
robot.rightBack.setPower(drivePower);
|
||||
} else {
|
||||
//if servo's position is less than Deliver position with a run-to tolerance of 0.05,
|
||||
//increment position at a rate of 0.05 per 150 milliseconds.
|
||||
if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
|
||||
lastMoveTime = System.currentTimeMillis();
|
||||
}//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
|
||||
//decrement position at a rate of 0.05 per 150 milliseconds.
|
||||
else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
|
||||
lastMoveTime = System.currentTimeMillis();
|
||||
} else if (System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(ARM_DELIVER);
|
||||
armPos = 0;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
//End of code for armPos = 3
|
||||
|
||||
// This moves the arm to Stow position, which is at servo position 0.72.
|
||||
if (engine.gamepad2.y) {
|
||||
armPos = 4;
|
||||
}
|
||||
|
||||
if (armPos == 4) {
|
||||
|
||||
buttonPressed = 'y';
|
||||
|
||||
if (Math.abs(drivePower) > 0.25) {
|
||||
drivePower = 0.15;
|
||||
robot.leftFront.setPower(drivePower);
|
||||
robot.leftBack.setPower(drivePower);
|
||||
robot.rightFront.setPower(drivePower);
|
||||
robot.rightBack.setPower(drivePower);
|
||||
} else {
|
||||
//if servo's position is less than Collect position with a run-to tolerance of 0.05,
|
||||
//increment position at a rate of 0.05 per 150 milliseconds.
|
||||
if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
|
||||
lastMoveTime = System.currentTimeMillis();
|
||||
}
|
||||
//Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
|
||||
else if (System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
robot.shoulder.setPosition(ARM_STOW);
|
||||
armPos = 0;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
// // This moves the arm to Collect position, which is at servo position 0.00.
|
||||
// if (engine.gamepad2.a && !engine.gamepad2.start) {
|
||||
// armPos = 1;
|
||||
// }
|
||||
//
|
||||
// if (armPos == 1) {
|
||||
//
|
||||
// buttonPressed = 'a';
|
||||
//
|
||||
// if (Math.abs(drivePower) > 0.25) {
|
||||
// drivePower = 0.15;
|
||||
// robot.leftFront.setPower(drivePower);
|
||||
// robot.leftBack.setPower(drivePower);
|
||||
// robot.rightFront.setPower(drivePower);
|
||||
// robot.rightBack.setPower(drivePower);
|
||||
// } else {
|
||||
// //if servo's position is greater than Collect position with a run-to tolerance of 0.05,
|
||||
// //decrement position at a rate of 0.05 per 150 milliseconds.
|
||||
// if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
|
||||
// lastMoveTime = System.currentTimeMillis();
|
||||
// } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(ARM_COLLECT);
|
||||
// armPos = 0;
|
||||
// }
|
||||
//
|
||||
// }
|
||||
//
|
||||
// }
|
||||
// //End of code for armPos = 1
|
||||
//
|
||||
// // This moves the arm to Precollect position, which is at servo position 0.05.
|
||||
// if (engine.gamepad2.x) {
|
||||
// armPos = 2;
|
||||
// }
|
||||
//
|
||||
// if (armPos == 2) {
|
||||
//
|
||||
// buttonPressed = 'x';
|
||||
//
|
||||
// if (Math.abs(drivePower) > 0.25) {
|
||||
// drivePower = 0.15;
|
||||
// robot.leftFront.setPower(drivePower);
|
||||
// robot.leftBack.setPower(drivePower);
|
||||
// robot.rightFront.setPower(drivePower);
|
||||
// robot.rightBack.setPower(drivePower);
|
||||
// } else {
|
||||
// //if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
|
||||
// //decrement position at a rate of 0.05 per 150 milliseconds.
|
||||
// if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
|
||||
// lastMoveTime = System.currentTimeMillis();
|
||||
// }//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
|
||||
// else if (System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(ARM_PRECOLLECT);
|
||||
// armPos = 0;
|
||||
// }
|
||||
//
|
||||
// }
|
||||
// }
|
||||
// //End of code for armPos = 2
|
||||
//
|
||||
// // This moves the arm to Deliver position, which is at servo position 0.28.
|
||||
// if (engine.gamepad2.b && !engine.gamepad2.start) {
|
||||
// armPos = 3;
|
||||
// }
|
||||
//
|
||||
// if (armPos == 3) {
|
||||
//
|
||||
// buttonPressed = 'b';
|
||||
//
|
||||
// if (Math.abs(drivePower) > 0.25) {
|
||||
// drivePower = 0.15;
|
||||
// robot.leftFront.setPower(drivePower);
|
||||
// robot.leftBack.setPower(drivePower);
|
||||
// robot.rightFront.setPower(drivePower);
|
||||
// robot.rightBack.setPower(drivePower);
|
||||
// } else {
|
||||
// //if servo's position is less than Deliver position with a run-to tolerance of 0.05,
|
||||
// //increment position at a rate of 0.05 per 150 milliseconds.
|
||||
// if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
|
||||
// lastMoveTime = System.currentTimeMillis();
|
||||
// }//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
|
||||
// //decrement position at a rate of 0.05 per 150 milliseconds.
|
||||
// else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
|
||||
// lastMoveTime = System.currentTimeMillis();
|
||||
// } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(ARM_DELIVER);
|
||||
// armPos = 0;
|
||||
// }
|
||||
//
|
||||
// }
|
||||
// }
|
||||
// //End of code for armPos = 3
|
||||
//
|
||||
// // This moves the arm to Stow position, which is at servo position 0.72.
|
||||
// if (engine.gamepad2.y) {
|
||||
// armPos = 4;
|
||||
// }
|
||||
//
|
||||
// if (armPos == 4) {
|
||||
//
|
||||
// buttonPressed = 'y';
|
||||
//
|
||||
// if (Math.abs(drivePower) > 0.25) {
|
||||
// drivePower = 0.15;
|
||||
// robot.leftFront.setPower(drivePower);
|
||||
// robot.leftBack.setPower(drivePower);
|
||||
// robot.rightFront.setPower(drivePower);
|
||||
// robot.rightBack.setPower(drivePower);
|
||||
// } else {
|
||||
// //if servo's position is less than Collect position with a run-to tolerance of 0.05,
|
||||
// //increment position at a rate of 0.05 per 150 milliseconds.
|
||||
// if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
|
||||
// lastMoveTime = System.currentTimeMillis();
|
||||
// }
|
||||
// //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
|
||||
// else if (System.currentTimeMillis() - lastMoveTime >= 150) {
|
||||
// robot.shoulder.setPosition(ARM_STOW);
|
||||
// armPos = 0;
|
||||
// }
|
||||
//
|
||||
// }
|
||||
// }
|
||||
//End of code for armPos = 4
|
||||
|
||||
if (engine.gamepad2.dpad_left) {
|
||||
robot.gripper.setPosition(GRIPPER_OPEN);
|
||||
} else if (engine.gamepad2.dpad_right) {
|
||||
robot.gripper.setPosition(GRIPPER_CLOSED);
|
||||
}
|
||||
// if (engine.gamepad2.dpad_left) {
|
||||
// robot.gripper.setPosition(GRIPPER_OPEN);
|
||||
// } else if (engine.gamepad2.dpad_right) {
|
||||
// robot.gripper.setPosition(GRIPPER_CLOSED);
|
||||
// }
|
||||
|
||||
gp1checker.update();
|
||||
gp2checker.update();
|
||||
|
||||
Reference in New Issue
Block a user