Added team notes and simplified Chassis Robot Driver code
This commit is contained in:
4
NOTES.md
Normal file
4
NOTES.md
Normal file
@ -0,0 +1,4 @@
|
||||
- Refactor of code
|
||||
- Possibly establish patterns
|
||||
- Also, establish github for students as an element of a professional portfolio (laura)
|
||||
-
|
@ -0,0 +1,99 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
@TeleOp( name = "scds-chassis-manual")
|
||||
public class SCDSChassisDriverMode extends OpMode {
|
||||
|
||||
DcMotor frontRight;
|
||||
DcMotor backRight;
|
||||
DcMotor frontLeft;
|
||||
DcMotor backLeft;
|
||||
public double axial;
|
||||
public double lateral;
|
||||
public double yaw;
|
||||
|
||||
final static double MOTOR_LO_SPEED_RATIO = 3.5;
|
||||
final static double MOTOR_HI_SPEED_RATIO = 2.25;
|
||||
final static double ARM_POWER = 3.5;
|
||||
double RUNNING_MOTOR_SPEED_RATIO = MOTOR_LO_SPEED_RATIO;
|
||||
double CURRENT_SPEED_RATIO = MOTOR_HI_SPEED_RATIO;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
telemetry.addData("Status","In Init()");
|
||||
telemetry.update();
|
||||
frontRight = hardwareMap.dcMotor.get("Drive front rt");
|
||||
backRight = hardwareMap.dcMotor.get("Drive back rt");
|
||||
frontLeft = hardwareMap.dcMotor.get("Drive front lt");
|
||||
backLeft = hardwareMap.dcMotor.get("Drive back lt");
|
||||
}
|
||||
|
||||
private void setForwardDirection() {
|
||||
telemetry.addData("Status","setForwardDirection()");
|
||||
telemetry.update();
|
||||
frontLeft.setDirection(DcMotor.Direction.REVERSE);
|
||||
backLeft.setDirection(DcMotor.Direction.FORWARD);
|
||||
frontRight.setDirection(DcMotor.Direction.REVERSE);
|
||||
backRight.setDirection(DcMotor.Direction.FORWARD);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
/*
|
||||
Initialize the wheels
|
||||
*/
|
||||
setForwardDirection();
|
||||
|
||||
/*
|
||||
Turn on high speed on the motors
|
||||
*/
|
||||
if(gamepad1.a) {
|
||||
RUNNING_MOTOR_SPEED_RATIO = MOTOR_HI_SPEED_RATIO;
|
||||
}
|
||||
|
||||
/*
|
||||
Turn on low speed on the motors
|
||||
*/
|
||||
if(gamepad1.b) {
|
||||
RUNNING_MOTOR_SPEED_RATIO = MOTOR_LO_SPEED_RATIO;
|
||||
}
|
||||
|
||||
axial = -gamepad1.left_stick_y/CURRENT_SPEED_RATIO; // Note: pushing stick forward gives negative value
|
||||
lateral = gamepad1.left_stick_x/CURRENT_SPEED_RATIO;
|
||||
yaw = gamepad1.right_stick_x/CURRENT_SPEED_RATIO;
|
||||
|
||||
// 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.
|
||||
double leftFrontPower = axial + lateral + yaw;
|
||||
double rightFrontPower = axial - lateral - yaw;
|
||||
double leftBackPower = axial - lateral + yaw;
|
||||
double rightBackPower = axial + lateral - yaw;
|
||||
|
||||
// Normalize the values so no wheel power exceeds 100%
|
||||
// This ensures that the robot maintains the desired motion.
|
||||
double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
|
||||
max = Math.max(max, Math.abs(leftBackPower));
|
||||
max = Math.max(max, Math.abs(rightBackPower));
|
||||
|
||||
if (max > 1.0) {
|
||||
leftFrontPower /= max;
|
||||
rightFrontPower /= max;
|
||||
leftBackPower /= max;
|
||||
rightBackPower /= max;
|
||||
}
|
||||
frontLeft.setPower(leftFrontPower);
|
||||
frontRight.setPower(rightFrontPower);
|
||||
backLeft.setPower(leftBackPower);
|
||||
backRight.setPower(rightBackPower);
|
||||
|
||||
// Show the elapsed game time and wheel power
|
||||
telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
|
||||
telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
|
||||
telemetry.update();
|
||||
|
||||
}
|
||||
|
||||
}
|
Reference in New Issue
Block a user