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

Copied Drive State for autonomous and modified the copy so it runs off of the Odometer Encoder to get distance driven instead of motor encoder

Browse Source
This commit is contained in:
SpencerPiha
2022-12-10 22:59:30 -06:00
parent 798849ae78
commit 020bec7f87
2 changed files with 122 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

114
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java Normal file
View File

@@ -0,0 +1,114 @@
package org.timecrafters.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TeleOp.states.PhoenixBot1;
public class DriverStateWithOdometer extends CyberarmState {
private final boolean stateDisabled;
PhoenixBot1 robot;
private int RampUpDistance;
private int RampDownDistance;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower, targetDrivePower;
private int traveledDistance;
@Override
public void start() {
robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
@Override
public void exec() {
if (stateDisabled) {
setHasFinished(true);
return;
}
double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
double delta = traveledDistance - Math.abs(CurrentPosition);
if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up
drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25;
}
else if (Math.abs(CurrentPosition) >= delta){
// ramping down
drivePower = ((delta / RampDownDistance) + 0.25);
} else {
// middle ground
drivePower = targetDrivePower;
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
drivePower = drivePower * -1;
}
if (Math.abs(CurrentPosition) < traveledDistance){
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
} else {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
}
@Override
public void telemetry() {
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
}
}

9
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
View File

@@ -42,7 +42,7 @@ public class PhoenixBot1 {
public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder;
public CRServo collectorLeft, collectorRight;
@@ -125,12 +125,19 @@ public class PhoenixBot1 {
backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// Dead Wheel encoder for driving
OdometerEncoder.setDirection(DcMotorSimple.Direction.FORWARD);
OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
HighRiserLeft.setDirection(Servo.Direction.REVERSE);
HighRiserRight.setDirection(Servo.Direction.FORWARD);
LowRiserLeft.setDirection(Servo.Direction.FORWARD);
LowRiserRight.setDirection(Servo.Direction.REVERSE);
CameraServo.setDirection(Servo.Direction.FORWARD);
LowRiserLeft.setPosition(0.35);