TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-17 15:42:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Setup new repo, implemented basic mecanum drive for minibot

Browse Source
This commit is contained in:
Cyberarm
2022-01-25 17:30:15 -06:00
parent 704b6948e3
commit be2dbd24a8
12 changed files with 808 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/MecanumMinibot.java Normal file
View File

@@ -0,0 +1,85 @@
package org.timecrafters.minibots.cyberarm;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import org.cyberarm.engine.V2.CyberarmEngine;
public class MecanumMinibot {
public static final int TURN_LEFT = 0;
public static final int TURN_RIGHT = 1;
public static final int STRAFE_LEFT = 0;
public static final int STRAFE_RIGHT = 1;
private CyberarmEngine engine;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
public MecanumMinibot(CyberarmEngine engine) {
this.engine = engine;
setupDrivetrain();
}
private void setupDrivetrain() {
frontLeftDrive = engine.hardwareMap.dcMotor.get("frontLeft");
frontRightDrive = engine.hardwareMap.dcMotor.get("frontRight");
backLeftDrive = engine.hardwareMap.dcMotor.get("backLeft");
backRightDrive = engine.hardwareMap.dcMotor.get("backRight");
frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
frontRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
}
public void driveAll(double power) {
frontLeftDrive.setPower(power);
frontRightDrive.setPower(power);
backLeftDrive.setPower(power);
backRightDrive.setPower(power);
}
public void driveStop() {
driveAll(0);
}
public void driveTurn(int direction, double power) {
if (direction == TURN_LEFT) {
frontLeftDrive.setPower(-power);
backLeftDrive.setPower(-power);
frontRightDrive.setPower(power);
backRightDrive.setPower(power);
} else if (direction == TURN_RIGHT) {
frontLeftDrive.setPower(power);
backLeftDrive.setPower(power);
frontRightDrive.setPower(-power);
backRightDrive.setPower(-power);
} else {
throw new RuntimeException("Invalid direction for driveTurn()");
}
}
public void driveStrafe(int direction, double power) {
if (direction == STRAFE_LEFT) {
frontLeftDrive.setPower(power);
frontRightDrive.setPower(-power);
backLeftDrive.setPower(-power);
backRightDrive.setPower(power);
} else if (direction == STRAFE_RIGHT) {
frontLeftDrive.setPower(-power);
frontRightDrive.setPower(power);
backLeftDrive.setPower(power);
backRightDrive.setPower(-power);
} else {
throw new RuntimeException("Invalid direction for driveStrafe()");
}
}
}

19
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/MecanumMinibotTeleOpEngine.java Normal file
View File

@@ -0,0 +1,19 @@
package org.timecrafters.minibots.cyberarm.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.cyberarm.MecanumMinibot;
import org.timecrafters.minibots.cyberarm.states.MecanumMinibotTeleOpState;
@TeleOp(name = "MecanumMinibot TeleOp", group = "minibot")
public class MecanumMinibotTeleOpEngine extends CyberarmEngine {
MecanumMinibot robot;
@Override
public void setup() {
robot = new MecanumMinibot(this);
addState(new MecanumMinibotTeleOpState(robot));
}
}

54
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java Normal file
View File

@@ -0,0 +1,54 @@
package org.timecrafters.minibots.cyberarm.states;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.cyberarm.MecanumMinibot;
public class MecanumMinibotTeleOpState extends CyberarmState {
private final MecanumMinibot robot;
private float speed;
public MecanumMinibotTeleOpState(MecanumMinibot robot) {
this.robot = robot;
}
@Override
public void exec() {
speed = 1.0f - engine.gamepad1.left_trigger;
if (engine.gamepad1.y) {
robot.driveAll(speed);
} else if (engine.gamepad1.a) {
robot.driveAll(-speed);
} else if (engine.gamepad1.x) {
robot.driveStrafe(MecanumMinibot.STRAFE_LEFT, speed);
} else if (engine.gamepad1.b) {
robot.driveStrafe(MecanumMinibot.STRAFE_RIGHT, speed);
} else if (engine.gamepad1.left_bumper) {
robot.driveTurn(MecanumMinibot.TURN_LEFT, speed);
} else if (engine.gamepad1.right_bumper) {
robot.driveTurn(MecanumMinibot.TURN_RIGHT, speed);
} else {
robot.driveStop();
}
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
super.buttonDown(gamepad, button);
}
@Override
public void buttonUp(Gamepad gamepad, String button) {
super.buttonUp(gamepad, button);
}
@Override
public void telemetry() {
engine.telemetry.addData("speed", speed);
}
}