@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()));
}
public void moveForwardToWall(double pow, int timeout) throws InterruptedException {
double angle = Math.abs(sensor.getGyroYaw());
double startAngle = angle;
double power = pow;
ElapsedTime time = new ElapsedTime();
time.startTime();
while (Math.abs(angle - startAngle) < 10 && time.milliseconds() < timeout) {
angle = Math.abs(sensor.getGyroYaw());
opMode.telemetry.addData("LeftPower", motorBL.getPower());
opMode.telemetry.addData("RightPower", motorBR.getPower());
opMode.telemetry.update();
if (angle < startAngle - 1) {
startMotors((power * .75), power);
} else if (angle > startAngle + 1) {
startMotors(power, (power * .75));
} else {
startMotors(power, power);
}
opMode.idle();
}
stopMotors();
angleError = sensor.getGyroYaw();
opMode.telemetry.update();
}
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");
}
public void moveFowardToLine(double ri, double le, int timeout) throws InterruptedException {
double angle;
double startAngle = Math.abs(sensor.getGyroYaw());
opMode.telemetry.update();
setNullValue();
ElapsedTime time = new ElapsedTime();
time.startTime();
while (!sensor.isLeftLine() && time.milliseconds() < timeout) {
angle = Math.abs(sensor.getGyroYaw());
opMode.telemetry.addData("LeftPower", motorBL.getPower());
opMode.telemetry.addData("RightPower", motorBR.getPower());
opMode.telemetry.update();
// if(angle < startAngle - 2) {
// startMotors((power * .75), power);
// } else if(angle > startAngle + 2) {
// startMotors(power, (power * .75));
// } else {
// startMotors(power, power);
// }
startMotors(ri, le);
opMode.idle();
}
stopMotors();
opMode.telemetry.update();
angleError = sensor.getGyroYaw();
}
/*
* Code to run when the op mode is first enabled goes here
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
*/
@Override
public void init_loop() {
startDate = new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(new Date());
runtime.reset();
telemetry.addData("Null Op Init Loop", runtime.toString());
range = hardwareMap.analogInput.get("range");
}
/*
* 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));
}
/**
* *
*
* <p>waitForTick implements a periodic delay. However, this acts like a metronome with a regular
* periodic tick. This is used to compensate for varying processing times for each cycle. The
* function looks at the elapsed cycle time, and sleeps for the remaining time interval.
*
* @param periodMs Length of wait cycle in mSec.
* @throws InterruptedException
*/
public void waitForTick(long periodMs) throws InterruptedException {
long remaining = periodMs - (long) period.milliseconds();
// sleep for the remaining portion of the regular cycle period.
if (remaining > 0) Thread.sleep(remaining);
// Reset the cycle clock for the next pass.
period.reset();
}
/*
* 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;
}
}
/*
* Code to run ONCE when the driver hits PLAY
*/
@Override
public void start() {
runtime.reset();
// @todo add all one time 'get ready to run' here
}
@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.
}
public void moveForwardUntilZero(double pow, double timeout) throws InterruptedException {
double angle = sensor.getGyroYaw();
ElapsedTime time = new ElapsedTime();
time.startTime();
while (Math.abs(angle) > 2 && time.milliseconds() < timeout) {
startMotors(-.3, -.5);
opMode.telemetry.addData("current", "zeroout");
opMode.telemetry.update();
opMode.idle();
}
stopMotors();
}
public void moveForwardToWallEnc(double pow, int timeout) throws InterruptedException {
double angle = Math.abs(sensor.getGyroYaw());
double startAngle = angle;
double power = pow;
int tick = 1;
resetEncoders();
int startEncoder = Math.abs(motorFL.getCurrentPosition());
int endEncoder;
ElapsedTime time = new ElapsedTime();
time.startTime();
while (Math.abs(angle - startAngle) < 10 && time.milliseconds() < timeout) {
angle = Math.abs(sensor.getGyroYaw());
opMode.telemetry.addData("LeftPower", motorBL.getPower());
opMode.telemetry.addData("RightPower", motorBR.getPower());
opMode.telemetry.update();
if (time.milliseconds() > 250 * tick) {
endEncoder = Math.abs(motorFL.getCurrentPosition());
if (startEncoder + 250 < endEncoder) {
break;
}
}
if (angle < startAngle - 1) {
startMotors((power * .75), power);
} else if (angle > startAngle + 1) {
startMotors(power, (power * .75));
} else {
startMotors(power, power);
}
opMode.idle();
}
stopMotors();
angleError = sensor.getGyroYaw();
opMode.telemetry.update();
}
/*
* 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;
}
}
/*
* Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
*/
@Override
public void loop() {
if (debugmode) telemetry.addData("Status", "Running: " + runtime.toString());
handleControls(); // function to read all input controls and set globals here
handleDrivetrain(); // function to handle drivetrain changes here
handleFeatures(); // function to handle auxillary hardware features here
telemetry.update(); // needed to ensure that driver station is updated, but udpate
// liomited by SDK to only four times per second
}
/**
* 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;
}
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");
}
/*
* Code to run when the op mode is first enabled goes here
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
*/
@Override
public void init_loop() {
telemetry.addData("navX Op Init Loop", runtime.toString());
}
@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 moveBackward(double pow, int encoderVal, int timeout) throws InterruptedException {
// sensor.resetGyro();
double angle;
double startAngle = Math.abs(sensor.getGyroYaw());
opMode.telemetry.update();
double error;
double power;
motorBL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
opMode.idle();
motorBR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
opMode.idle();
motorFR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
opMode.idle();
motorFL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
opMode.idle();
motorBL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
opMode.idle();
motorBR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
opMode.idle();
motorFR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
opMode.idle();
motorFL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
opMode.idle();
setNullValue();
nullValue = 0;
ElapsedTime time = new ElapsedTime();
time.startTime();
int currentEncoder = getEncoderAvg() - nullValue;
while (encoderVal > currentEncoder && time.milliseconds() < timeout) {
opMode.telemetry.update();
angle = Math.abs(sensor.getGyroYaw());
currentEncoder = getEncoderAvg() - nullValue;
error = (double) (encoderVal - currentEncoder) / encoderVal;
power = (pow * error) - .25;
Range.clip(power, -1, 1);
opMode.telemetry.addData("Power", power);
opMode.telemetry.addData("LeftPower", motorBL.getPower());
opMode.telemetry.addData("RightPower", motorBR.getPower());
opMode.telemetry.update();
if (angle < startAngle - 2) {
startMotors((power), (power * .75));
} else if (angle > startAngle + 2) {
startMotors((power * .75), (power));
} else {
startMotors(power, power);
}
startMotors(power, power);
opMode.idle();
}
stopMotors();
opMode.telemetry.update();
angleError = sensor.getGyroYaw();
}
@Override
public void runOpMode() throws InterruptedException {
climber = hardwareMap.servo.get(Keys.climber);
swivel = hardwareMap.servo.get(Keys.swivel);
hang = hardwareMap.servo.get(Keys.hang);
clampLeft = hardwareMap.servo.get(Keys.clampLeft);
clampRight = hardwareMap.servo.get(Keys.clampRight);
dump = hardwareMap.servo.get(Keys.dump);
fr = hardwareMap.dcMotor.get(Keys.frontRight);
fl = hardwareMap.dcMotor.get(Keys.frontLeft);
bl = hardwareMap.dcMotor.get(Keys.backLeft);
br = hardwareMap.dcMotor.get(Keys.backRight);
collector = hardwareMap.dcMotor.get(Keys.collector);
fl.setDirection(DcMotor.Direction.REVERSE);
bl.setDirection(DcMotor.Direction.REVERSE);
dump.setPosition(Keys.DUMP_INIT);
swivel.setPosition(Keys.SWIVEL_CENTER);
hang.setPosition(Keys.HANG_INIT);
clampLeft.setPosition(Keys.CLAMP_LEFT_INIT);
clampRight.setPosition(Keys.CLAMP_RIGHT_INIT);
climber.setPosition(Keys.CLIMBER_INITIAL_STATE);
collector.setDirection(DcMotor.Direction.REVERSE);
sonarAbovePhone = hardwareMap.analogInput.get(Keys.SONAR_ABOVE_PHONE);
sonarFoot = hardwareMap.analogInput.get(Keys.SONAR_FOOT);
navx_device =
AHRS.getInstance(
hardwareMap.deviceInterfaceModule.get(Keys.advancedSensorModule),
Keys.NAVX_DIM_I2C_PORT,
AHRS.DeviceDataType.kProcessedData,
Keys.NAVX_DEVICE_UPDATE_RATE_HZ);
while (!calibration_complete) {
calibration_complete = !navx_device.isCalibrating();
if (!calibration_complete) {
telemetry.addData("Calibration Complete?", "No");
}
}
telemetry.addData("Calibration Complete?", "Yes");
// telemetry.addData("Start Autonomous?", "Yes");
waitForStart();
smoothMoveVol2(48, false);
telemetry.addData("starting", "smoothDump starting");
timer = new ElapsedTime();
timer.reset();
smoothDump(timer);
mCamera = ((FtcRobotControllerActivity) hardwareMap.appContext).mCamera;
// i need to init the camera and also get the instance of the camera //on pic take
// protocol
telemetry.addData("camera", "initingcameraPreview");
((FtcRobotControllerActivity) hardwareMap.appContext).initCameraPreview(mCamera, this);
// wait, because I have handler wait three seconds b4 it'll take a picture, in initCamera
sleep(Vision.RETRIEVE_FILE_TIME);
// now we are going to retreive the image and convert it to bitmap
SharedPreferences prefs =
hardwareMap
.appContext
.getApplicationContext()
.getSharedPreferences("com.quan.companion", Context.MODE_PRIVATE);
String path = prefs.getString(Keys.pictureImagePathSharedPrefsKeys, "No path found");
Log.e("path", path);
telemetry.addData("image", path);
// debug stuff - telemetry.addData("camera", "path: " + path);
File imgFile = new File(path);
image = BitmapFactory.decodeFile(imgFile.getAbsolutePath());
Log.e("image", image.toString());
// cool now u have the image file u just took the picture of
VisionProcess mVP = new VisionProcess(image);
Log.e("starting output", "start");
telemetry.addData("starting output", "doing smart computer stuff now");
Beacon beacon = mVP.output(hardwareMap.appContext);
Log.e("beacon", beacon.toString());
telemetry.addData("beacon", beacon);
}
@Override
public void start() {
motorOscTimer.reset();
servoOscTimer.reset();
spamPrevention.reset();
}
@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());
}
/*
* This method will be called repeatedly in a loop
* @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
*/
@Override
public void loop() {
telemetry.addData("1 Start", "NullOp started at " + startDate);
telemetry.addData("2 Status", "running for " + runtime.toString());
}