TimeCrafters/UltimateGoal
1
0
Fork 0
mirror of https://github.com/TimeCrafters/UltimateGoal.git synced 2025-12-13 05:02:34 +00:00
Code Issues Projects Releases Wiki Activity

Fine tuned the diamiter value for unit conversions. Added ability to disable IMUDrive and IMUTurn with the configuration tool.

Browse Source
This commit is contained in:
RobotNRP
2020-10-10 11:44:18 -05:00
parent 3556ecf9b8
commit 67b83d3ace
4 changed files with 81 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java
View File

@@ -15,6 +15,8 @@ public class IMUDrive extends CyberarmState {
private int tickTarget;
private float angleRelative;
private float angleTarget;
private int tickStart;
private long finishDelay;
public IMUDrive(Robot robot, String groupName, String actionName) {
this.robot = robot;
@@ -30,11 +32,17 @@ public class IMUDrive extends CyberarmState {
double inchesTarget = robot.stateConfiguration.variable(groupName, actionName, "inches").value();
tickTarget = (int) robot.inchesToTicks(inchesTarget);
angleTarget = robot.stateConfiguration.variable(groupName, actionName, "angle").value();
finishDelay = robot.stateConfiguration.variable(groupName,actionName,"delay").value();
}
@Override
public void start() {
if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
setHasFinished(true);
}
angleTarget=robot.getRotation();
tickStart = robot.encoderRight.getCurrentPosition();
}
@Override
@@ -42,9 +50,10 @@ public class IMUDrive extends CyberarmState {
robot.updateLocation();
if (Math.abs(robot.encoderRight.getCurrentPosition()) > tickTarget) {
robot.encoderRight.setPower(0);
robot.encoderLeft.setPower(0);
int ticksTraveled = Math.abs( robot.encoderRight.getCurrentPosition()-tickStart);
if (ticksTraveled > tickTarget) {
robot.setDrivePower(0,0);
sleep(finishDelay);
setHasFinished(true);
} else {
@@ -57,8 +66,21 @@ public class IMUDrive extends CyberarmState {
double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
robot.encoderRight.setPower(rightPower * powerAdjust);
robot.encoderLeft.setPower(leftPower * powerAdjust);
robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower);
}
}
@Override
public void telemetry() {
engine.telemetry.addLine("Measured Values");
engine.telemetry.addData("Y", robot.ticksToInches(robot.getLocationY()));
engine.telemetry.addData("X", robot.ticksToInches(robot.getLocationX()));
engine.telemetry.addLine();
engine.telemetry.addData("Rotation", robot.getRotation());
engine.telemetry.addLine();
engine.telemetry.addLine("Total Travel");
engine.telemetry.addData("Left", robot.ticksToInches(robot.traveledLeft));
engine.telemetry.addData("Right", robot.ticksToInches(robot.traveledRight));
}
}

52
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java
View File

@@ -9,7 +9,7 @@ public class IMUTurn extends CyberarmState {
private String actionName;
private String groupName;
private double power;
private double angleTarget;
private float angleTarget;
private int turnDirection;
private float angleAllowance;
private boolean useOptimalDirection;
@@ -29,21 +29,18 @@ public class IMUTurn extends CyberarmState {
useOptimalDirection = (turnDirection==0);
}
@Override
public void start() {
if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
setHasFinished(true);
}
}
@Override
public void exec() {
robot.updateLocation();
int OptimalDirection;
if (angleTarget > 180 + robot.getRotation()){
OptimalDirection = -1;
} else if (angleTarget < -180 + robot.getRotation()){
OptimalDirection = 1;
} else if (angleTarget > robot.getRotation()){
OptimalDirection = 1;
}else {
OptimalDirection = -1;
}
int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation());
if (!useOptimalDirection && OptimalDirection == turnDirection){
useOptimalDirection = true;
@@ -53,14 +50,37 @@ public class IMUTurn extends CyberarmState {
turnDirection = OptimalDirection;
}
robot.encoderLeft.setPower(-power * turnDirection);
robot.encoderRight.setPower(power * turnDirection);
robot.setDrivePower(power * turnDirection,-power * turnDirection);
if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) {
robot.encoderRight.setPower(0);
robot.encoderLeft.setPower(0);
robot.setDrivePower(0,0);
setHasFinished(true);
}
}
private int getOptimalDirection(float angleTarget, float angleCurrent){
if (angleCurrent < 0) {
angleCurrent += 360;
}
if (angleTarget < 0) {
angleTarget += 360;
}
float degreeDifferance = angleTarget - angleCurrent;
if (degreeDifferance > 180 || (degreeDifferance < 0 && degreeDifferance > -180)) {
return -1;
} else {
return 1;
}
}
@Override
public void telemetry() {
engine.telemetry.addData("rotation", robot.getRotation());
}
}

3
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java
View File

@@ -20,5 +20,8 @@ public class LocalizerTestingEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new IMUDrive(robot,"group", "010_drive"));
addState(new IMUTurn(robot,"group", "020_turn"));
addState(new IMUDrive(robot,"group", "030_drive"));
addState(new IMUTurn(robot,"group", "040_turn"));
}
}

17
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java
View File

@@ -29,7 +29,7 @@ public class Robot {
double BIAS_LEFT = -1.0;
double BIAS_RIGHT = -0.87;
double Circumference_Encoder = Math.PI * 3.8;
double Circumference_Encoder = Math.PI * 4;
int Counts_Per_Revolution = 8192;
//Robot Localizatoin
@@ -37,6 +37,9 @@ public class Robot {
private double locationY;
private float rotation;
public double traveledLeft;
public double traveledRight;
private int encoderFrontPrevious = 0;
private int encoderLeftPrevious = 0;
private int encoderBackPrevious = 0;
@@ -50,6 +53,11 @@ public class Robot {
// encoderBack = hardwareMap.dcMotor.get("encoderBack");
encoderRight = hardwareMap.dcMotor.get("encoderRight");
encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
encoderRight.setDirection(DcMotorSimple.Direction.REVERSE);
encoderLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -72,13 +80,18 @@ public class Robot {
}
public void updateLocation(){
rotation = imu.getAngularOrientation().firstAngle;
// IMU orientation is inverted to have clockwise be positive.
rotation = -imu.getAngularOrientation().firstAngle;
float rotationChange = rotation - rotationPrevious;
int encoderLeftCurrent = encoderLeft.getCurrentPosition();
int encoderRightCurrent = encoderRight.getCurrentPosition();
double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
double encoderRightChange = encoderRightCurrent - encoderRightPrevious;
traveledLeft += Math.abs(encoderLeftChange);
traveledRight += Math.abs(encoderRightChange);
encoderLeftPrevious = encoderLeftCurrent;
encoderRightPrevious = encoderRightCurrent;
rotationPrevious = rotation;