mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-15 20:12:35 +00:00
Automated Drive
This commit is contained in:
SpencerPiha
@@ -1,11 +1,14 @@
|
||||
package org.timecrafters.testing.states;
|
||||
|
||||
import com.qualcomm.hardware.bosch.BNO055IMU;
|
||||
import com.qualcomm.robotcore.hardware.CRServo;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||
|
||||
public class PrototypeBot1 {
|
||||
|
||||
@@ -16,6 +19,8 @@ public class PrototypeBot1 {
|
||||
|
||||
public CRServo collectorLeft, collectorRight;
|
||||
|
||||
public BNO055IMU imu;
|
||||
|
||||
// public Servo collectorWrist;
|
||||
|
||||
public PrototypeBot1(CyberarmEngine engine) {
|
||||
@@ -25,12 +30,25 @@ public class PrototypeBot1 {
|
||||
}
|
||||
|
||||
private void setupRobot () {
|
||||
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||
|
||||
parameters.mode = BNO055IMU.SensorMode.IMU;
|
||||
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
|
||||
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
|
||||
parameters.loggingEnabled = false;
|
||||
|
||||
this.imu = engine.hardwareMap.get(BNO055IMU.class,"imu");
|
||||
imu.initialize(parameters);
|
||||
|
||||
imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
|
||||
|
||||
|
||||
|
||||
//motors configuration
|
||||
frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left");
|
||||
frontRightDrive = engine.hardwareMap.dcMotor.get("Front Right");
|
||||
backRightDrive = engine.hardwareMap.dcMotor.get("Back Left");
|
||||
backLeftDrive = engine.hardwareMap.dcMotor.get("Back Right");
|
||||
backRightDrive = engine.hardwareMap.dcMotor.get("Back Right");
|
||||
backLeftDrive = engine.hardwareMap.dcMotor.get("Back Left");
|
||||
|
||||
// servo configuration
|
||||
|
||||
@@ -46,16 +64,16 @@ public class PrototypeBot1 {
|
||||
|
||||
//motors direction and encoders
|
||||
|
||||
frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
frontRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
backLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
}
|
||||
|
||||
@@ -2,14 +2,10 @@ package org.timecrafters.testing.states;
|
||||
|
||||
import android.annotation.SuppressLint;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
import java.lang.annotation.Target;
|
||||
import java.util.Objects;
|
||||
|
||||
public class PrototypeTeleOPState extends CyberarmState {
|
||||
|
||||
private final PrototypeBot1 robot;
|
||||
@@ -27,7 +23,8 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
private double speed = 1; // used for the normal speed while driving
|
||||
private double slowSpeed = 0.5; // used for slow mode speed while driving
|
||||
private int CyclingArmUpAndDown = 0;
|
||||
private int DriveFowardAndBack;
|
||||
private int DriveForwardAndBack;
|
||||
private int RobotPosition, RobotStartingPosition;
|
||||
|
||||
public PrototypeTeleOPState(PrototypeBot1 robot) {
|
||||
this.robot = robot;
|
||||
@@ -94,26 +91,25 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
// bprev = false;
|
||||
// }
|
||||
|
||||
//Drivetrain Variables
|
||||
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
|
||||
double x = engine.gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
|
||||
double rx = engine.gamepad1.left_stick_x;
|
||||
|
||||
// Denominator is the largest motor power (absolute value) or 1
|
||||
// This ensures all the powers maintain the same ratio, but only when
|
||||
// at least one is out of the range [-1, 1]
|
||||
|
||||
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (y + x + rx) / denominator;
|
||||
double backLeftPower = (y - x + rx) / denominator;
|
||||
double frontRightPower = (y - x - rx) / denominator;
|
||||
double backRightPower = (y + x - rx) / denominator;
|
||||
|
||||
robot.frontLeftDrive.setPower(-frontLeftPower * speed);
|
||||
robot.backLeftDrive.setPower(backLeftPower * speed);
|
||||
robot.frontRightDrive.setPower(-frontRightPower * speed);
|
||||
robot.backRightDrive.setPower(backRightPower * speed);
|
||||
|
||||
// //Drivetrain Variables
|
||||
// double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
|
||||
// double x = engine.gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
|
||||
// double rx = engine.gamepad1.left_stick_x;
|
||||
//
|
||||
// // Denominator is the largest motor power (absolute value) or 1
|
||||
// // This ensures all the powers maintain the same ratio, but only when
|
||||
// // at least one is out of the range [-1, 1]
|
||||
//
|
||||
// double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
// double frontLeftPower = (y + x + rx) / denominator;
|
||||
// double backLeftPower = (y - x + rx) / denominator;
|
||||
// double frontRightPower = (y - x - rx) / denominator;
|
||||
// double backRightPower = (y + x - rx) / denominator;
|
||||
//
|
||||
// robot.frontLeftDrive.setPower(-frontLeftPower * speed);
|
||||
// robot.backLeftDrive.setPower(backLeftPower * speed);
|
||||
// robot.frontRightDrive.setPower(-frontRightPower * speed);
|
||||
// robot.backRightDrive.setPower(backRightPower * speed);
|
||||
|
||||
|
||||
if (engine.gamepad2.y) {
|
||||
@@ -232,39 +228,61 @@ public class PrototypeTeleOPState extends CyberarmState {
|
||||
// }
|
||||
// }
|
||||
// SPENCER____________________________________________________________________
|
||||
// if (engine.gamepad1.start){
|
||||
//
|
||||
// switch (DriveFowardAndBack) {
|
||||
//
|
||||
// // Drive Foward
|
||||
// case 0:
|
||||
// if (robot.backLeftDrive.getCurrentPosition() == 0) {
|
||||
// robot.backLeftDrive.setTargetPosition(4000);
|
||||
// robot.backRightDrive.setTargetPosition(4000);
|
||||
// robot.frontLeftDrive.setTargetPosition(4000);
|
||||
// robot.backRightDrive.setTargetPosition(4000);
|
||||
// robot.backLeftDrive.setPower(1);
|
||||
// robot.backRightDrive.setPower(1);
|
||||
// robot.frontLeftDrive.setPower(1);
|
||||
// robot.backRightDrive.setPower(1);
|
||||
//
|
||||
// }
|
||||
// } else {
|
||||
//
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// // lower arm up
|
||||
// case 1:
|
||||
// if (robot.LowRiserLeft.getPosition() < 1) {
|
||||
// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
|
||||
// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
|
||||
// } else {
|
||||
// CyclingArmUpAndDown = CyclingArmUpAndDown +1;
|
||||
// }
|
||||
// break;
|
||||
if (engine.gamepad1.start) {
|
||||
RobotPosition = robot.backRightDrive.getCurrentPosition();
|
||||
|
||||
switch (DriveForwardAndBack) {
|
||||
|
||||
case 0:
|
||||
RobotStartingPosition = RobotPosition;
|
||||
drivePower = 1;
|
||||
|
||||
DriveForwardAndBack += 1;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (RobotPosition - RobotStartingPosition < 6250){
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
} else
|
||||
{
|
||||
DriveForwardAndBack += 1;
|
||||
drivePower = -1;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
if (RobotPosition - RobotStartingPosition >= 0) {
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
} else
|
||||
{
|
||||
DriveForwardAndBack += 1;
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
DriveForwardAndBack = 1;
|
||||
drivePower = 1;
|
||||
break;
|
||||
|
||||
|
||||
} // switch ending
|
||||
} // if gamepad 1 start ending
|
||||
else {
|
||||
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
|
||||
}
|
||||
|
||||
if (engine.gamepad2.start){
|
||||
|
||||
|
||||
Reference in New Issue
Block a user