TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

Disabled all of my old burnsville autos so they don't show. I also implemented a max turning power.

Browse Source
This commit is contained in:
SpencerPiha
2024-02-11 18:23:52 -06:00
parent da958f567e
commit b57b4c54f1
7 changed files with 36 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -15,7 +16,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville audience blue", preselectTeleOp = "Competition V1 TeleOp")
@Disabled
public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine {
CompetitionRobotV1 robot;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville audience red", preselectTeleOp = "Competition V1 TeleOp")
@Disabled
public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine {

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -15,6 +16,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville BackDrop blue", preselectTeleOp = "Competition V1 TeleOp")
@Disabled
public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
@@ -14,6 +15,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
@Disabled
public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {

14
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
View File

@@ -20,6 +20,7 @@ public class DriveToCoordinatesState extends CyberarmState {
public boolean posSpecific;
public double maxXPower;
public double maxYPower;
public double maxHPower;
private String actionName;
public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
@@ -31,6 +32,7 @@ public class DriveToCoordinatesState extends CyberarmState {
this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value();
this.maxXPower = robot.configuration.variable(groupName, actionName, "maxXPower").value();
this.maxYPower = robot.configuration.variable(groupName, actionName, "maxYPower").value();
this.maxHPower = robot.configuration.variable(groupName, actionName, "maxHPower").value();
this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value();
this.objectPos = robot.configuration.variable(groupName, actionName, "objectPos").value();
this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value();
@@ -45,22 +47,15 @@ public class DriveToCoordinatesState extends CyberarmState {
robot.xTarget = xTarget;
robot.yMaxPower = maxYPower;
robot.xMaxPower = maxXPower;
robot.hMaxPower = maxHPower;
} else {
setHasFinished(true);
}
Log.d("TTT?", "" + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
Log.d("TTT?", "" + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget);
}
@Override
public void exec() {
// robot.yMaxPower = maxYPower;
// robot.xMaxPower = maxXPower;
// robot.hTarget = hTarget;
// robot.yTarget = yTarget;
// robot.xTarget = xTarget;
if (posSpecific) {
if (objectPos != robot.objectPos) {
@@ -73,7 +68,8 @@ public class DriveToCoordinatesState extends CyberarmState {
}
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) > 2) {
setHasFinished(true);
}
}

1
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java
View File

@@ -18,6 +18,7 @@ public class DriveToCoordinatesTask extends CyberarmState {
public void exec() {
robot.XDrivePowerModifier();
robot.YDrivePowerModifier();
robot.HDrivePowerModifier();
robot.DriveToCoordinates();
}
}

36
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
View File

@@ -103,11 +103,13 @@ public class CompetitionRobotV1 extends Robot {
public double yMaxPower = 1;
public double xMaxPower = 1;
public double hMaxPower = 1;
public double pidX;
public double pidY;
public double pidH;
public double rawPidX;
public double rawPidY;
public double rawPidH;
public double xVelocity;
public double yVelocity;
public double deltaTime = 0;
@@ -284,7 +286,7 @@ public class CompetitionRobotV1 extends Robot {
* A Controller taking error of the heading position and converting to a power in the direction of a target.
* @param reference reference is the target position
* @param current current is the measured sensor value.
* @return A power to the target the position
* @return A power to the target position
*
*/
public double HeadingPIDControl(double reference, double current){
@@ -349,15 +351,27 @@ public class CompetitionRobotV1 extends Robot {
pidX = rawPidX;
}
}
public void HDrivePowerModifier () {
rawPidH = HeadingPIDControl(Math.toRadians(hTarget), imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
if (Math.abs(rawPidH) > hMaxPower) {
if (rawPidH < 0) {
pidH = -hMaxPower;
} else {
pidH = hMaxPower;
}
} else {
pidH = rawPidH;
}
}
public void DriveToCoordinates () {
// determine the powers needed for each direction
// this uses PID to adjust needed Power for robot to move to target
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(pidH), 1);
// field oriented math, (rotating the global field to the relative field)
double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading);
@@ -365,10 +379,10 @@ public class CompetitionRobotV1 extends Robot {
// finding approximate power for each wheel.
frontLeftPower = (rotY + rotX + rx) / denominator;
backLeftPower = (rotY - rotX + rx) / denominator;
frontRightPower = (rotY - rotX - rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator;
frontLeftPower = (rotY + rotX + pidH) / denominator;
backLeftPower = (rotY - rotX + pidH) / denominator;
frontRightPower = (rotY - rotX - pidH) / denominator;
backRightPower = (rotY + rotX - pidH) / denominator;
// apply my powers
frontLeft.setPower(frontLeftPower);
@@ -412,12 +426,6 @@ public class CompetitionRobotV1 extends Robot {
}
// pidController.setPID(p, i, d);
// int armPos = clawArm.getCurrentPosition();
// double pid = pidController.calculate(armPos, target);
//
// power = pid;
clawArm.setTargetPosition(target);
clawArm.setPower(0.4);