public void smoothDump(ElapsedTime timer) {
moveStraight(8.5, false, .3);
double pos = Keys.CLIMBER_INITIAL_STATE;
// .85 to .31 so you want to decrement
timer.reset();
telemetry.addData("place", "before while");
while (pos > Keys.CLIMBER_DUMP) {
timer.reset();
while (timer.time() * 1000 < 200) {
// do nothing
try {
sleep(2);
} catch (InterruptedException e) {
e.printStackTrace();
}
telemetry.addData("waiting", timer.time() * 1000);
}
pos -= .05;
climber.setPosition(pos);
/*telemetry.addData("place","during while");
telemetry.addData("timer",timer.time());
telemetry.addData("timer math",((int)(timer.time()*1000)));
telemetry.addData("timer math2",((int)(timer.time()*1000))%200);
telemetry.addData("climber",climber.getPosition());
telemetry.addData("constant","INIT"+Keys.CLIMBER_INITIAL_STATE+" DUMP"+Keys.CLIMBER_DUMP);*/
}
// once it is here, it finished dumping.
// retract - the sudden should be ok cuz hopefully by that time it will have already dumped
climber.setPosition(Keys.CLIMBER_INITIAL_STATE);
moveStraight(8.5, true, .3);
telemetry.addData("place", "after while");
}
@Override
public void loop() {
ElapsedTime timer = new ElapsedTime();
double timeLimit = 3;
/* if (gamepad1.dpad_up) {
robot.barGrabberServo.goHome();
}
if (gamepad1.dpad_down) {
robot.barGrabberServo.goGrabBar();
}
if (gamepad1.dpad_up) {
// go at servo direct
}
if (gamepad1.dpad_down) {
// go at servo direct
}*/
for (int i = 0; i < 11; i++) {
if (timer.time() > timeLimit) {
timer.reset();
robot.leftZipLineServo.setPosition(i / 10);
}
}
telemetry.addData(
"servo", "positiom " + String.format("%.2f", robot.leftZipLineServo.getPosition()));
}
/*
* handleBattery
*
* The Matrix controller has a separate battery whose voltage can be read.
*/
protected void handleBattery() {
if ((firstBattery) || (spamPrevention.time() > SPAM_PREVENTION_FREQ)) {
battery = mc.getBattery();
spamPrevention.reset();
firstBattery = false;
}
telemetry.addData("Battery: ", ((float) battery / 1000));
}
@Override
public boolean timeslice() {
if (timer.time() >= timeout) {
new SingleShotTimerEvent(this, EventKind.EXPIRED).postEvent();
return true; // Task is done, will be deleted.
} else return false; // Task needs to keep running.
}
/*
* handleServos
*
* Oscillate the servos.
*/
protected void handleServos() {
if ((firstServos) || (servoOscTimer.time() > SERVO_OSC_FREQ)) {
if (servoPosition == 0.0) {
servoPosition = 1.0;
} else {
servoPosition = 0.0;
}
servo1.setPosition(servoPosition);
servo2.setPosition(servoPosition);
servo3.setPosition(servoPosition);
servo4.setPosition(servoPosition);
servoOscTimer.reset();
firstServos = false;
}
}
/*
* handleMotors
*
* Oscillate the motors.
*/
protected void handleMotors() {
if ((firstMotors) || (motorOscTimer.time() > MOTOR_OSC_FREQ)) {
motorPower = -motorPower;
/*
* The MatrixDcMotorController's setMotorPower() method may take
* a collection of motors. If this is chosen, then the controller will
* set a pending bit. The pending bit tells the controller to
* defer turning on, or changing the current set point, for a motor
* until the pending bit is cleared.
*
* When the pending bit is cleared all motor power values are applied
* simultaneously. setMotorPower() handles the pending bit for you.
*/
mc.setMotorPower(motorSet, motorPower);
motorOscTimer.reset();
firstMotors = false;
}
}
/**
* Update the angles using the raw readings.
*
* @param rawX raw rotation along x axis
* @param rawY raw rotation along y axis
* @param rawZ raw rotation along z axis
*/
public void update(int rawX, int rawY, int rawZ) {
if (stopWatch == null) {
// first time? initialize the stop watch
stopWatch = new ElapsedTime();
} else {
// integrate values
double elapsed = stopWatch.time();
pitch += ((rawX + prevX) / 2.0) * elapsed;
roll += ((rawY + prevY) / 2.0) * elapsed;
heading += ((rawZ + prevZ) / 2.0) * elapsed;
stopWatch.reset();
}
// remember these readings
prevX = rawX;
prevY = rawY;
prevZ = rawZ;
}
@Override
public void runOpMode() {
elapsedTime = new ElapsedTime();
dim = hardwareMap.deviceInterfaceModule.get("dim");
dim.setDigitalChannelMode(GYROSCOPE_SPOT, DigitalChannelController.Mode.OUTPUT);
gyro = hardwareMap.gyroSensor.get("gyro");
dim.setDigitalChannelState(GYROSCOPE_SPOT, true);
motorBL = hardwareMap.dcMotor.get("motorBL");
motorBR = hardwareMap.dcMotor.get("motorBR");
motorFL = hardwareMap.dcMotor.get("motorFL");
elapsedTime.startTime();
motorFR = hardwareMap.dcMotor.get("motorFR");
// color = hardwareMap.colorSensor.get("color");
int distance1 = 5550;
int distance3 = 9700;
double currentAngle = 0.0;
while (motorBL.getCurrentPosition() < distance1) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
}
while (currentAngle < 90.0) {
startMotors(-1, -1, -1, -1);
getEncoderValues();
currentAngle = gyro.getRotation();
}
while (motorBL.getCurrentPosition() < distance3) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
}
getTime();
elapsedTime.reset();
double currentTime = 0.0;
while (currentTime < 5.0) {
getEncoderValues();
getTime();
currentTime = elapsedTime.time();
}
while (motorBL.getCurrentPosition() > 9000) { // 9034
startMotors(1, -1, -1, 1);
getEncoderValues();
}
while (currentAngle > 45.0) {
startMotors(-1, -1, -1, -1);
getEncoderValues();
currentAngle = gyro.getRotation();
}
elapsedTime.reset();
currentTime = 0.0;
while (currentTime < 5.0) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
getTime();
currentTime = elapsedTime.time();
}
stopMotors();
motorBL.close();
motorFL.close();
motorBR.close();
motorFR.close();
}
public void getTime() {
telemetry.addData("time", elapsedTime.time());
}
@Override
public void runOpMode() throws InterruptedException {
lwa = hardwareMap.dcMotor.get("leftwheelA");
lwb = hardwareMap.dcMotor.get("leftwheelB");
rwa = hardwareMap.dcMotor.get("rightwheelA");
rwb = hardwareMap.dcMotor.get("rightwheelB");
rwa.setDirection(DcMotor.Direction.REVERSE);
rwb.setDirection(DcMotor.Direction.REVERSE);
liftL = hardwareMap.dcMotor.get("liftL");
liftR = hardwareMap.dcMotor.get("liftR");
// liftR.setDirection(DcMotor.Direction.REVERSE);
// liftL.setDirection(DcMotor.Direction.REVERSE);
collector = hardwareMap.dcMotor.get("collector");
// rightCR = hardwareMap.servo.get("rightCR");
// leftCR = hardwareMap.servo.get("leftCR");
swivel = hardwareMap.servo.get("swivel");
dump = hardwareMap.servo.get("dump");
trigL = hardwareMap.servo.get("trigL");
trigR = hardwareMap.servo.get("trigR");
dds = hardwareMap.servo.get("dds");
holdL = hardwareMap.servo.get("holdL");
holdR = hardwareMap.servo.get("holdR");
holdC = hardwareMap.servo.get("holdC");
// leftPivot = hardwareMap.servo.get("leftPivot");
// rightPivot = hardwareMap.servo.get("rightPivot");
lineSensor = hardwareMap.opticalDistanceSensor.get("dist1");
touch = hardwareMap.touchSensor.get("touch");
Gyro = hardwareMap.gyroSensor.get("Gyro");
// leftPivot.setPosition(1);
// rightPivot.setPosition(0);
dump.setPosition(0);
trigL.setPosition(0.8);
trigR.setPosition(0.05);
// leftCR.setPosition(0.5);
// rightCR.setPosition(0.5);
dds.setPosition(1);
holdL.setPosition(0.75);
holdR.setPosition(0.05);
holdC.setPosition(1);
double lineSensorValue;
double heading = 0;
// double integratedHeading = Gyro.getIntegratedZValue();
double headingError;
double targetHeading;
double drivesteering;
double driveGain = 0.005;
double leftPower;
double rightPower;
double midPower = 0;
double minPower = 0.2;
Gyro.calibrate();
// wait for the start button to be pressed
waitForStart();
timer = new ElapsedTime();
collector.setPower(0);
while (rwa.getCurrentPosition() > -7700 && timer.time() < 15) {
rwa.setPower(-0.75);
rwb.setPower(-0.75);
lwa.setPower(-0.75);
lwb.setPower(-0.75);
}
rwa.setPower(0);
rwb.setPower(0);
lwa.setPower(0);
lwb.setPower(0);
sleep(100);
heading = Gyro.getHeading();
telemetry.addData("heading", heading);
heading = 359;
sleep(100);
while (heading > 225) {
heading = Gyro.getHeading();
// integratedHeading = Gyro.getIntegratedZValue();
telemetry.addData("heading", heading);
// telemetry.addData("Integrated", integratedHeading);
targetHeading = 225;
headingError = targetHeading - heading;
drivesteering = driveGain * headingError;
if (drivesteering > 1) {
drivesteering = 1;
telemetry.addData("Caught illegal value", "reset drivesteering to 1");
}
leftPower = midPower + drivesteering;
rightPower = midPower + drivesteering;
if (leftPower > minPower) {
leftPower = minPower;
}
if (rightPower < minPower) {
rightPower = minPower;
}
lwa.setPower(leftPower);
lwb.setPower(leftPower);
rwa.setPower(rightPower);
rwb.setPower(rightPower);
}
while (heading < 225) {
heading = Gyro.getHeading();
// integratedHeading = Gyro.getIntegratedZValue();
telemetry.addData("heading", heading);
// telemetry.addData("Integrated", integratedHeading);
targetHeading = 225;
headingError = targetHeading - heading;
drivesteering = driveGain * headingError;
if (drivesteering > 1) {
drivesteering = 1;
telemetry.addData("Caught illegal value", "reset drivesteering to 1");
}
rightPower = midPower - drivesteering;
leftPower = midPower + drivesteering;
if (rightPower > minPower) {
rightPower = minPower;
}
if (leftPower < minPower) {
leftPower = minPower;
}
lwa.setPower(leftPower);
lwb.setPower(leftPower);
rwa.setPower(rightPower);
rwb.setPower(rightPower);
}
lwa.setPower(1);
lwb.setPower(1);
rwa.setPower(1);
rwb.setPower(1);
sleep(400);
lwa.setPower(0);
lwb.setPower(0);
rwa.setPower(0);
rwb.setPower(0);
telemetry.addData("Event", "Done");
sleep(100);
while (!touch.isPressed() && timer.time() < 20) {
lineSensorValue = lineSensor.getLightDetectedRaw();
if (lineSensorValue < 10) {
lwa.setPower(0.5);
lwb.setPower(0.5);
rwa.setPower(0.0);
rwb.setPower(0.0);
} else {
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.5);
rwb.setPower(0.5);
}
}
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.0);
rwb.setPower(0.0);
sleep(100);
collector.setPower(0.0);
sleep(100);
if (timer.time() < 20) {
lwa.setPower(-0.35);
lwb.setPower(-0.35);
rwa.setPower(-0.35);
rwb.setPower(-0.35);
sleep(225);
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.0);
rwb.setPower(0.0);
sleep(100);
dds.setPosition(0);
sleep(900);
}
lwa.setPower(-1);
lwb.setPower(-1);
rwa.setPower(-1);
rwb.setPower(-1);
sleep(500);
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.0);
rwb.setPower(0.0);
while (heading > 130) {
heading = Gyro.getHeading();
telemetry.addData("heading", heading);
rwa.setPower(0.5);
rwb.setPower(0.5);
lwa.setPower(-0.5);
lwb.setPower(-0.5);
}
lwa.setPower(-0.5);
lwb.setPower(-0.5);
rwa.setPower(-0.5);
rwb.setPower(-0.5);
sleep(1000);
dds.setPosition(1);
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.0);
rwb.setPower(0.0);
sleep(500);
}
public void run() {
RobotLog.v("EventLoopRunnable has started");
try {
ElapsedTime elapsedEventLoop = new ElapsedTime();
double sMinLoopInterval = 0.001D;
long msLoopIntervalStep = 5L;
while (!Thread.interrupted()) {
while (elapsedEventLoop.time() < sMinLoopInterval) {
Thread.sleep(msLoopIntervalStep);
}
elapsedEventLoop.reset();
if (RobotLog.hasGlobalErrorMsg()) {
EventLoopManager.this.buildAndSendTelemetry(
"SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
}
if (EventLoopManager.this.elapsedSinceHeartbeatReceived.startTime() == 0.0D) {
Thread.sleep(500L);
} else if (EventLoopManager.this.elapsedSinceHeartbeatReceived.time() > 2.0D) {
EventLoopManager.this.handleDroppedConnection();
EventLoopManager.this.currentPeerAddressAndPort = null;
EventLoopManager.this.elapsedSinceHeartbeatReceived = new ElapsedTime(0L);
}
Iterator syncdDeviceIterator = EventLoopManager.this.syncdDevices.iterator();
SyncdDevice syncdDevice;
while (syncdDeviceIterator.hasNext()) {
syncdDevice = (SyncdDevice) syncdDeviceIterator.next();
syncdDevice.blockUntilReady();
}
boolean unblockOnException = false;
try {
unblockOnException = true;
EventLoopManager.this.eventLoop.loop();
unblockOnException = false;
} catch (Exception e) {
RobotLog.e("Event loop threw an exception");
RobotLog.logStacktrace(e);
String exceptionMessage =
e.getClass().getSimpleName()
+ (e.getMessage() != null ? " - " + e.getMessage() : "");
RobotLog.setGlobalErrorMsg(
"User code threw an uncaught exception: " + exceptionMessage);
EventLoopManager.this.buildAndSendTelemetry(
"SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
throw new RobotCoreException("EventLoop Exception in loop()");
} finally {
if (unblockOnException) {
Iterator var9 = EventLoopManager.this.syncdDevices.iterator();
while (var9.hasNext()) {
SyncdDevice var10 = (SyncdDevice) var9.next();
var10.startBlockingWork();
}
}
}
syncdDeviceIterator = EventLoopManager.this.syncdDevices.iterator();
while (syncdDeviceIterator.hasNext()) {
syncdDevice = (SyncdDevice) syncdDeviceIterator.next();
syncdDevice.startBlockingWork();
}
}
} catch (InterruptedException var20) {
RobotLog.v("EventLoopRunnable interrupted");
EventLoopManager.this.reportRobotStatus(RobotState.STOPPED);
} catch (RobotCoreException var21) {
RobotLog.v("RobotCoreException in EventLoopManager: " + var21.getMessage());
EventLoopManager.this.reportRobotStatus(RobotState.EMERGENCY_STOP);
EventLoopManager.this.buildAndSendTelemetry(
"SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
}
try {
EventLoopManager.this.eventLoop.teardown();
} catch (Exception var17) {
RobotLog.w("Caught exception during looper teardown: " + var17.toString());
RobotLog.logStacktrace(var17);
if (RobotLog.hasGlobalErrorMsg()) {
EventLoopManager.this.buildAndSendTelemetry(
"SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
}
}
RobotLog.v("EventLoopRunnable has exited");
}