package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.DcMotor;
@Autonomous(name = "Sensor : Touch Sensor", group = "Sensor")
//@Disabled
public class TouchSensor extends LinearOpMode {
DcMotor leftmotor1;
DcMotor leftmotor2;
DcMotor rightmotor1;
DcMotor rightmotor2;
DigitalChannel digitalTouch1;
DigitalChannel digitalTouch2;
@Override
public void runOpMode() {
digitalTouch1 = hardwareMap.get(DigitalChannel.class, "sensor_touch1");
digitalTouch2 = hardwareMap.get(DigitalChannel.class,"sensor_touch2");
leftmotor1 = hardwareMap.dcMotor.get("stanga fata");
leftmotor2 = hardwareMap.dcMotor.get("stanga spate");
rightmotor1 = hardwareMap.dcMotor.get("dreapta fata");
rightmotor2 = hardwareMap.dcMotor.get("dreapta spate");
rightmotor1.setDirection(DcMotor.Direction.REVERSE);
rightmotor2.setDirection(DcMotor.Direction.REVERSE);
digitalTouch1.setMode(DigitalChannel.Mode.INPUT);
digitalTouch2.setMode(DigitalChannel.Mode.INPUT);
waitForStart();
leftmotor1.setPower(-0.3);
leftmotor2.setPower(-0.3);
rightmotor1.setPower(-0.3);
rightmotor2.setPower(-0.3);
while (opModeIsActive()) {
if (digitalTouch2.getState() == true) {
telemetry.addData("Touch Sensor", " Nu este apăsat");
} else if (digitalTouch2.getState() == false) {
leftmotor1.setPower(0.3);
leftmotor2.setPower(0.3);
rightmotor1.setPower(0.3);
rightmotor2.setPower(0.3);
telemetry.addData("Touch Sensor", " Este apăsat");
}
telemetry.update();
if (digitalTouch1.getState() == true) {
telemetry.addData("Touch Sensor", " Nu este apăsat");
} else if (digitalTouch1.getState() == false) {
leftmotor1.setPower(-0.3);
leftmotor2.setPower(-0.3);
rightmotor1.setPower(-0.3);
rightmotor2.setPower(-0.3);
telemetry.addData("Touch Sensor", " Este apăsat");
}
telemetry.update();
}
}
}