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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Nathaniel Palme
2020-10-13 15:48:09 -05:00
parent 4e40aa8d7e 67b83d3ace
commit d9261dc32d
7 changed files with 251 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/build.gradle
View File

@@ -20,4 +20,5 @@ repositories {
dependencies {
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation project(path: ':TimeCraftersConfigurationTool')
}

12
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java
View File

@@ -22,7 +22,7 @@ public class EncoderTest extends CyberarmState {
robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y);
ticksLeft=robot.encoderLeft.getCurrentPosition();
ticksLeft= robot.encoderLeft.getCurrentPosition();
ticksRight=robot.encoderRight.getCurrentPosition();
@@ -50,16 +50,16 @@ public class EncoderTest extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("controler", engine.gamepad1.left_stick_y);
engine.telemetry.addData("controller", engine.gamepad1.left_stick_y);
engine.telemetry.addLine("Latency Values");
engine.telemetry.addData("Y", robot.getLocationY());
engine.telemetry.addData("X", robot.getLocationX());
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("Actual Values");
engine.telemetry.addData("Left", ticksLeft);
engine.telemetry.addData("Right", ticksRight);
engine.telemetry.addData("Left", robot.ticksToInches(ticksLeft));
engine.telemetry.addData("Right", robot.ticksToInches(ticksRight));
// engine.telemetry.addLine("");
// engine.telemetry.addData("Front", robot.encoderFront);

86
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java Normal file
View File

@@ -0,0 +1,86 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import android.util.Log;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TimeCraftersConfigurationTool.backend.Backend;
import org.timecrafters.UltimateGoal.Robot;
public class IMUDrive extends CyberarmState {
private Robot robot;
private String actionName;
private String groupName;
private double power;
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;
this.actionName = actionName;
this.groupName = groupName;
}
@Override
public void init() {
Log.i("Config", Backend.instance().gsonForConfig().toJson(robot.stateConfiguration.getConfig()));
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
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
public void exec() {
robot.updateLocation();
int ticksTraveled = Math.abs( robot.encoderRight.getCurrentPosition()-tickStart);
if (ticksTraveled > tickTarget) {
robot.setDrivePower(0,0);
sleep(finishDelay);
setHasFinished(true);
} else {
angleRelative = robot.getRelativeAngle(angleTarget, robot.getRotation());
double turnPowerCorrection = Math.pow(0.03 * angleRelative, 3) + 0.01 * angleRelative;
double rightPower = power + turnPowerCorrection;
double leftPower = power - turnPowerCorrection;
double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
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));
}
}

86
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java Normal file
View File

@@ -0,0 +1,86 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
public class IMUTurn extends CyberarmState {
private Robot robot;
private String actionName;
private String groupName;
private double power;
private float angleTarget;
private int turnDirection;
private float angleAllowance;
private boolean useOptimalDirection;
public IMUTurn(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.actionName = actionName;
this.groupName = groupName;
}
@Override
public void init() {
power = robot.stateConfiguration.variable(groupName,actionName, "power").value();
angleTarget = robot.stateConfiguration.variable(groupName,actionName, "angle").value();
angleAllowance = robot.stateConfiguration.variable(groupName,actionName, "allowance").value();
turnDirection = robot.stateConfiguration.variable(groupName, actionName, "direction").value();
useOptimalDirection = (turnDirection==0);
}
@Override
public void start() {
if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
setHasFinished(true);
}
}
@Override
public void exec() {
robot.updateLocation();
int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation());
if (!useOptimalDirection && OptimalDirection == turnDirection){
useOptimalDirection = true;
}
if (useOptimalDirection){
turnDirection = OptimalDirection;
}
robot.setDrivePower(power * turnDirection,-power * turnDirection);
if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) {
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());
}
}

27
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java Normal file
View File

@@ -0,0 +1,27 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Robot;
@Autonomous (name = "Localizer Test")
public class LocalizerTestingEngine extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
super.init();
}
@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"));
}
}

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

@@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.cyberarm.NeXT.StateConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration;
public class Robot {
@@ -15,7 +16,7 @@ public class Robot {
this.hardwareMap = hardwareMap;
}
public StateConfiguration stateConfiguration = new StateConfiguration();
public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
public BNO055IMU imu;
//drive system
@@ -25,14 +26,20 @@ public class Robot {
public DcMotor encoderBack;
public DcMotor encoderRight;
double BIAS_LEFT = 1.0;
double BIAS_RIGHT = 0.87;
double BIAS_LEFT = -1.0;
double BIAS_RIGHT = -0.87;
double Circumference_Encoder = Math.PI * 4;
int Counts_Per_Revolution = 8192;
//Robot Localizatoin
private double locationX;
private double locationY;
private float rotation;
public double traveledLeft;
public double traveledRight;
private int encoderFrontPrevious = 0;
private int encoderLeftPrevious = 0;
private int encoderBackPrevious = 0;
@@ -46,7 +53,12 @@ public class Robot {
// encoderBack = hardwareMap.dcMotor.get("encoderBack");
encoderRight = hardwareMap.dcMotor.get("encoderRight");
encoderLeft.setDirection(DcMotorSimple.Direction.REVERSE);
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);
encoderRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -68,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;
@@ -89,7 +106,7 @@ public class Robot {
}
public double getRotation() {
public float getRotation() {
return rotation;
}
@@ -100,4 +117,25 @@ public class Robot {
public double getLocationY() {
return locationY;
}
public double ticksToInches(double ticks) {
return ticks * (Circumference_Encoder / Counts_Per_Revolution);
}
public double inchesToTicks(double inches) {
return inches * (Counts_Per_Revolution / Circumference_Encoder);
}
public float getRelativeAngle(float reference, float current) {
float relative = current - reference;
if (relative < -180) {
relative += 360;
}
if (relative > 180) {
relative -= 360;
}
return relative;
}
}

2
TimeCraftersConfigurationTool/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/TimeCraftersConfiguration.java
View File

@@ -70,7 +70,7 @@ public class TimeCraftersConfiguration {
}
public Variable variable(String groupName, String actionName, String variableName) {
final Action action = action(groupName, groupName);
final Action action = action(groupName, actionName);
for (Variable variable : action.getVariables()) {
if (variable.name.trim().equals(variableName.trim())) {