motor control is moved to MotorControl.java
untested code probably doesn't work pushing to main anyways
This commit is contained in:
parent
9f98e079ff
commit
eaa9343791
|
@ -1,5 +1,6 @@
|
||||||
<?xml version="1.0" encoding="UTF-8"?>
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
<project version="4">
|
<project version="4">
|
||||||
|
<component name="ExternalStorageConfigurationManager" enabled="true" />
|
||||||
<component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="Android Studio default JDK" project-jdk-type="JavaSDK">
|
<component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="Android Studio default JDK" project-jdk-type="JavaSDK">
|
||||||
<output url="file://$PROJECT_DIR$/build/classes" />
|
<output url="file://$PROJECT_DIR$/build/classes" />
|
||||||
</component>
|
</component>
|
||||||
|
|
|
@ -0,0 +1,37 @@
|
||||||
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
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;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode(){
|
||||||
|
initParts();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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");
|
||||||
|
|
||||||
|
// 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();
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,71 @@
|
||||||
|
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);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
|
@ -1,5 +1,6 @@
|
||||||
package org.firstinspires.ftc.teamcode;
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
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;
|
||||||
|
@ -9,7 +10,7 @@ import com.qualcomm.robotcore.hardware.Gyroscope;
|
||||||
import com.qualcomm.robotcore.hardware.Servo;
|
import com.qualcomm.robotcore.hardware.Servo;
|
||||||
|
|
||||||
@TeleOp
|
@TeleOp
|
||||||
|
@Disabled
|
||||||
public class MyFIRSTJavaOpMode extends LinearOpMode {
|
public class MyFIRSTJavaOpMode extends LinearOpMode {
|
||||||
private Gyroscope imu;
|
private Gyroscope imu;
|
||||||
private DcMotor frontRight;
|
private DcMotor frontRight;
|
||||||
|
|
|
@ -8,41 +8,43 @@ import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
public class controllerOpMode extends OpMode {
|
public class controllerOpMode extends OpMode {
|
||||||
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
|
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
|
||||||
private double drive,strafe, turn;
|
private double drive,strafe, turn;
|
||||||
private double driveMult = 0.25;
|
private double speed;
|
||||||
public void init(){
|
MotorControl driveMotors;
|
||||||
frontRight = hardwareMap.get(DcMotor.class, "frontRight");
|
|
||||||
frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
|
|
||||||
rearRight = hardwareMap.get(DcMotor.class, "rearRight");
|
|
||||||
rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
|
|
||||||
telemetry.addData("Status", "Initialized");
|
|
||||||
telemetry.update();
|
|
||||||
}
|
|
||||||
public void loop() {
|
public void loop() {
|
||||||
strafe = gamepad1.left_stick_x;
|
|
||||||
|
detectSpeedChange();
|
||||||
|
|
||||||
drive = gamepad1.left_stick_y;
|
drive = gamepad1.left_stick_y;
|
||||||
|
strafe = gamepad1.left_stick_x;
|
||||||
turn = gamepad1.right_stick_x;
|
turn = gamepad1.right_stick_x;
|
||||||
|
|
||||||
|
telemetry.addData("Speed", "Current Speed = " + Math.round(speed*100));
|
||||||
|
telemetry.update();
|
||||||
|
driveMotors.drive(drive, strafe, turn, speed);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
public void init(){
|
||||||
|
driveMotors = new MotorControl();
|
||||||
|
speed = 0.25;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void detectSpeedChange(){
|
||||||
if (gamepad1.dpad_up){
|
if (gamepad1.dpad_up){
|
||||||
if (driveMult<=1) {
|
if (speed <= 1) {
|
||||||
driveMult += 0.0005;
|
speed += 0.0005;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (gamepad1.dpad_down){
|
if (gamepad1.dpad_down){
|
||||||
if (driveMult>=0) {
|
if (speed >= 0) {
|
||||||
driveMult -= 0.0005;
|
speed -= 0.0005;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
telemetry.addData("Speed", "Current Speed = " + Math.round(driveMult*100));
|
|
||||||
telemetry.update();
|
|
||||||
|
|
||||||
|
|
||||||
frontRight.setPower(driveMult*(drive - strafe - turn));
|
|
||||||
rearRight.setPower(driveMult*(drive + strafe - turn));
|
|
||||||
frontLeft.setPower(-1*driveMult*(drive + strafe + turn));
|
|
||||||
rearLeft.setPower(-1*driveMult*(drive - strafe + turn));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in a new issue