TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2023-12-04 23:43:09 -06:00
parent 58f4308588 77f162a81f
commit 0b962a04e4
7 changed files with 141 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java
View File

@@ -18,7 +18,7 @@ public class RedCrabMinibot {
public static final int ClawArm_COLLECT = 2;
/// TUNING CONSTANTS ///
public static final double DRIVETRAIN_MAX_SPEED = 0.5;
public static final double DRIVETRAIN_MAX_SPEED = 1.0;
public static final double CLAW_ARM_MAX_SPEED = 0.5;
public static final double WINCH_MAX_SPEED = 0.5;
public static final double CLAW_ARM_STOW_ANGLE = 0.0;
@@ -38,8 +38,8 @@ public class RedCrabMinibot {
public static final double DRONE_LATCH_LAUNCH_POSITION = 0.5;
public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000;
public static final double HOOK_ARM_STOW_POSITION = 0.0;
public static final double HOOK_ARM_UP_POSITION = 0.5;
public static final double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8
public static final double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0
/// MOTOR CONSTANTS ///
public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4;
@@ -78,10 +78,10 @@ public class RedCrabMinibot {
backRight.resetEncoder();
/// --- MOTOR DIRECTIONS
frontLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
frontRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
backLeft.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
backRight.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
frontLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
frontRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
backLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
backRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
/// --- MOTOR BRAKING MODE
/// --- NOTE: Having BRAKE mode set for drivetrain helps with consistently of control

6
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java
View File

@@ -27,10 +27,10 @@ public class Pilot extends CyberarmState {
public void exec() {
drivetrain();
// clawArmAndWristController();
clawArmAndWristController();
// clawController();
// droneLatchController();
// hookArmController();
// hookArmController(); // disabled for swrist debug
// winchController();
}
@@ -149,7 +149,7 @@ public class Pilot extends CyberarmState {
}
robot.clawArm.set(RedCrabMinibot.CLAW_ARM_MAX_SPEED);
robot.clawArm.set(RedCrabMinibot. CLAW_ARM_MAX_SPEED);
}
private void clawController() {

4
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java
View File

@@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoFirstDriveState;
import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoTurnState;
import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoSecDriveState;
import dev.cyberarm.engine.V2.CyberarmEngine;
@@ -13,8 +14,9 @@ public class SodiPizzaAutoRedRightEngine extends CyberarmEngine {
@Override
public void setup() {
blackboardSet("readyToTurn", 0);
addState(new SodiPizzaAutoFirstDriveState());
addState(new SodiPizzaAutoTurnState());
addParallelStateToLastState(new SodiPizzaAutoFirstDriveState());
addState(new SodiPizzaAutoSecDriveState());
}
}

53
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java
View File

@@ -11,9 +11,8 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
final private SodiPizzaMinibotObject robot;
final private String groupName, actionName;
private long lastMoveTime;
private int targetPos = 2500;
private double drivePower;
public int readyToTurn;
private double drivePower, drivePowerRaw;
public int readyToTurn, neededTicks, currentTicks, targetTicks = 1500;
public SodiPizzaAutoFirstDriveState() {
groupName = " ";
@@ -21,6 +20,25 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
robot = new SodiPizzaMinibotObject();
robot.setup();
}
private double getDrivePower() {
if (Math.abs(neededTicks) > 1) {
drivePower = (drivePowerRaw * neededTicks) / 10;
}
return drivePower;
}
public void CalculateNeededTicks() {
if (targetTicks >= 0 && currentTicks >= 0) {
neededTicks = Math.abs(targetTicks - currentTicks);
} else if (targetTicks < 0 && currentTicks < 0) {
neededTicks = Math.abs(targetTicks - currentTicks);
} else if (targetTicks > 0 && currentTicks < 0) {
neededTicks = (targetTicks + Math.abs(currentTicks));
} else if (targetTicks < 0 && currentTicks > 0) {
neededTicks = (currentTicks + Math.abs(targetTicks));
}
}
@SuppressLint("SuspiciousIndentation")
@@ -34,10 +52,9 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
@Override
public void telemetry() {
engine.telemetry.addData("Target Ticks?", robot.leftFront.getTargetPosition());
engine.telemetry.addData("Current Ticks?", robot.leftFront.getCurrentPosition());
engine.telemetry.addData("Ticks Needed?", robot.leftFront.getTargetPosition() -
robot.leftFront.getCurrentPosition());
engine.telemetry.addData("Target Ticks?", targetTicks);
engine.telemetry.addData("Current Ticks?", currentTicks);
engine.telemetry.addData("Ticks Needed?", neededTicks);
engine.telemetry.addLine();
engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn);
engine.telemetry.addData("Distance Sensor Reading", robot.distSensor.getDistance(DistanceUnit.MM));
@@ -49,15 +66,20 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
readyToTurn = engine.blackboardGet("readyToTurn");
// Move forward from 0 to targetPos
currentTicks = robot.leftFront.getCurrentPosition();
CalculateNeededTicks();
// Move forward from 0 to targetTicks
if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && readyToTurn == 0) {
robot.leftFront.setTargetPosition(targetPos);
robot.leftBack.setTargetPosition(targetPos);
robot.rightFront.setTargetPosition(targetPos);
robot.rightBack.setTargetPosition(targetPos);
robot.leftFront.setTargetPosition(targetTicks);
robot.leftBack.setTargetPosition(targetTicks);
robot.rightFront.setTargetPosition(targetTicks);
robot.rightBack.setTargetPosition(targetTicks);
drivePower = 0.5;
drivePowerRaw = 0.5;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
@@ -66,9 +88,10 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
}
//Stop and finish set after return to 0
else if (robot.leftFront.getCurrentPosition() >= targetPos - 10 && robot.leftFront.getCurrentPosition() <= targetPos + 10) {
else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) {
drivePower = 0;
drivePowerRaw = 0;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);

72
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java
View File

@@ -1,5 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import dev.cyberarm.engine.V2.CyberarmState;
@@ -8,6 +10,9 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState {
final private SodiPizzaMinibotObject robot;
final private String groupName, actionName;
private long lastMoveTime;
private double drivePower, drivePowerRaw;
public int readyToTurn, neededTicks, currentTicks, targetTicks = 500;
public SodiPizzaAutoSecDriveState() {
groupName = " ";
@@ -15,14 +20,81 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState {
robot = new SodiPizzaMinibotObject();
robot.setup();
}
private double getDrivePower() {
if (Math.abs(neededTicks) > 1) {
drivePower = (drivePowerRaw * neededTicks) / 10;
}
return drivePower;
}
public void CalculateNeededTicks() {
if (targetTicks >= 0 && currentTicks >= 0) {
neededTicks = Math.abs(targetTicks - currentTicks);
} else if (targetTicks < 0 && currentTicks < 0) {
neededTicks = Math.abs(targetTicks - currentTicks);
} else if (targetTicks > 0 && currentTicks < 0) {
neededTicks = (targetTicks + Math.abs(currentTicks));
} else if (targetTicks < 0 && currentTicks > 0) {
neededTicks = (currentTicks + Math.abs(targetTicks));
}
}
@Override
public void start() {
robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
@Override
public void exec() {
readyToTurn = engine.blackboardGet("readyToTurn");
currentTicks = robot.leftFront.getCurrentPosition();
CalculateNeededTicks();
// Move forward from 0 to targetPos
if (robot.leftFront.getCurrentPosition() <= 10 && readyToTurn == 2) {
robot.leftFront.setTargetPosition(targetTicks);
robot.leftBack.setTargetPosition(targetTicks);
robot.rightFront.setTargetPosition(targetTicks);
robot.rightBack.setTargetPosition(targetTicks);
drivePowerRaw = 0.5;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
}
//Stop and finish set after return to 0
else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) {
drivePowerRaw = 0;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
engine.blackboardSet("readyToTurn", 3);
setHasFinished(true);
}
}
}

22
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java
View File

@@ -1,5 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import android.annotation.SuppressLint;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
@@ -7,7 +9,7 @@ import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import dev.cyberarm.engine.V2.CyberarmState;
public class SodiPizzaAutoTurnState extends CyberarmState {
final private SodiPizzaMinibotObject robot = new SodiPizzaMinibotObject();
final private SodiPizzaMinibotObject robot;
final private String groupName, actionName;
private long lastMoveTime;
private double turnSpeedRaw, turnSpeed;
@@ -27,11 +29,12 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
public SodiPizzaAutoTurnState() {
groupName = " ";
actionName = " ";
robot = new SodiPizzaMinibotObject();
robot.setup();
}
private double getTurnSpeed() {
if (Math.abs(neededRot) > 5) {
if (Math.abs(neededRot) > 1) {
turnSpeed = (turnSpeedRaw * neededRot) / 10;
}
return turnSpeed;
@@ -77,6 +80,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
}
@SuppressLint("SuspiciousIndentation")
@Override
public void exec() {
@@ -90,14 +94,20 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
if (readyToTurn == 0) {
targetRot = 0;
if (currentRot < targetRot - 1) {
if (currentRot <= targetRot - 1) {
turnSpeedRaw = 0.5;
getTurnSpeed();
robot.rightFront.setPower(robot.leftFront.getPower() + turnSpeed);
robot.rightBack.setPower(robot.leftBack.getPower() + turnSpeed);
}
} else if (currentRot >= targetRot + 1)
turnSpeedRaw = 0.5;
getTurnSpeed();
robot.leftFront.setPower(robot.rightFront.getPower() + turnSpeed);
}
@@ -128,7 +138,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightFront.setPower(-turnSpeed);
robot.rightBack.setPower(-turnSpeed);
} else if (readyToTurn == 1 && Math.abs(neededRot) < 3) {
} else if (readyToTurn == 1 && Math.abs(neededRot) < 2) {
turnSpeedRaw = 0;
getTurnSpeed();
@@ -138,7 +148,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
engine.blackboardSet("readyToTurn", 0);
engine.blackboardSet("readyToTurn", 2);
}
}