Announcement

Collapse
No announcement yet.

Strafing during autonomous

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

  • Strafing during autonomous

    Our robot strafes during teleop but not during our autonomous code. The robot drives backwards 14 inches then makes a twitch and the program ends, it should strafe right 24 inches. Here is our code.
    package org.firstinspires.ftc.teamcode;

    import com.qualcomm.robotcore.eventloop.opmode.Autonomous ;
    import com.qualcomm.robotcore.robot.Robot;
    import com.qualcomm.robotcore.util.ElapsedTime;
    import com.qualcomm.robotcore.hardware.ColorSensor;
    import com.qualcomm.robotcore.eventloop.opmode.LinearOpMo de;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.Servo;
    import com.qualcomm.robotcore.util.ElapsedTime;

    @Autonomous(name="Red_right_drive", group="Pushbot")
    //@Disabled
    public class Red_right_drive extends LinearOpMode {

    /* Declare OpMode members. */
    RobotHardware robot = new RobotHardware(); // Utilize RobotHardware to Access OpMode Members
    private ElapsedTime runtime = new ElapsedTime();

    @Override
    public void runOpMode() throws InterruptedException {
    robot.init(hardwareMap);

    // Telemetry Message During Robot Autonmous
    telemetry.addData("Status", "Ready to run");
    telemetry.update();

    // Wait in Game
    while (!opModeIsActive()&&!isStopRequested())
    {telemetry.addData("Status", "Waiting in Init");
    telemetry.update(); }

    //Step One : Calculate the distance of travel based on encoders: 18 tics/inch

    // Left Motors
    int initLeft1Count = robot.left1Motor.getCurrentPosition();
    int tgLeft1Count = initLeft1Count + 18;
    int initLeft2Count = robot.left2Motor.getCurrentPosition();
    int tgLeft2Count = initLeft2Count +18;

    // Right Motors
    int initRight1Count = robot.right1Motor.getCurrentPosition();
    int tgRight1Count = initRight1Count + 18;
    int initRight2Count = robot.right2Motor.getCurrentPosition();
    int tgRight2Count = initRight2Count + 18;

    {telemetry.addData("Status", "Step One Complete");
    telemetry.update(); }


    // Step Two : Stop and Reset Encoders

    robot.left1Motor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);
    robot.left2Motor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);
    robot.right1Motor.setMode(DcMotor.RunMode.STOP_AND _RESET_ENCODER);
    robot.right2Motor.setMode(DcMotor.RunMode.STOP_AND _RESET_ENCODER);

    robot.left1Motor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);
    robot.left2Motor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);
    robot.right1Motor.setMode(DcMotor.RunMode.RUN_USIN G_ENCODER);
    robot.right2Motor.setMode(DcMotor.RunMode.RUN_USIN G_ENCODER);

    {telemetry.addData("Status", "Step Two Complete");
    telemetry.update(); }


    // Step Three : drive towards the foundation

    robot.left1Motor.setPower(-.5);
    robot.left2Motor.setPower(-.5);
    robot.right1Motor.setPower(-.5);
    robot.right2Motor.setPower(-.5);

    while (opModeIsActive() && (robot.left1Motor.getCurrentPosition() > tgLeft1Count * -14)) ;
    while (opModeIsActive() && (robot.left2Motor.getCurrentPosition() > tgLeft2Count * -14)) ;
    while (opModeIsActive() && (robot.right1Motor.getCurrentPosition()> tgRight1Count * -14)) ;
    while (opModeIsActive() && (robot.right2Motor.getCurrentPosition() > tgRight2Count * -14)) ;

    {telemetry.addData("Status", "Step Three Complete");
    telemetry.update(); }


    //The program works great above this point!!!

    // Step Four : Stop and Reset Encoders

    robot.left1Motor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);
    robot.left2Motor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);
    robot.right1Motor.setMode(DcMotor.RunMode.STOP_AND _RESET_ENCODER);
    robot.right2Motor.setMode(DcMotor.RunMode.STOP_AND _RESET_ENCODER);

    sleep(1000);

    {telemetry.addData("Status", "Step Four Complete");
    telemetry.update(); }

    //NOTHING BELOW THIS POINT WORKS...
    // Step Five : Drive Forward Towards Foundation
    //
    robot.left1Motor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);
    robot.left2Motor.setMode(DcMotor.RunMode.STOP_AND_ RESET_ENCODER);
    robot.right1Motor.setMode(DcMotor.RunMode.STOP_AND _RESET_ENCODER);
    robot.right2Motor.setMode(DcMotor.RunMode.STOP_AND _RESET_ENCODER);

    robot.left1Motor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);
    robot.left2Motor.setMode(DcMotor.RunMode.RUN_USING _ENCODER);
    robot.right1Motor.setMode(DcMotor.RunMode.RUN_USIN G_ENCODER);
    robot.right2Motor.setMode(DcMotor.RunMode.RUN_USIN G_ENCODER);

    robot.left1Motor.setPower(.5);
    robot.left2Motor.setPower(-.5);
    robot.right1Motor.setPower(-.5);
    robot.right2Motor.setPower(.5);

    while (opModeIsActive() && (robot.left1Motor.getCurrentPosition() > tgLeft1Count * 24)) ;
    while (opModeIsActive() && (robot.left2Motor.getCurrentPosition() < tgLeft2Count * -24)) ;
    while (opModeIsActive() && (robot.right1Motor.getCurrentPosition() < tgRight1Count * -24)) ;
    while (opModeIsActive() && (robot.right2Motor.getCurrentPosition() > tgRight2Count * 24)) ;
    Last edited by thensley; 10-31-2019, 03:25 PM.

  • #2
    Not positive this will work, but we set all motor powers to zero before resetting our encoders. Try adding
    robot.left1Motor.setPower(0);
    robot.left2Motor.setPower(0);
    robot.right1Motor.setPower(0);
    robot.right2Motor.setPower(0);

    before your stop and reset encoder step in step 4 of your program. Also you should not need the second set of stop and reset encoder lines.

    Comment


    • #3
      I think your second set of whiles are off. The greater thans and less thans are reversed. When it starts, the current position (0) is less than tgLeft1Count * 24, so the while is never true. This explains the little twitch where the powers are set but the position is immediately reached. Hope this helps!

      Comment

      Working...
      X