Added debugging for encoder logic
This commit is contained in:
@ -29,8 +29,6 @@
|
|||||||
|
|
||||||
package org.firstinspires.ftc.teamcode;
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
||||||
@ -39,12 +37,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
|||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
|
||||||
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
|
||||||
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
@ -81,12 +74,15 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
|||||||
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
|
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
|
||||||
*/
|
*/
|
||||||
|
|
||||||
@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode")
|
@TeleOp(name = "Basic: Omni Linear OpMode", group = "Linear OpMode")
|
||||||
public class BasicOmniOpMode_Linear extends LinearOpMode {
|
public class BasicOmniOpMode_Linear extends LinearOpMode {
|
||||||
|
|
||||||
// Declare OpMode members for each of the 4 motors.
|
// Declare OpMode members for each of the 4 motors.
|
||||||
private final ElapsedTime runtime = new ElapsedTime();
|
private final ElapsedTime runtime = new ElapsedTime();
|
||||||
|
private Encoder leftFront;
|
||||||
|
private Encoder rightFront;
|
||||||
|
private Encoder leftRear;
|
||||||
|
private Encoder rightRear;
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() {
|
public void runOpMode() {
|
||||||
|
|
||||||
@ -97,18 +93,12 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
||||||
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
|
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
|
||||||
|
|
||||||
|
leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR));
|
||||||
|
leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR));
|
||||||
|
rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR));
|
||||||
|
rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR));
|
||||||
|
|
||||||
|
|
||||||
// TODO: replace these with your encoder ports
|
|
||||||
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
|
||||||
Encoder rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
|
|
||||||
Encoder strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
|
|
||||||
|
|
||||||
// TODO: reverse any encoders necessary
|
|
||||||
leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
|
|
||||||
rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
|
|
||||||
strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
|
|
||||||
|
|
||||||
// ########################################################################################
|
// ########################################################################################
|
||||||
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
|
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
|
||||||
// ########################################################################################
|
// ########################################################################################
|
||||||
@ -124,11 +114,20 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
|
rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
|
||||||
rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
|
rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
|
||||||
|
|
||||||
|
leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
|
||||||
|
leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
leftBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
rightBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// Wait for the game to start (driver presses START)
|
// Wait for the game to start (driver presses START)
|
||||||
telemetry.addData("Status", "Initialized");
|
telemetry.addData("Status", "Initialized");
|
||||||
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
|
|
||||||
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
|
|
||||||
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
|
|
||||||
waitForStart();
|
waitForStart();
|
||||||
@ -139,16 +138,16 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
double max;
|
double max;
|
||||||
|
|
||||||
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
|
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
|
||||||
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
|
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
|
||||||
double lateral = gamepad1.left_stick_x;
|
double lateral = gamepad1.left_stick_x;
|
||||||
double yaw = gamepad1.right_stick_x;
|
double yaw = gamepad1.right_stick_x;
|
||||||
|
|
||||||
// Combine the joystick requests for each axis-motion to determine each wheel's power.
|
// Combine the joystick requests for each axis-motion to determine each wheel's power.
|
||||||
// Set up a variable for each drive wheel to save the power level for telemetry.
|
// Set up a variable for each drive wheel to save the power level for telemetry.
|
||||||
double leftFrontPower = axial + lateral + yaw;
|
double leftFrontPower = axial + lateral + yaw;
|
||||||
double rightFrontPower = axial - lateral - yaw;
|
double rightFrontPower = axial - lateral - yaw;
|
||||||
double leftBackPower = axial - lateral + yaw;
|
double leftBackPower = axial - lateral + yaw;
|
||||||
double rightBackPower = axial + lateral - yaw;
|
double rightBackPower = axial + lateral - yaw;
|
||||||
|
|
||||||
// Normalize the values so no wheel power exceeds 100%
|
// Normalize the values so no wheel power exceeds 100%
|
||||||
// This ensures that the robot maintains the desired motion.
|
// This ensures that the robot maintains the desired motion.
|
||||||
@ -157,10 +156,10 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
max = Math.max(max, Math.abs(rightBackPower));
|
max = Math.max(max, Math.abs(rightBackPower));
|
||||||
|
|
||||||
if (max > 1.0) {
|
if (max > 1.0) {
|
||||||
leftFrontPower /= max;
|
leftFrontPower /= max;
|
||||||
rightFrontPower /= max;
|
rightFrontPower /= max;
|
||||||
leftBackPower /= max;
|
leftBackPower /= max;
|
||||||
rightBackPower /= max;
|
rightBackPower /= max;
|
||||||
}
|
}
|
||||||
|
|
||||||
// This is test code:
|
// This is test code:
|
||||||
@ -187,12 +186,14 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
rightBackDrive.setPower(rightBackPower);
|
rightBackDrive.setPower(rightBackPower);
|
||||||
|
|
||||||
// Show the elapsed game time and wheel power.
|
// Show the elapsed game time and wheel power.
|
||||||
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
telemetry.addData("Status", "Run Time: " + runtime);
|
||||||
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
|
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
|
||||||
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
|
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
|
||||||
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
|
telemetry.addData("Encoder Front Left", leftFrontDrive.getDirection() + " : " + leftFrontDrive.getCurrentPosition());
|
||||||
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
|
telemetry.addData("Encoder Front Right", rightFrontDrive.getCurrentPosition());
|
||||||
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
telemetry.addData("Encoder Back Left", leftBackDrive.getCurrentPosition());
|
||||||
|
telemetry.addData("Encoder Back Right", rightBackDrive.getCurrentPosition());
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
}}
|
}
|
||||||
|
}
|
||||||
|
Reference in New Issue
Block a user