Announcement

Collapse
No announcement yet.

New question about encoders

Collapse
X
 
  • Filter
  • Time
  • Show
Clear All
new posts

  • New question about encoders

    After reading and rereading my code is realized the I had RUN_USING_ENCODERS in the init(). I also had RUN_TO_POSITION in the encoderDrive method. Will having two different methods in the same class cause any issues?

    My code is shown below.

    //POSITION BLUE 1 - SHOT TWO PARTICLES IN CENTER VORTEX - SET BLUE BEACON #1 - PARK ON RAMP
    package org.firstinspires.ftc.teamcode;

    import com.qualcomm.hardware.modernrobotics.ModernRobotic sI2cRangeSensor;
    import com.qualcomm.robotcore.eventloop.opmode.Autonomous ;
    import com.qualcomm.robotcore.eventloop.opmode.LinearOpMo de;
    import com.qualcomm.robotcore.hardware.CRServo;
    import com.qualcomm.robotcore.hardware.ColorSensor;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.OpticalDistanceSen sor;
    import com.qualcomm.robotcore.hardware.Servo;
    import com.qualcomm.robotcore.util.ElapsedTime;

    import org.firstinspires.ftc.robotcore.external.navigatio n.DistanceUnit;


    @Autonomous(name = "AutoBlue_1", group = "Blue")

    public class AutoBlue_1 extends LinearOpMode {

    private ElapsedTime runtime = new ElapsedTime();

    //DC-MOTORS
    private DcMotor LFMotor, LRMotor, RFMotor, RRMotor;
    public DcMotor Launcher, Collector, Elevator;

    //SENSORS
    public OpticalDistanceSensor ODS; //MODERN ROBOTICS OPTICAL DISTANCE SENSOR

    public ModernRoboticsI2cRangeSensor Distance_Sensor; //MODERN ROBOTICS RANGE SENSOR

    public ColorSensor Color_Sensor; //MODERN ROBOTICS COLOR SENSOR

    //SERVOS
    public Servo BeaconServo1;

    static final double extend = 0.55;
    static final double retract = 0.02;

    //CAPBALL CLAMPING SERVO
    public CRServo Capball_Clamp;
    public Servo Capball_Clamp2;

    //LIFT SERVO
    public Servo Holder;

    //DRIVE TO WHITE LINE AND STOP
    private static final double WHITE_THRESHOLD = 0.086; // DIFFERENCE BETWEEN THE WHITE LINE AND THE GRAY TILES
    private static final double APPROACH_SPEED2 = 0.4;

    private static final double COUNTS_PER_MOTOR_REV = 1120; // USING ANDYMARK NEVEREST 60;1 MOTORS
    private static final double DRIVE_GEAR_REDUCTION = 1.0;
    private static final double WHEEL_DIAMETER_INCHES = 4.0; // CALCULATE THE 4" WHEEL CIRCUMFERENCE
    private static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
    (WHEEL_DIAMETER_INCHES * 3.1415);
    private static final double DRIVE_SPEED = 0.5;
    private static final double TURN_SPEED = 0.4;
    private static final double SLOW_DRIVE_SPEED = 0.3;
    private static final double FAST_DRIVE_SPEED = 0.7;

    @Override
    public void runOpMode() throws InterruptedException {
    //DRIVER MOTOR HARDWARE CONFIGURATION
    LFMotor = hardwareMap.dcMotor.get("lfmotor"); //LEFT MOTOR CONTROLLER PORT 1
    LFMotor.setDirection(DcMotor.Direction.REVERSE);
    LFMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENC ODER);
    LFMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) ;

    LRMotor = hardwareMap.dcMotor.get("lrmotor"); //LEFT MOTOR CONTROLLER PORT 2
    LRMotor.setDirection(DcMotor.Direction.REVERSE);
    LRMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENC ODER);
    LRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) ;

    RFMotor = hardwareMap.dcMotor.get("rfmotor"); //RIGHT MOTOR CONTROLLER PORT 1
    RFMotor.setDirection(DcMotor.Direction.FORWARD);
    RFMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENC ODER);
    RFMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) ;

    RRMotor = hardwareMap.dcMotor.get("rrmotor"); //RIGHT MOTOR CONTROLLER PORT 2
    RRMotor.setDirection(DcMotor.Direction.FORWARD);
    RRMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENC ODER);
    RRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) ;

    //SENSOR HARDWARE CONFIGURATION
    ODS = hardwareMap.opticalDistanceSensor.get("sensor_ods" ); //USED TO DETECT THE DIFFERENCE BETWEEN THE WHITE LINE AND GRAY TILES
    ODS.enableLed(true); // turn on LED of light sensor.

    Distance_Sensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class , "rangeSensor"); //USED TO DETERMINE THE DISTANCE TO THE BEACONS
    Distance_Sensor.getDistance(DistanceUnit.INCH);

    Color_Sensor = (hardwareMap.colorSensor.get("color")); //USED TO DETERMINE IF THE BEACON IS RED OR BLUE
    Color_Sensor.enableLed(false); //MAKE COLOR SENSOR PASSIVE

    //SERVO HARDWARE CONFIGURATION
    BeaconServo1 = hardwareMap.servo.get("bs1");
    BeaconServo1.setPosition(retract); //RETRACT BEACON SELECTOR TO REMAIN IN THE 18 INCH CUBE DURING SETUP

    Launcher = hardwareMap.dcMotor.get("launcher");
    Elevator = hardwareMap.dcMotor.get("elevator");
    Collector = hardwareMap.dcMotor.get("collector");

    Holder = hardwareMap.servo.get("holder");
    Holder.setPosition(.7);

    //CAPBALL CLAMPING SERVO
    Capball_Clamp = hardwareMap.crservo.get("clamp");
    Capball_Clamp2 = hardwareMap.servo.get("cc2");
    Capball_Clamp.setPower(0);
    Capball_Clamp2.setPosition(.9);

    // Wait for the game to start (driver presses PLAY)
    // Abort this loop is started or stopped.
    while (!(isStarted() || isStopRequested())) {

    // Display the light level while we are waiting to start
    telemetry.addData("Light Level", ODS.getLightDetected());
    telemetry.addData("Status", "Resetting Encoders");
    telemetry.addData("Path0", "Starting at %7d :%7d :%7d :%7d",
    LFMotor.getCurrentPosition(), LRMotor.getCurrentPosition(),
    RFMotor.getCurrentPosition(), RRMotor.getCurrentPosition());
    telemetry.update();
    // Send telemetry message to indicate successful Encoder reset

    //WAITING TO START ROUND

    waitForStart();

    encoderDrive(SLOW_DRIVE_SPEED, 14, 14, 14, 14, 5.0);

    //DELETE CODE TO SAVE SPACE

    encoderDrive(FAST_DRIVE_SPEED, 15, 15, 15, 15, 4.0);
    encoderDrive(TURN_SPEED, 10, 10, -10, -10, 4.0);
    encoderDrive(FAST_DRIVE_SPEED, 38, 38, 38, 38, 4.0);

    }

    }

    //END OF AUTONOMOUS


    public void encoderDrive(double speed,
    double leftInches1, double leftInches2,
    double rightInches1, double rightInches2, double timeoutS) {

    int newFrontLeftTarget, newFrontRightTarget, newRearLeftTarget, newRearRightTarget;

    // Ensure that the opmode is still active
    if (opModeIsActive()) {
    idle();
    // Determine new target position, and pass to motor controller
    newFrontLeftTarget = LFMotor.getCurrentPosition() + (int) (leftInches1 * COUNTS_PER_INCH);
    newRearLeftTarget = LRMotor.getCurrentPosition() + (int) (leftInches2 * COUNTS_PER_INCH);
    newFrontRightTarget = RFMotor.getCurrentPosition() + (int) (rightInches1 * COUNTS_PER_INCH);
    newRearRightTarget = RRMotor.getCurrentPosition() + (int) (rightInches2 * COUNTS_PER_INCH);
    idle();
    LFMotor.setTargetPosition(newFrontLeftTarget);
    LRMotor.setTargetPosition(newRearLeftTarget);
    RFMotor.setTargetPosition(newFrontRightTarget);
    RRMotor.setTargetPosition(newRearRightTarget);
    idle();
    LFMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);// Turn On RUN_TO_POSITION
    LRMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    RFMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    RRMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    idle();

    runtime.reset(); // reset the timeout time and start motion.

    LFMotor.setPower(Math.abs(speed));
    LRMotor.setPower(Math.abs(speed));
    RFMotor.setPower(Math.abs(speed));
    RRMotor.setPower(Math.abs(speed));
    idle();

    // keep looping while we are still active, and there is time left, and both motors are running.
    while (opModeIsActive() &&
    (runtime.seconds() < timeoutS) &&
    (LFMotor.isBusy() && LRMotor.isBusy() &&
    RFMotor.isBusy() && RRMotor.isBusy())) {

    telemetry.addData("Path1", "Running to %7d :%7d",
    newFrontLeftTarget, newRearLeftTarget,
    newFrontRightTarget, newRearRightTarget);
    telemetry.addData("Path2", "Running at %7d :%7d",
    LFMotor.getCurrentPosition(), LRMotor.getCurrentPosition(),
    RFMotor.getCurrentPosition(), RRMotor.getCurrentPosition());
    telemetry.update();
    }

    LFMotor.setPower(0);
    LRMotor.setPower(0);
    RFMotor.setPower(0);
    RRMotor.setPower(0);

    LFMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) ; // Turn off RUN_TO_POSITION
    LRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) ;
    RFMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) ;
    RRMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) ;


    }
    }
    }

  • #2
    Only one mode will be active at a time for each motor. The last mode set will be in effect.
    Certain modes (i.e. RUN_TO_POSITION) may take a little bit of time, but the SDK takes care of this and hides the details.

    Comment

    Working...
    X