Announcement

Collapse
No announcement yet.

Servo Moving when Teleop is Initialized

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

  • Servo Moving when Teleop is Initialized

    When we initialize our teleop program, our servo "lowerArm" changes position drastically. We cannot find the issue in the program and any help would be greatly appreciated! This is the program:







    package org.firstinspires.ftc.teamcode;


    import com.qualcomm.robotcore.eventloop.opmode.OpMode;
    import com.qualcomm.robotcore.util.ElapsedTime;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.Servo;
    import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
    import com.qualcomm.robotcore.util.Range;

    /**
    * This file contains an example of an iterative (Non-Linear) "OpMode".
    * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
    * The names of OpModes appear on the menu of the FTC Driver Station.
    * When an selection is made from the menu, the corresponding OpMode
    * class is instantiated on the Robot Controller and executed.
    *
    * This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot
    * It includes all the skeletal structure that all iterative OpModes contain.
    *
    * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
    * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
    */

    @TeleOp(name="Tom", group="Iterative Opmode")

    public class Tom extends OpMode{

    org.firstinspires.ftc.teamcode.HardwarePushbot robot = new HardwarePushbot();
    // Declare OpMode members.
    private ElapsedTime runtime = new ElapsedTime();
    private DcMotor leftDrive = null;
    private DcMotor rightDrive = null;
    private Servo top_hand = null;
    private Servo lowerArm = null;
    private Servo upperArm = null;
    private Servo leftHand = null;




    private double handOffset = 0.0 ; // Servo mid position
    private final double CLAW_SPEED = 0.01 ; // sets rate to move serv
    private final double HAND_SPEED = 0.01 ; // sets rate to move serv
    private double upper;
    private double lower;
    /*
    * Code to run ONCE when the driver hits INIT
    */
    @Override
    public void init() {
    robot.init(hardwareMap);


    // Initialize the hardware variables. Note that the strings used here as parameters
    // to 'get' must correspond to the names assigned during the robot configuration
    // step (using the FTC Robot Controller app on the phone).
    leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
    rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
    top_hand = hardwareMap.get(Servo.class, "top_hand");
    leftHand = hardwareMap.get(Servo.class, "leftHand");
    upperArm = hardwareMap.get(Servo.class, "upperArm");
    lowerArm = hardwareMap.get(Servo.class, "lowerArm");


    // Most robots need the motor on one side to be reversed to drive forward
    // Reverse the motor that runs backwards when connected directly to the battery
    leftDrive.setDirection(DcMotor.Direction.REVERSE);
    rightDrive.setDirection(DcMotor.Direction.FORWARD) ;

    // Tell the driver that initialization is complete.
    telemetry.addData("Status", "Initialized");

    }

    /*
    * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
    */
    @Override
    public void init_loop() {
    }

    /*
    * Code to run ONCE when the driver hits PLAY
    */
    @Override
    public void start() {
    }

    /*
    * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
    */
    @Override
    public void loop() {
    // Setup a variable for each drive wheel to save power level for telemetry
    double leftPower;
    double rightPower;


    if (gamepad1.right_bumper) {
    handOffset += CLAW_SPEED;
    }
    else if (gamepad1.left_bumper)
    handOffset -= CLAW_SPEED;


    //Press front bumpers to move hand and pick up score elements
    if (gamepad2.right_bumper) {
    handOffset += HAND_SPEED;
    }
    else if (gamepad2.left_bumper)
    handOffset -= HAND_SPEED;


    // Tank Mode uses one stick to control each wheel.
    // - This requires no math, but it is hard to drive forward slowly and keep straight.
    rightPower = -gamepad1.left_stick_y ;
    leftPower = -gamepad1.right_stick_y ;

    // Send calculated power to arm
    leftDrive.setPower(leftPower);
    rightDrive.setPower(rightPower);

    upper = gamepad2.left_stick_y*.5 ;
    lower = gamepad2.right_stick_y*.5 ;

    upperArm.setPosition(upper);
    lowerArm.setPosition(lower);


    // Show the elapsed game time and wheel power.
    telemetry.addData("Status", "Run Time: " + runtime.toString());
    telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower);

    }

    /*
    * Code to run ONCE after the driver hits STOP
    */
    @Override
    public void stop() {
    }

    }


  • #2
    You need to move each of your servos to your desired starting position in your init() method'. Since you are using the joystick to control the servo positions, you probably want to start the servos off at 0.0:

    Code:
    @Override
    public void init() {
       ...
       upperArm = hardwareMap.get(Servo.class, "upperArm");
       lowerArm = hardwareMap.get(Servo.class, "lowerArm");
       upperArm.setPosition(0.0);
       lowerArm.setPosition(0.0);
       ...
    }

    Comment


    • #3
      You should use a servo programmer or test program to determine your servo starting and extent positions. Then during init, set the position to the starting position (may or may not be 0).

      Comment


      • #4
        It doesn't make sense to me that running this teleop will cause a servo to move during initialization. That is because you don't use setPosition during the init() method.

        I'm going to respectfully disagree with Alex here - in that you don't want that setPosition(0) in your init method. At least not for teleop. (For Autonomnous you definitely do want it in init()) The reason is that between autonomous and teleop you don't want the robot moving at all because that can get you called for starting early. So I usually tell teams to put it in the start() method instead.

        As an experiment try putting this:
        Code:
        top_hand = hardwareMap.get(Servo.class, "top_hand");
        leftHand = hardwareMap.get(Servo.class, "leftHand");
        upperArm = hardwareMap.get(Servo.class, "upperArm");
        lowerArm = hardwareMap.get(Servo.class, "lowerArm");
        Into the start() method. My thought is that *maybe* the SDK is initializing the position as soon as the the hardwareMap.get() is called - or maybe the constructor of the Servo class. If after making that change the servos move when you hit "play" for the opmode that would say my guess is probably correcct. After that I would file a bug with the SDK people.

        Comment


        • #5
          Thank you Dan for the correction. Yes, you want to avoid moving the servo before start() in Teleop. Please disregard my earlier post.

          thensley, Make sure you run your autonomous before you run your teleop, and make sure you instantiate and set position on your "lowerArm" servo somewhere in your autonomous. Does that make a difference?

          Comment


          • #6
            One caution when using servos. Analog servos will typically go limp at loss of signal (LOS). Many/most digital servos will hold the last good position as long as they still have power. The Rev servo power module always supplies power.

            So in testing verify your situation and when asking for help, be sure to include hardware details
            Technical Coach, Newton Busters FTC 10138, FLL 3077
            Chicago 'burbs, IL

            Comment


            • #7
              Thank you all for you're suggestions! We had to change the servo position in the hardware program and this fixed the issue.

              Comment

              Working...
              X