karx/FreeOfCharge2022-23
1
1
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

MotorBox is now working

Browse source 
Comments added throughout
AutoModeTest has some initial code, untested


Former-commit-id: f6dfd4ac4c170befcecec1f889767857abc244ea
This commit is contained in:
nn2wg 2021-11-01 18:31:40 -05:00
parent aec0183577
commit 69caee2dc7
4 changed files with 171 additions and 95 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

48
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoModeTest.java
View file

@ -2,36 +2,48 @@ package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
@Autonomous
public class AutoModeTest extends LinearOpMode {
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
private double drive, strafe, turn;
private double driveMult = 0.25;
MotorBox motors;
// part that actually runs
@Override
public void runOpMode(){
initParts();
initStuff();
motors.drive(10, 0, 0, 1);
}
public void initParts(){
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
// converts turns at a specific angle
// assuming value passed is never 0
// TODO: measure encoder value corresponding to 90 degrees
/*
public void turn(int angle){
int strength;
int direction;
if (angle > 0)
direction = 1;
else
direction = -1;
strength = // TODO: measure and add corresponding code
motors.drive(strength, 0, direction, 1);
}
*/
// turns on encoders
frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rearRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rearLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// telemetry messages
telemetry.addData("Status", "Initialized");
telemetry.update();
// initializes the MotorBox motors
public void initStuff(){
motors = new MotorBox(
hardwareMap.get(DcMotor.class, "frontRight"),
hardwareMap.get(DcMotor.class, "frontLeft"),
hardwareMap.get(DcMotor.class, "rearRight"),
hardwareMap.get(DcMotor.class, "rearLeft"),
true
);
}
}

110
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorBox.java Normal file
View file

@ -0,0 +1,110 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.hardware.DcMotor;
public class MotorBox {
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
private double frontRightPower, frontLeftPower, rearRightPower, rearLeftPower;
private boolean toPosition;
public MotorBox(DcMotor frontRight, DcMotor frontLeft, DcMotor rearRight, DcMotor rearLeft, boolean toPosition){
init(frontRight, frontLeft, rearRight, rearLeft, toPosition);
}
// drive code
// works with both encoder power and encoder position theoretically
public String drive(double yDrive, double xDrive, double turn, double speed) {
// sets up the initial power values based on input
frontRightPower = yDrive - xDrive;
frontLeftPower = yDrive + xDrive;
rearRightPower = yDrive + xDrive;
rearLeftPower = yDrive - xDrive;
// debugging
String returnVal = "Front Right = " + frontRightPower + "\nFront Left = " + frontLeftPower + "\nRear Right = " + rearRightPower + "\nRear Left = " + rearLeftPower +
"\nyDrive = " + yDrive + "\nxDrive = " + xDrive +
"\nTurn = " + turn;
// does the turn calculations
if (turn != 0) {
addTurn(turn);
}
// multiplies by speed
frontRightPower *= speed;
frontLeftPower *= speed;
rearRightPower *= speed;
rearLeftPower *= speed;
// applies the power
frontRight.setPower(-1 * frontRightPower);
frontLeft.setPower(frontLeftPower);
rearRight.setPower(-1 * rearRightPower);
rearLeft.setPower(rearLeftPower);
// return for debugging
return returnVal;
}
// changes value corresponding to turn
// two states based on if the MotorControl
private void addTurn(double turn){
if (toPosition) {
if (turn > 0) {
frontRightPower *= -1;
rearRightPower *= -1;
}
if (turn < 0) {
frontLeftPower *= -1;
rearLeftPower *= -1;
}
}
else {
if (turn > 0) {
frontRightPower *= (1 - turn);
rearRightPower *= (1 - turn);
}
if (turn < 0) {
frontLeftPower *= (1 - turn);
rearLeftPower *= (1 - turn);
}
}
}
// initializes the motors, sets up the correct encoder mode, and sets the toPosition boolean
private void init(DcMotor motor1, DcMotor motor2, DcMotor motor3, DcMotor motor4, boolean toPosition){
frontRight = motor1;
frontLeft = motor2;
rearRight = motor3;
rearLeft = motor4;
// stops and resets encoders
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rearRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rearLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// sets the toPosition boolean
this.toPosition = toPosition;
// turns on encoders
if (toPosition) {
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
else{
frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rearRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rearLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}
}

71
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorControl.java
View file

@ -1,71 +0,0 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap;
import com.qualcomm.robotcore.hardware.DcMotor;
public class MotorControl {
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
private double frontRightPower, frontLeftPower, rearRightPower, rearLeftPower;
public MotorControl(){
init();
}
private void calcDrive(double yDrive, double xDrive){
frontRightPower = yDrive - xDrive;
frontLeftPower = yDrive + xDrive;
rearRightPower = yDrive + xDrive;
rearLeftPower = yDrive - xDrive;
}
private void addTurn(double turn){
if (turn > 0) {
frontRightPower *= (1 - turn);
rearRightPower *= (1 - turn);
}
if (turn < 0) {
frontLeftPower *= (1 + turn);
rearLeftPower *= (1 + turn);
}
}
public void drive(double yDrive, double xDrive, double speed, double turn){
calcDrive(yDrive, xDrive);
if (turn != 0) {
addTurn(turn);
}
frontRightPower*=speed;
frontLeftPower*=speed;
rearRightPower*=speed;
rearLeftPower*=speed;
frontRight.setPower(-1*frontRightPower);
frontLeft.setPower(frontLeftPower);
rearRight.setPower(-1*rearRightPower);
rearLeft.setPower(rearLeftPower);
}
private void init(){
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
// stops and resets encoders
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rearRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rearLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// turns on encoders
frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rearRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rearLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}

37
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controllerOpMode.java
View file

@ -9,30 +9,55 @@ public class controllerOpMode extends OpMode {
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
private double drive,strafe, turn;
private double speed;
MotorControl driveMotors;
MotorBox driveMotors;
// part that actually runs
// naturally loops
public void loop() {
// checks to see if the speed change button is pressed
detectSpeedChange();
drive = gamepad1.left_stick_y;
// sets the driver strafe and turn values
// drive is negated so 1 is forward
drive = -1*gamepad1.left_stick_y;
strafe = gamepad1.left_stick_x;
turn = gamepad1.right_stick_x;
// prints the speed
telemetry.addData("Speed", "Current Speed = " + Math.round(speed*100));
telemetry.update();
driveMotors.drive(drive, strafe, turn, speed);
// makes it drive
// result is just debug values
String result = driveMotors.drive(drive, strafe, turn, speed);
//debugging
//telemetry.addData("Results", result);
/*
telemetry.addData("Drive", "Drive = " + drive);
telemetry.addData("Strafe", "Strafe = " + strafe);
*/
//show telemetry
telemetry.update();
}
// initializes the driveMotors MotorBox and speed
public void init(){
driveMotors = new MotorControl();
driveMotors = new MotorBox(
hardwareMap.get(DcMotor.class, "frontRight"),
hardwareMap.get(DcMotor.class, "frontLeft"),
hardwareMap.get(DcMotor.class, "rearRight"),
hardwareMap.get(DcMotor.class, "rearLeft"),
false
);
speed = 0.25;
}
// detects the speed change button and changes speed accordingly
public void detectSpeedChange(){
if (gamepad1.dpad_up){
if (speed <= 1) {