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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Sodi
2023-01-19 20:36:18 -06:00
parent 505aeb850a 6ae272003e
commit 30e2e0d50a
44 changed files with 1307 additions and 523 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.idea/jarRepositories.xml generated
View File

@@ -26,5 +26,10 @@
<option name="name" value="Google" />
<option name="url" value="https://dl.google.com/dl/android/maven2/" />
</remote-repository>
<remote-repository>
<option name="id" value="maven" />
<option name="name" value="maven" />
<option name="url" value="https://maven.brott.dev/" />
</remote-repository>
</component>
</project>

6
TeamCode/build.gradle
View File

@@ -20,8 +20,14 @@ android {
buildFeatures {
mlModelBinding true
}
androidResources {
noCompress 'tflite'
}
packagingOptions {
jniLibs {
pickFirsts += ['**/*.so']
}
jniLibs.useLegacyPackaging true
}
}

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
@@ -16,6 +17,7 @@ import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "Left Side")
@Disabled
public class LeftSideAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;

186
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java Normal file
View File

@@ -0,0 +1,186 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "left 2 cone auto")
public class LeftTwoConeAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
public void setup() {
robot = new PhoenixBot1(this);
// 1 Rotate camera down at the signal
addState(new ServoCameraRotate(robot, "LeftTwoCone", "01-0"));
// 2 Scan custom image
addState(new ConeIdentification(robot, "LeftTwoCone", "02-0"));
// 3 Rotate Camera up, out of the way so it doesn't crash into stuff
addState(new ServoCameraRotate(robot, "LeftTwoCone", "03-0"));
// 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "04-0"));
// addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "04-1"));
addState(new TopArm(robot, "LeftTwoCone", "04-1"));
// 6 Raise lower arm while slowly driving at the junction (parallel state)
addState(new BottomArm(robot, "LeftTwoCone", "05-0"));
// 6 Turn Towards and look for junction with sensor
addState(new RotationState(robot, "LeftTwoCone", "06-0"));
// 6-1 drive forward or backwards if needed this is customizable so we can adapt
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "06-1"));
// 6-3 align to junction with rotation or skip if it looks like it won't be able to
addState(new JunctionAllignmentDistanceState(robot, "LeftTwoCone", "06-3"));
addState(new JunctionAllignmentAngleState(robot, "LeftTwoCone", "06-4"));
//pause
addState(new BottomArm(robot, "LeftTwoCone", "06-2"));
// 7 Drop bottom arm down on the junction to place cone
addState(new BottomArm(robot, "LeftTwoCone", "07-0"));
addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "07-1"));
// 7-1 drive back slightly
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "07-2"));
// 8 Drop cone as soon as arm is in position
addState(new CollectorState(robot, "LeftTwoCone", "08-0"));
// 8-1 drive back to lose contact
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "08-1"));
// 8-1 realign to old angle
addState(new RotationState(robot, "LeftTwoCone", "08-2"));
// 9 Raise bottom arm to clear junction
addParallelStateToLastState(new BottomArm(robot, "LeftTwoCone", "09-0"));
addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "09-1"));
// // 10 Back up and bring lower arm down (parallel state)
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "10-0"));
addParallelStateToLastState(new BottomArm(robot, "LeftTwoCone", "10-1"));
// 11 Rotate towards stack
addState(new RotationState(robot, "LeftTwoCone", "11-0"));
//
// 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
addState(new TopArm(robot, "LeftTwoCone", "12-0"));
// drive forward at the stack without sensor
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "12-1"));
// turn and align at stack
addState(new RotationState(robot, "LeftTwoCone", "12-2"));
//
// 13 Drive at stack while collecting and check to see when we grab it
addState(new CollectorDistanceState(robot, "LeftTwoCone", "13-0"));
//
// // 14 Back up and raise arm
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "14-0"));
addState(new TopArm(robot, "LeftTwoCone", "14-1"));
// 14-2 align perpendicular too the wall
addState(new RotationState(robot, "LeftTwoCone", "14-2"));
//
// 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "15-0"));
//
// 16 Rotate and use sensor to find junction
addState(new RotationState(robot, "LeftTwoCone", "16-0"));
//
// 17 Drive Towards Junction (This is optional, idk if this is needed atm)
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "17-0"));
// // 18 Bring upper arm down
addState(new TopArm(robot, "LeftTwoCone", "18-0"));
//
// // 19 Drop cone
addState(new CollectorState(robot, "LeftTwoCone", "19-0"));
addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "19-1"));
//
// // 20 Bring upper arm up
// addState(new TopArm(robot, "LeftTwoCone", "20-0"));
//
// // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "21-0"));
//
// // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
// addState(new TopArm(robot, "LeftTwoCone", "22-0"));
//
// // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack
// addState(new CollectorDistanceState(robot, "LeftTwoCone", "23-0"));
//
// // 24 Drive Back and lift up all the way to position for the low junction
// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "24-0"));
// addState(new TopArm(robot, "LeftTwoCone", "24-1"));
//
// // 25 Drive back faster after the cone is fully off of the stack
// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "25-0"));
//
// // 26 Turn and look for the low junction with the distance sensor and align
// addState(new RotationState(robot, "LeftTwoCone", "26-0"));
//
// // 27 Drive forward / backwards if you need to. (check with distance sensor)
// addState(new JunctionAllignmentState(robot, "LeftTwoCone", "26-1"));
//
// // 28 Bring Upper arm down on junction
// addState(new TopArm(robot, "LeftTwoCone", "28-0"));
//
// // 29 Let go of cone right after arm is in position
// addState(new CollectorState(robot, "LeftTwoCone", "29-0"));
//
// // 30 Raise arm as soon as the cone is dropped
// addState(new TopArm(robot, "LeftTwoCone", "30-0"));
//
// // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction
// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "31-0"));
//
// // 32 Rotate towards Stack of cones
addState(new RotationState(robot, "LeftTwoCone", "32-0"));
//
// // 33 Decide which path after scanning image from earlier
addState(new PathDecision(robot, "LeftTwoCone", "33-0"));
//
// // 34 Drive backwards, forwards, or stay put
addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-1"));
addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-2"));
addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-3"));
//
// // 35 Rotate towards alliance terminal
addState(new RotationState(robot, "LeftTwoCone", "35-0"));
addState(new TopArm(robot, "RightTwoCone", "36-0"));
}
@Override
public void loop() {
super.loop();
telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
}
}

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
@@ -15,6 +16,7 @@ import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "Right Side")
@Disabled
public class RightSideAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;

119
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java
View File

@@ -7,6 +7,7 @@ import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
@@ -16,9 +17,9 @@ import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "4 cone auto right")
@Autonomous (name = "Right 2 cone auto")
public class RightFourConeAutonomousEngine extends CyberarmEngine {
public class RightTwoConeAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
@@ -26,147 +27,153 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
robot = new PhoenixBot1(this);
// 1 Rotate camera down at the signal
addState(new ServoCameraRotate(robot, "RightFourCone", "01-0"));
addState(new ServoCameraRotate(robot, "RightTwoCone", "01-0"));
// 2 Scan custom image
addState(new ConeIdentification(robot, "RightFourCone", "02-0"));
addState(new ConeIdentification(robot, "RightTwoCone", "02-0"));
// 3 Rotate Camera up, out of the way so it doesn't crash into stuff
addState(new ServoCameraRotate(robot, "RightFourCone", "03-0"));
addState(new ServoCameraRotate(robot, "RightTwoCone", "03-0"));
// 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
addState(new DriverStateWithOdometer(robot, "RightFourCone", "04-0"));
// addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
addState(new TopArm(robot, "RightFourCone", "04-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "04-0"));
// addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "04-1"));
addState(new TopArm(robot, "RightTwoCone", "04-1"));
// 6 Raise lower arm while slowly driving at the junction (parallel state)
addState(new BottomArm(robot, "RightFourCone", "05-0"));
addState(new BottomArm(robot, "RightTwoCone", "05-0"));
// 6 Turn Towards and look for junction with sensor
addState(new RotationState(robot, "RightFourCone", "06-0"));
addState(new RotationState(robot, "RightTwoCone", "06-0"));
// 6-1 drive forward or backwards if needed this is customizable so we can adapt
addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "06-1"));
// 6-3 align to junction with rotation or skip if it looks like it won't be able to
addState(new JunctionAllignmentDistanceState(robot, "RightFourCone", "06-3"));
addState(new JunctionAllignmentAngleState(robot, "RightFourCone", "06-4"));
addState(new JunctionAllignmentDistanceState(robot, "RightTwoCone", "06-3"));
addState(new JunctionAllignmentAngleState(robot, "RightTwoCone", "06-4"));
//pause
addState(new BottomArm(robot, "RightFourCone", "06-2"));
addState(new BottomArm(robot, "RightTwoCone", "06-2"));
// 7 Drop bottom arm down on the junction to place cone
addState(new BottomArm(robot, "RightFourCone", "07-0"));
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "07-1"));
addState(new BottomArm(robot, "RightTwoCone", "07-0"));
addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "07-1"));
// 7-1 drive back slightly
addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-2"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "07-2"));
// 8 Drop cone as soon as arm is in position
addState(new CollectorState(robot, "RightFourCone", "08-0"));
addState(new CollectorState(robot, "RightTwoCone", "08-0"));
// 8-1 drive back to lose contact
addState(new DriverStateWithOdometer(robot, "RightFourCone", "08-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "08-1"));
// 8-1 realign to old angle
addState(new RotationState(robot, "RightFourCone", "08-2"));
addState(new RotationState(robot, "RightTwoCone", "08-2"));
// 9 Raise bottom arm to clear junction
addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "09-0"));
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "09-1"));
addParallelStateToLastState(new BottomArm(robot, "RightTwoCone", "09-0"));
addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "09-1"));
// // 10 Back up and bring lower arm down (parallel state)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "10-0"));
addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "10-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "10-0"));
addParallelStateToLastState(new BottomArm(robot, "RightTwoCone", "10-1"));
// 11 Rotate towards stack
addState(new RotationState(robot, "RightFourCone", "11-0"));
addState(new RotationState(robot, "RightTwoCone", "11-0"));
//
// 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
addState(new TopArm(robot, "RightFourCone", "12-0"));
addState(new TopArm(robot, "RightTwoCone", "12-0"));
// drive forward at the stack without sensor
addState(new DriverStateWithOdometer(robot, "RightFourCone", "12-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "12-1"));
// turn and align at stack
addState(new RotationState(robot, "RightFourCone", "12-2"));
addState(new RotationState(robot, "RightTwoCone", "12-2"));
//
// 13 Drive at stack while collecting and check to see when we grab it
addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
addState(new CollectorDistanceState(robot, "RightTwoCone", "13-0"));
//
// // 14 Back up and raise arm
addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
addState(new TopArm(robot, "RightFourCone", "14-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "14-0"));
addState(new TopArm(robot, "RightTwoCone", "14-1"));
// 14-2 align perpendicular too the wall
addState(new RotationState(robot, "RightFourCone", "14-2"));
addState(new RotationState(robot, "RightTwoCone", "14-2"));
//
// 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "15-0"));
//
// 16 Rotate and use sensor to find junction
addState(new RotationState(robot, "RightFourCone", "16-0"));
addState(new RotationState(robot, "RightTwoCone", "16-0"));
//
// 17 Drive Towards Junction (This is optional, idk if this is needed atm)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "17-0"));
// // 18 Bring upper arm down
addState(new TopArm(robot, "RightFourCone", "18-0"));
addState(new TopArm(robot, "RightTwoCone", "18-0"));
//
// // 19 Drop cone
addState(new CollectorState(robot, "RightFourCone", "19-0"));
addState(new CollectorState(robot, "RightTwoCone", "19-0"));
addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "19-1"));
//
// // 20 Bring upper arm up
// addState(new TopArm(robot, "RightFourCone", "20-0"));
// addState(new TopArm(robot, "RightTwoCone", "20-0"));
//
// // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "21-0"));
//
// // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
// addState(new TopArm(robot, "RightFourCone", "22-0"));
// addState(new TopArm(robot, "RightTwoCone", "22-0"));
//
// // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack
// addState(new CollectorDistanceState(robot, "RightFourCone", "23-0"));
// addState(new CollectorDistanceState(robot, "RightTwoCone", "23-0"));
//
// // 24 Drive Back and lift up all the way to position for the low junction
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "24-0"));
// addState(new TopArm(robot, "RightFourCone", "24-1"));
// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "24-0"));
// addState(new TopArm(robot, "RightTwoCone", "24-1"));
//
// // 25 Drive back faster after the cone is fully off of the stack
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "25-0"));
// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "25-0"));
//
// // 26 Turn and look for the low junction with the distance sensor and align
// addState(new RotationState(robot, "RightFourCone", "26-0"));
// addState(new RotationState(robot, "RightTwoCone", "26-0"));
//
// // 27 Drive forward / backwards if you need to. (check with distance sensor)
// addState(new JunctionAllignmentState(robot, "RightFourCone", "26-1"));
// addState(new JunctionAllignmentState(robot, "RightTwoCone", "26-1"));
//
// // 28 Bring Upper arm down on junction
// addState(new TopArm(robot, "RightFourCone", "28-0"));
// addState(new TopArm(robot, "RightTwoCone", "28-0"));
//
// // 29 Let go of cone right after arm is in position
// addState(new CollectorState(robot, "RightFourCone", "29-0"));
// addState(new CollectorState(robot, "RightTwoCone", "29-0"));
//
// // 30 Raise arm as soon as the cone is dropped
// addState(new TopArm(robot, "RightFourCone", "30-0"));
// addState(new TopArm(robot, "RightTwoCone", "30-0"));
//
// // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "31-0"));
//
// // 32 Rotate towards Stack of cones
addState(new RotationState(robot, "RightFourCone", "32-0"));
addState(new RotationState(robot, "RightTwoCone", "32-0"));
//
// // 33 Decide which path after scanning image from earlier
addState(new PathDecision(robot, "RightFourCone", "33-0"));
addState(new PathDecision(robot, "RightTwoCone", "33-0"));
//
// // 34 Drive backwards, forwards, or stay put
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-1"));
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-2"));
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-3"));
addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-1"));
addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-2"));
addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-3"));
//
// // 35 Rotate towards alliance terminal
// addState(new RotationState(robot, "RightFourCone", "35-0"));
addState(new RotationState(robot, "RightTwoCone", "35-0"));
addState(new TopArm(robot, "RightTwoCone", "36-0"));
}

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java
View File

@@ -89,9 +89,9 @@ public class ConeIdentification extends CyberarmState {
if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) {
bestConfidence = recognition.getConfidence();
if (recognition.getLabel().equals("2 Bulb")) {
if (recognition.getLabel().equals("#2")) {
engine.blackboardSet("parkPlace", "2");
} else if (recognition.getLabel().equals("3 Panel")) {
} else if (recognition.getLabel().equals("#3")) {
engine.blackboardSet("parkPlace", "3");
} else {

11
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java
View File

@@ -49,12 +49,12 @@ public class DriverParkPlaceState extends CyberarmState {
setHasFinished(true);
}
if (placement.equals(intendedPlacement)) {
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
double delta = traveledDistance - Math.abs(robot.OdometerEncoder.getCurrentPosition());
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) <= RampUpDistance) {
// ramping up
drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
} else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
drivePower = (Math.abs((float) robot.OdometerEncoder.getCurrentPosition()) / RampUpDistance) + 0.25;
} else if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) >= delta) {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.25);
} else {
@@ -71,7 +71,7 @@ public class DriverParkPlaceState extends CyberarmState {
drivePower = drivePower * -1;
}
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) {
if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
@@ -96,6 +96,7 @@ public class DriverParkPlaceState extends CyberarmState {
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoder.getCurrentPosition());
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);

10
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
View File

@@ -15,6 +15,7 @@ public class DriverStateWithOdometer extends CyberarmState {
private int maximumTolerance;
private float direction;
private boolean targetAchieved = false;
private double CurrentPosition;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -36,12 +37,14 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderLeft.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);
robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -55,7 +58,12 @@ public class DriverStateWithOdometer extends CyberarmState {
return;
}
double CurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition());
double RightCurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition());
double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition());
if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up

8
TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
View File

@@ -35,12 +35,12 @@ public class RotationState extends CyberarmState {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
drivePowerVariable = 0.3 * drivePower;
if (Math.abs(drivePowerVariable) < 0.3) {
drivePowerVariable = 0.4 * drivePower;
if (Math.abs(drivePowerVariable) < 0.4) {
if (drivePowerVariable < 0){
drivePowerVariable = -0.3;
drivePowerVariable = -0.4;
} else {
drivePowerVariable = 0.3;
drivePowerVariable = 0.4;
}
}
debugStatus = "Rotate Slow";

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserState.java
View File

@@ -7,7 +7,7 @@ import com.qualcomm.robotcore.hardware.ColorSensor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
import org.timecrafters.minibots.states.MecanumRobot;
public class LaserState extends CyberarmState {
Rev2mDistanceSensor laser;

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

@@ -44,7 +44,7 @@ public class PhoenixBot1 {
public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo;
private final CyberarmEngine engine;
public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance;
public Rev2mDistanceSensor collectorDistance, /*downSensor,*/ leftPoleDistance, rightPoleDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder, OdometerEncoderLeft;
@@ -104,7 +104,7 @@ public class PhoenixBot1 {
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
// downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance");
@@ -195,8 +195,8 @@ public class PhoenixBot1 {
LowRiserLeft.setPosition(0.35);
LowRiserRight.setPosition(0.35);
HighRiserLeft.setPosition(0.45);
HighRiserRight.setPosition(0.45);
HighRiserLeft.setPosition(0.40);
HighRiserRight.setPosition(0.40);
CameraServo.setPosition(0.775);

22
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java
View File

@@ -58,7 +58,7 @@ public class PhoenixTeleOPState extends CyberarmState {
engine.telemetry.addData("Drive Power", drivePower);
engine.telemetry.addData("Delta Rotation", DeltaRotation);
engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM));
// engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Left Pole Distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Odometer Encoder, Right", robot.OdometerEncoder.getCurrentPosition());
@@ -483,15 +483,15 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.imu.initialize(parameters);
}
}
public double downSensor() {
double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5;
Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
return Distance;
// public double downSensor() {
// double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5;
// Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
// return Distance;
}
}

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/Engine.java → TeamCode/src/main/java/org/timecrafters/minibots/Engine.java
View File

@@ -1,9 +1,9 @@
package org.timecrafters.minibots.cyberarm;
package org.timecrafters.minibots;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.cyberarm.engines.Common;
import org.timecrafters.minibots.engines.Common;
@TeleOp (name = "light test")

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/MecanumMinibot.java → TeamCode/src/main/java/org/timecrafters/minibots/MecanumMinibot.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.minibots.cyberarm;
package org.timecrafters.minibots;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/PortsCheckGeneric.java → TeamCode/src/main/java/org/timecrafters/minibots/PortsCheckGeneric.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.minibots.cyberarm;
package org.timecrafters.minibots;
import org.cyberarm.engine.V2.CyberarmEngine;

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/State.java → TeamCode/src/main/java/org/timecrafters/minibots/State.java
View File

@@ -1,9 +1,9 @@
package org.timecrafters.minibots.cyberarm;
package org.timecrafters.minibots;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.cyberarm.engines.Common;
import org.timecrafters.minibots.engines.Common;
public class State extends CyberarmState {

266
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java Normal file
View File

@@ -0,0 +1,266 @@
package org.timecrafters.minibots.cyberarm.chiron;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
public class Robot {
public final DcMotorEx leftDrive, rightDrive, frontDrive, backDrive, liftDrive;
public final ServoImplEx gripper, wrist;
public final IMU imu;
public final ColorSensor indicatorA, indicatorB;
public final double imuAngleOffset;
public boolean wristManuallyControlled = false;
public boolean automaticAntiTipActive = false;
public boolean hardwareFault = false;
public Status status = Status.OKAY;
public enum LiftPosition {
COLLECT,
GROUND,
LOW,
MEDIUM,
HIGH
}
public enum Status {
OKAY,
MONITORING,
WARNING,
DANGER
}
private final CyberarmEngine engine;
private final TimeCraftersConfiguration configuration;
public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) {
this.engine = engine;
this.configuration = configuration;
imuAngleOffset = hardwareConfig("imu_angle_offset").value();
// Define hardware
leftDrive = engine.hardwareMap.get(DcMotorEx.class, "left_drive"); // MOTOR PORT: ?
rightDrive = engine.hardwareMap.get(DcMotorEx.class, "right_drive"); // MOTOR PORT: ?
frontDrive = engine.hardwareMap.get(DcMotorEx.class, "front_drive"); // MOTOR PORT: ?
backDrive = engine.hardwareMap.get(DcMotorEx.class, "back_drive"); // MOTOR PORT: ?
liftDrive = engine.hardwareMap.get(DcMotorEx.class, "lift_drive"); // MOTOR PORT: ?
gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper"); // SERVO PORT: ?
wrist = engine.hardwareMap.get(ServoImplEx.class, "wrist"); // SERVO PORT: ?
indicatorA = engine.hardwareMap.colorSensor.get("indicator_A"); // I2C
indicatorB = engine.hardwareMap.colorSensor.get("indicator_B"); // I2C
imu = engine.hardwareMap.get(IMU.class, "imu");
// Configure hardware
// MOTORS
// DIRECTION
leftDrive.setDirection(hardwareConfig("left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
rightDrive.setDirection(hardwareConfig("right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
frontDrive.setDirection(hardwareConfig("front_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
backDrive.setDirection(hardwareConfig("back_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
liftDrive.setDirection(hardwareConfig("lift_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
// RUNMODE
leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); liftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// ZERO POWER BEHAVIOR
leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
liftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// SERVOS (POSITIONAL)
// Gripper
gripper.setDirection(hardwareConfig("gripper_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE);
gripper.setPosition(hardwareConfig("gripper_initial_position").value());
// Wrist
wrist.setDirection(hardwareConfig("wrist_direction_forward").value() ? Servo.Direction.FORWARD : Servo.Direction.REVERSE);
wrist.setPosition(hardwareConfig("wrist_initial_position").value());
// SENSORS
// COLOR SENSORS
indicatorA.enableLed(false);
indicatorB.enableLed(false);
// IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
imu.initialize(parameters);
}
public void standardTelemetry() {
// Motor Powers
engine.telemetry.addLine("Motor Powers");
engine.telemetry.addData(" Left Drive", leftDrive.getPower());
engine.telemetry.addData(" Right Drive", rightDrive.getPower());
engine.telemetry.addData(" Front Drive", frontDrive.getPower());
engine.telemetry.addData(" Back Drive", backDrive.getPower());
engine.telemetry.addLine();
engine.telemetry.addData(" Lift Drive", backDrive.getPower());
engine.telemetry.addLine();
// Motor Positions
engine.telemetry.addLine("Motor Positions");
engine.telemetry.addData(" Left Drive", "%d (%.2f in)", leftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, leftDrive.getCurrentPosition()));
engine.telemetry.addData(" Right Drive", "%d (%.2f in)", rightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, rightDrive.getCurrentPosition()));
engine.telemetry.addData(" Front Drive", "%d (%.2f in)", frontDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontDrive.getCurrentPosition()));
engine.telemetry.addData(" Back Drive", "%d (%.2f in)", backDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backDrive.getCurrentPosition()));
engine.telemetry.addLine();
engine.telemetry.addData(" Lift Drive", "%d (%.2d degrees)", leftDrive.getCurrentPosition(), ticksToAngle(liftDrive.getCurrentPosition()));
engine.telemetry.addLine();
// Motor Currents
engine.telemetry.addLine("Motor Currents (AMPS)");
engine.telemetry.addData(" Left Drive", leftDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Right Drive", rightDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Front Drive", frontDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Back Drive", backDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addLine();
engine.telemetry.addData(" Lift Drive", backDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addLine();
// Motor Directions
engine.telemetry.addLine("Motor Directions");
engine.telemetry.addData(" Left Drive", leftDrive.getDirection());
engine.telemetry.addData(" Right Drive", rightDrive.getDirection());
engine.telemetry.addData(" Front Drive", frontDrive.getDirection());
engine.telemetry.addData(" Back Drive", backDrive.getDirection());
engine.telemetry.addLine();
engine.telemetry.addData(" Lift Drive", backDrive.getDirection());
engine.telemetry.addLine();
// Servos
engine.telemetry.addLine("Servos");
engine.telemetry.addData(" Gripper Direction", gripper.getDirection());
engine.telemetry.addData(" Gripper Position", gripper.getPosition());
engine.telemetry.addData(" Gripper Enabled", gripper.isPwmEnabled());
engine.telemetry.addLine();
engine.telemetry.addData(" Wrist Direction", wrist.getDirection());
engine.telemetry.addData(" Wrist Position", wrist.getPosition());
engine.telemetry.addData(" Wrist Enabled", wrist.isPwmEnabled());
engine.telemetry.addLine();
// Sensors / IMU
engine.telemetry.addLine("IMU");
engine.telemetry.addData(" Facing", facing());
engine.telemetry.addData(" Turn Rate", turnRate());
}
public TimeCraftersConfiguration getConfiguration() {
return configuration;
}
// For: Drive Wheels
public int unitToTicks(DistanceUnit unit, double distance) {
double inches = unit.toInches(unit.fromUnit(unit, distance)); // NOTE: UNTESTED
// FIXME: This should be stored as a presudo constant at initialization
double wheelRadius = tuningConfig("wheel_radius").value();
double gearRatio = tuningConfig("wheel_gear_ratio").value();
double ticksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
return 0;
}
// For: Drive Wheels
public double ticksToUnit(DistanceUnit unit, int ticks) {
return 0;
}
// For: Lift Arm
public int angleToTicks(double angle) {
return 0;
}
// For: Lift Arm
public double ticksToAngle(int ticks) {
return 0;
}
public Variable hardwareConfig(String variableName) {
Action hardwareConfiguration = configuration.action("Robot", "Hardware");
for (Variable v : hardwareConfiguration.getVariables()) {
if (variableName.trim().equals(v.name)) {
return v;
}
}
throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Hardware");
}
public Variable tuningConfig(String variableName) {
Action action = configuration.action("Robot", "Tuning");
for (Variable v : action.getVariables()) {
if (variableName.trim().equals(v.name)) {
return v;
}
}
throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Tuning");
}
// TODO: Convert to 360 degree range with +90 degrees being on the RIGHT
public double facing() {
// FIXME: Apply imuAngleOffset
return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
}
public double turnRate() {
return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
}
}

23
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java Normal file
View File

@@ -0,0 +1,23 @@
package org.timecrafters.minibots.cyberarm.chiron.engines;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.minibots.cyberarm.chiron.Robot;
import org.timecrafters.minibots.cyberarm.chiron.states.DriverControlState;
public class TeleOpEngine extends CyberarmEngine {
private Robot robot;
@Override
public void setup() {
this.robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"));
addState(new DriverControlState(robot));
}
@Override
public void loop() {
super.loop();
robot.standardTelemetry();
}
}

284
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java Normal file
View File

@@ -0,0 +1,284 @@
package org.timecrafters.minibots.cyberarm.chiron.states;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.cyberarm.engine.V2.GamepadChecker;
import org.timecrafters.minibots.cyberarm.chiron.Robot;
public class DriverControlState extends CyberarmState {
private final Robot robot;
private final GamepadChecker controller;
private final double releaseConfirmationDelay;
private double lastLiftManualControlTime = 0, lastWristManualControlTime = 0, lastLEDStatusAnimationTime = 0;
private boolean LEDStatusToggle = false;
public DriverControlState(Robot robot) {
this.robot = robot;
this.controller = new GamepadChecker(engine, engine.gamepad1);
this.releaseConfirmationDelay = robot.tuningConfig("cone_release_confirmation_delay").value(); // ms
}
@Override
public void exec() {
double forwardSpeed = engine.gamepad1.left_stick_y * -1;
double rightSpeed = engine.gamepad1.right_stick_x;
double forwardAngle = robot.facing();
robot.status = Robot.Status.OKAY;
move(forwardAngle, forwardSpeed, rightSpeed);
liftManualControl();
wristManualControl();
automatics();
controller.update();
}
// FIXME: replace .setPower with .setVelocity
private void move(double forwardAngle, double forwardSpeed, double rightSpeed) {
if (robot.automaticAntiTipActive || robot.hardwareFault) {
return;
}
if (rightSpeed == 0 && forwardSpeed != 0) { // DRIVE STRAIGHT
robot.leftDrive.setPower(forwardSpeed);
robot.rightDrive.setPower(forwardSpeed);
robot.frontDrive.setPower(0);
robot.backDrive.setPower(0);
} else if (rightSpeed != 0 && forwardSpeed == 0) { // TURN IN PLACE
robot.leftDrive.setPower(rightSpeed);
robot.rightDrive.setPower(-rightSpeed);
robot.frontDrive.setPower(rightSpeed);
robot.backDrive.setPower(-rightSpeed);
} else if (rightSpeed != 0 && forwardSpeed != 0) { // ANGLE DRIVE
// TODO
stopDrive();
} else {
stopDrive();
}
}
private void stopDrive() {
robot.leftDrive.setPower(0);
robot.rightDrive.setPower(0);
robot.frontDrive.setPower(0);
robot.backDrive.setPower(0);
}
private void liftManualControl() {
if (robot.hardwareFault) {
return;
}
robot.status = Robot.Status.WARNING;
double stepInterval = robot.tuningConfig("lift_manual_step_interval").value();
int stepSize = robot.tuningConfig("lift_manual_step_size").value();
if ((engine.gamepad1.left_trigger > 0 || engine.gamepad1.right_trigger > 0) && runTime() - lastWristManualControlTime >= stepInterval) {
lastWristManualControlTime = runTime();
if (engine.gamepad1.left_trigger > 0) { // Lift DOWN
// robot.liftDrive.setVelocity(5, AngleUnit.DEGREES);
robot.liftDrive.setTargetPosition(robot.leftDrive.getTargetPosition() - stepSize);
} else if (engine.gamepad1.right_trigger > 0) { // Lift UP
robot.liftDrive.setTargetPosition(robot.leftDrive.getTargetPosition() + stepSize);
}
}
// FIXME: Detect when the triggers have been released and park lift arm at the current position
}
private void wristManualControl() {
if (robot.hardwareFault) {
return;
}
double stepInterval = robot.tuningConfig("wrist_manual_step_interval").value();
double stepSize = robot.tuningConfig("wrist_manual_step_size").value();
if ((engine.gamepad1.dpad_left || engine.gamepad1.dpad_right) && runTime() - lastLiftManualControlTime >= stepInterval) {
lastLiftManualControlTime = runTime();
if (engine.gamepad1.dpad_left) { // Wrist Left
robot.wrist.setPosition(robot.wrist.getPosition() - stepSize);
} else if (engine.gamepad1.dpad_right) { // Wrist Right
robot.wrist.setPosition(robot.wrist.getPosition() + stepSize);
}
}
}
private void stopLift() {
robot.liftDrive.setPower(0);
}
private void automatics() {
automaticWrist();
automaticAntiTip(); // NO OP
automaticHardwareMonitor();
automaticLEDStatus();
}
private void automaticWrist() {
if (robot.wristManuallyControlled) {
return;
}
if (robot.ticksToAngle(robot.liftDrive.getCurrentPosition()) >= 50) {
robot.wrist.setPosition(robot.hardwareConfig("wrist_deposit_position").value());
} else {
robot.wrist.setPosition(robot.hardwareConfig("wrist_initial_position").value());
}
}
// NO-OP
private void automaticAntiTip() {
// TODO: Take over drivetrain if robot starts to tip past a preconfigured point
// return control if past point of no-return or tipping is no longer a concern
// TODO: Calculate motion inverse to the normal of current direction
}
private void automaticHardwareMonitor() {
// Check that encoders are updating as expect, etc.
// NOTE: Robot should prevent/halt all movement in the event of a fault
// robot.hardwareFault = true;
if (robot.hardwareFault) {
robot.status = Robot.Status.DANGER;
stopDrive();
stopLift();
}
}
private void automaticLEDStatus() {
switch (robot.status) {
case OKAY:
robot.indicatorA.enableLed(false);
robot.indicatorB.enableLed(false);
break;
case MONITORING:
robot.indicatorA.enableLed(true);
robot.indicatorB.enableLed(true);
break;
case WARNING:
if (runTime() - lastLEDStatusAnimationTime >= 500){
lastLEDStatusAnimationTime = runTime();
LEDStatusToggle = !LEDStatusToggle;
robot.indicatorA.enableLed(LEDStatusToggle);
robot.indicatorA.enableLed(!LEDStatusToggle);
}
break;
case DANGER:
if (runTime() - lastLEDStatusAnimationTime >= 200){
lastLEDStatusAnimationTime = runTime();
LEDStatusToggle = !LEDStatusToggle;
robot.indicatorA.enableLed(LEDStatusToggle);
robot.indicatorA.enableLed(LEDStatusToggle);
}
break;
}
}
private void liftPosition(Robot.LiftPosition position) {
if (robot.hardwareFault) {
return;
}
robot.status = Robot.Status.WARNING;
switch (position) {
case COLLECT:
robot.liftDrive.setTargetPosition(robot.angleToTicks(120));
break;
case GROUND:
robot.liftDrive.setTargetPosition(robot.angleToTicks(100));
break;
case LOW:
robot.liftDrive.setTargetPosition(robot.angleToTicks(80));
break;
case MEDIUM:
robot.liftDrive.setTargetPosition(robot.angleToTicks(35));
break;
case HIGH:
robot.liftDrive.setTargetPosition(robot.angleToTicks(15));
break;
default:
throw new RuntimeException("Unexpected lift position!");
}
}
private void gripperOpen() {
robot.gripper.setPosition(robot.hardwareConfig("gripper_open_position").value());
}
private void gripperClosed() {
robot.gripper.setPosition(robot.hardwareConfig("gripper_closed_position").value());
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad != engine.gamepad1) {
return;
}
// Gripper Control
if (button.equals("left_bumper")) {
gripperOpen();
} else if (button.equals("right_bumper")) {
gripperClosed();
}
// Wrist Control
if (button.equals("dpad_down")) {
robot.wristManuallyControlled = false;
robot.wrist.setPosition(robot.hardwareConfig("wrist_deposit_position").value());
} else if (button.equals("dpad_up")) {
robot.wristManuallyControlled = false;
robot.wrist.setPosition(robot.hardwareConfig("wrist_initial_position").value());
}
// Automatic Lift Control
if (button.equals("a")) {
liftPosition(Robot.LiftPosition.COLLECT);
} else if (button.equals("x")) {
liftPosition(Robot.LiftPosition.GROUND);
} else if (button.equals("b")) {
liftPosition(Robot.LiftPosition.LOW);
} else if (button.equals("y")) {
liftPosition(Robot.LiftPosition.MEDIUM);
}
}
@Override
public void buttonUp(Gamepad gamepad, String button) {
if (gamepad != engine.gamepad1) {
return;
}
}
}

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/BlitzkriegState.java
View File

@@ -1,4 +0,0 @@
package org.timecrafters.minibots.cyberarm.states;
public class BlitzkriegState {
}

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Common.java → TeamCode/src/main/java/org/timecrafters/minibots/engines/Common.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.minibots.cyberarm.engines;
package org.timecrafters.minibots.engines;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;

7
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/FieldOrientedEngine.java → TeamCode/src/main/java/org/timecrafters/minibots/engines/FieldOrientedEngine.java
View File

@@ -1,11 +1,10 @@
package org.timecrafters.minibots.cyberarm.engines;
package org.timecrafters.minibots.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.cyberarm.states.FieldOrientedDrive;
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
import org.timecrafters.minibots.states.FieldOrientedDrive;
import org.timecrafters.minibots.states.MecanumRobot;
@TeleOp(name = "Field Oriented Drive")

6
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/MecanumMinibotTeleOpEngine.java → TeamCode/src/main/java/org/timecrafters/minibots/engines/MecanumMinibotTeleOpEngine.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.minibots.cyberarm.engines;
package org.timecrafters.minibots.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.cyberarm.MecanumMinibot;
import org.timecrafters.minibots.cyberarm.states.MecanumMinibotTeleOpState;
import org.timecrafters.minibots.MecanumMinibot;
import org.timecrafters.minibots.states.MecanumMinibotTeleOpState;
@TeleOp(name = "MecanumMinibot TeleOp", group = "minibot")
public class MecanumMinibotTeleOpEngine extends CyberarmEngine {

6
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_Robot_Engine.java → TeamCode/src/main/java/org/timecrafters/minibots/engines/Mecanum_Robot_Engine.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.minibots.cyberarm.engines;
package org.timecrafters.minibots.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
import org.timecrafters.minibots.cyberarm.states.Mecanum_Robot_State;
import org.timecrafters.minibots.states.MecanumRobot;
import org.timecrafters.minibots.states.Mecanum_Robot_State;
@TeleOp(name = "Mecanum Robot TeleOp")

6
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.java → TeamCode/src/main/java/org/timecrafters/minibots/engines/Mecanum_fancy_drive_TeleOp.java
View File

@@ -1,11 +1,11 @@
package org.timecrafters.minibots.cyberarm.engines;
package org.timecrafters.minibots.engines;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
import org.timecrafters.minibots.cyberarm.states.Mecanum_Fancy_Drive_State;
import org.timecrafters.minibots.states.MecanumRobot;
import org.timecrafters.minibots.states.Mecanum_Fancy_Drive_State;
@Disabled
@TeleOp(name = "Fancy Drive TeleOp")

6
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/PingPongEngine.java → TeamCode/src/main/java/org/timecrafters/minibots/engines/PingPongEngine.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.minibots.cyberarm.engines;
package org.timecrafters.minibots.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
import org.timecrafters.minibots.cyberarm.states.PingPongState;
import org.timecrafters.minibots.states.MecanumRobot;
import org.timecrafters.minibots.states.PingPongState;
@TeleOp (name = "Sodi PingPong")

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/TasksEngine.java → TeamCode/src/main/java/org/timecrafters/minibots/engines/TasksEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.minibots.cyberarm.engines;
package org.timecrafters.minibots.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

6
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/pickle_engine.java → TeamCode/src/main/java/org/timecrafters/minibots/engines/pickle_engine.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.minibots.cyberarm.engines;
package org.timecrafters.minibots.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.cyberarm.pickle_minibot_general;
import org.timecrafters.minibots.cyberarm.states.pickle_teleop_state;
import org.timecrafters.minibots.pickle_minibot_general;
import org.timecrafters.minibots.states.pickle_teleop_state;
@TeleOp (name = "pickle_minibot teleop", group = "minibot")
public class pickle_engine extends CyberarmEngine {

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/pickle_minibot_general.java → TeamCode/src/main/java/org/timecrafters/minibots/pickle_minibot_general.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.minibots.cyberarm;
package org.timecrafters.minibots;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;

0
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/.editorconfig → TeamCode/src/main/java/org/timecrafters/minibots/states/.editorconfig
View File

44
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/AutonomousReversalExperiment.java → TeamCode/src/main/java/org/timecrafters/minibots/states/AutonomousReversalExperiment.java
View File

@@ -1,22 +1,22 @@
package org.timecrafters.minibots.cyberarm.states;
import org.cyberarm.engine.V2.CyberarmState;
public class AutonomousReversalExperiment extends CyberarmState {
MecanumRobot robot;
public AutonomousReversalExperiment (MecanumRobot robot) {
this.robot = robot;
}
@Override
public void exec() {
if (engine.gamepad1.dpad_up) {
}
}
}
package org.timecrafters.minibots.states;
import org.cyberarm.engine.V2.CyberarmState;
public class AutonomousReversalExperiment extends CyberarmState {
MecanumRobot robot;
public AutonomousReversalExperiment (MecanumRobot robot) {
this.robot = robot;
}
@Override
public void exec() {
if (engine.gamepad1.dpad_up) {
}
}
}

4
TeamCode/src/main/java/org/timecrafters/minibots/states/BlitzkriegState.java Normal file
View File

@@ -0,0 +1,4 @@
package org.timecrafters.minibots.states;
public class BlitzkriegState {
}

0
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DriveState.java → TeamCode/src/main/java/org/timecrafters/minibots/states/DriveState.java
View File

134
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/FieldOrientedDrive.java → TeamCode/src/main/java/org/timecrafters/minibots/states/FieldOrientedDrive.java
View File

@@ -1,67 +1,67 @@
package org.timecrafters.minibots.cyberarm.states;
//adb connect 192.168.43.1
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import org.cyberarm.engine.V2.CyberarmState;
public class FieldOrientedDrive extends CyberarmState {
MecanumRobot robot;
BNO055IMU imu;
public FieldOrientedDrive(MecanumRobot robot) {
this.robot = robot;
}
@Override
public void init() {
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
// parameters.loggingEnabled = true;
// parameters.loggingTag = "IMU";
imu = engine.hardwareMap.get(BNO055IMU.class, "imu");
imu.initialize(parameters);
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
imu.initialize(parameters);
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
}
@Override
public void exec() {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.LIGHT_CHASE_BLUE);
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed!
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = engine.gamepad1.right_stick_x;
// Read inverse IMU heading, as the IMU heading is CW positive
double botHeading = -imu.getAngularOrientation().firstAngle;
double rotX = x * Math.cos(botHeading) - y * Math.sin(botHeading);
double rotY = x * Math.sin(botHeading) + y * Math.cos(botHeading);
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
robot.frontLeftDrive.setPower(frontLeftPower * .95);
robot.backRightDrive.setPower(backLeftPower * .95);
robot.frontRightDrive.setPower(frontRightPower * 1);
robot.backRightDrive.setPower(backRightPower * 1);
}
}
package org.timecrafters.minibots.states;
//adb connect 192.168.43.1
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import org.cyberarm.engine.V2.CyberarmState;
public class FieldOrientedDrive extends CyberarmState {
MecanumRobot robot;
BNO055IMU imu;
public FieldOrientedDrive(MecanumRobot robot) {
this.robot = robot;
}
@Override
public void init() {
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
// parameters.loggingEnabled = true;
// parameters.loggingTag = "IMU";
imu = engine.hardwareMap.get(BNO055IMU.class, "imu");
imu.initialize(parameters);
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
imu.initialize(parameters);
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
}
@Override
public void exec() {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.LIGHT_CHASE_BLUE);
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed!
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = engine.gamepad1.right_stick_x;
// Read inverse IMU heading, as the IMU heading is CW positive
double botHeading = -imu.getAngularOrientation().firstAngle;
double rotX = x * Math.cos(botHeading) - y * Math.sin(botHeading);
double rotY = x * Math.sin(botHeading) + y * Math.cos(botHeading);
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
robot.frontLeftDrive.setPower(frontLeftPower * .95);
robot.backRightDrive.setPower(backLeftPower * .95);
robot.frontRightDrive.setPower(frontRightPower * 1);
robot.backRightDrive.setPower(backRightPower * 1);
}
}

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java → TeamCode/src/main/java/org/timecrafters/minibots/states/MecanumMinibotTeleOpState.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.minibots.cyberarm.states;
package org.timecrafters.minibots.states;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.cyberarm.MecanumMinibot;
import org.timecrafters.minibots.MecanumMinibot;
public class MecanumMinibotTeleOpState extends CyberarmState {

100
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumRobot.java → TeamCode/src/main/java/org/timecrafters/minibots/states/MecanumRobot.java
View File

@@ -1,50 +1,50 @@
package org.timecrafters.minibots.cyberarm.states;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmEngine;
public class MecanumRobot {
private CyberarmEngine engine;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
public RevBlinkinLedDriver ledDriver;
public MecanumRobot(CyberarmEngine engine) {
this.engine = engine;
setupRobot();
}
private void setupRobot() {
//motors configuration
frontLeftDrive = engine.hardwareMap.dcMotor.get("front left");
frontRightDrive = engine.hardwareMap.dcMotor.get("front right");
backRightDrive = engine.hardwareMap.dcMotor.get("back left");
backLeftDrive = engine.hardwareMap.dcMotor.get("back right");
ledDriver = engine.hardwareMap.get(RevBlinkinLedDriver.class, "lights");
//motors direction and encoders
frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}
package org.timecrafters.minibots.states;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmEngine;
public class MecanumRobot {
private CyberarmEngine engine;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
public RevBlinkinLedDriver ledDriver;
public MecanumRobot(CyberarmEngine engine) {
this.engine = engine;
setupRobot();
}
private void setupRobot() {
//motors configuration
frontLeftDrive = engine.hardwareMap.dcMotor.get("front left");
frontRightDrive = engine.hardwareMap.dcMotor.get("front right");
backRightDrive = engine.hardwareMap.dcMotor.get("back left");
backLeftDrive = engine.hardwareMap.dcMotor.get("back right");
ledDriver = engine.hardwareMap.get(RevBlinkinLedDriver.class, "lights");
//motors direction and encoders
frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}

209
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java → TeamCode/src/main/java/org/timecrafters/minibots/states/Mecanum_Fancy_Drive_State.java
View File

@@ -1,105 +1,104 @@
package org.timecrafters.minibots.cyberarm.states;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class Mecanum_Fancy_Drive_State extends CyberarmState {
private final MecanumRobot robot;
public boolean A;
public boolean X;
public boolean Y;
private double drivePower = 1;
private boolean bprev;
public Mecanum_Fancy_Drive_State(MecanumRobot robot) {
this.robot = robot;
}
@Override
public void init() {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
}
@Override
public void exec() {
//Gamepad Read
A = engine.gamepad1.a;
X = engine.gamepad1.x;
Y = engine.gamepad1.y;
//Drivetrain Variables
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = engine.gamepad1.right_stick_x;
//toggle for drive speed
boolean b = engine.gamepad1.b;
if (b && !bprev) {
if (drivePower == 1) {
drivePower = 0.5;
} else {
drivePower = 1;
}
}
bprev = b;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
// As I programed this and ran it, I realized everything was backwards
// in direction so to fix that i either went in the robot object state and reversed
// directions on drive motors or put a negative in behind the joystick power to reverse it.
// I put negatives in to reverse it because it was the easiest at the moment.
robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
robot.backLeftDrive.setPower(backLeftPower * drivePower);
robot.frontRightDrive.setPower(frontRightPower * drivePower);
robot.backRightDrive.setPower(backRightPower * drivePower);
//-------------------------------------------------------------------------------------------------------------------
// LIGHT CONTROLS
if (drivePower == 1) {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.LIGHT_CHASE_BLUE);
}
else if (drivePower == 0.5){
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW);
}
}
//------------------------------------------------------------------------------------------------------------------------------------
// Telemetry Data
@Override
public void telemetry() {
engine.telemetry.addData("Speed", drivePower);
engine.telemetry.addData("FrontLeftEncoder", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("FrontRightEncoder", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftEncoder", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightEncoder", robot.backRightDrive.getCurrentPosition());
}
}
package org.timecrafters.minibots.states;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import org.cyberarm.engine.V2.CyberarmState;
public class Mecanum_Fancy_Drive_State extends CyberarmState {
private final MecanumRobot robot;
public boolean A;
public boolean X;
public boolean Y;
private double drivePower = 1;
private boolean bprev;
public Mecanum_Fancy_Drive_State(MecanumRobot robot) {
this.robot = robot;
}
@Override
public void init() {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
}
@Override
public void exec() {
//Gamepad Read
A = engine.gamepad1.a;
X = engine.gamepad1.x;
Y = engine.gamepad1.y;
//Drivetrain Variables
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = engine.gamepad1.right_stick_x;
//toggle for drive speed
boolean b = engine.gamepad1.b;
if (b && !bprev) {
if (drivePower == 1) {
drivePower = 0.5;
} else {
drivePower = 1;
}
}
bprev = b;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
// As I programed this and ran it, I realized everything was backwards
// in direction so to fix that i either went in the robot object state and reversed
// directions on drive motors or put a negative in behind the joystick power to reverse it.
// I put negatives in to reverse it because it was the easiest at the moment.
robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
robot.backLeftDrive.setPower(backLeftPower * drivePower);
robot.frontRightDrive.setPower(frontRightPower * drivePower);
robot.backRightDrive.setPower(backRightPower * drivePower);
//-------------------------------------------------------------------------------------------------------------------
// LIGHT CONTROLS
if (drivePower == 1) {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.LIGHT_CHASE_BLUE);
}
else if (drivePower == 0.5){
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW);
}
}
//------------------------------------------------------------------------------------------------------------------------------------
// Telemetry Data
@Override
public void telemetry() {
engine.telemetry.addData("Speed", drivePower);
engine.telemetry.addData("FrontLeftEncoder", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("FrontRightEncoder", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftEncoder", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightEncoder", robot.backRightDrive.getCurrentPosition());
}
}

234
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java → TeamCode/src/main/java/org/timecrafters/minibots/states/Mecanum_Robot_State.java
View File

@@ -1,117 +1,117 @@
package org.timecrafters.minibots.cyberarm.states;
import org.cyberarm.engine.V2.CyberarmState;
public class Mecanum_Robot_State extends CyberarmState {
// adb connect 192.168.43.1
private final MecanumRobot robot;
private float maxSpeed = 1;
private double halfSpeed = 0.5;
public Mecanum_Robot_State(MecanumRobot robot) {
this.robot = robot;
}
@Override
public void exec() {
if (engine.gamepad1.left_trigger > 0.5){
if (engine.gamepad1.left_bumper) {
robot.backRightDrive.setPower(halfSpeed);
robot.frontRightDrive.setPower(-halfSpeed);
robot.backLeftDrive.setPower(-halfSpeed);
robot.frontLeftDrive.setPower(halfSpeed);
} else if (engine.gamepad1.right_bumper) {
robot.backRightDrive.setPower(-halfSpeed);
robot.frontRightDrive.setPower(halfSpeed);
robot.backLeftDrive.setPower(halfSpeed);
robot.frontLeftDrive.setPower(-halfSpeed);
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_up) {
robot.frontLeftDrive.setPower(-halfSpeed);
robot.backRightDrive.setPower(-halfSpeed);
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_up) {
robot.frontRightDrive.setPower(-halfSpeed);
robot.backLeftDrive.setPower(-halfSpeed);
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_down) {
robot.backLeftDrive.setPower(halfSpeed);
robot.frontRightDrive.setPower(halfSpeed);
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_down) {
robot.backRightDrive.setPower(halfSpeed);
robot.frontLeftDrive.setPower(halfSpeed);
}
else {
robot.backRightDrive.setPower(engine.gamepad1.right_stick_y * halfSpeed) ;
robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y * halfSpeed);
robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y * halfSpeed);
robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y * halfSpeed);
}
}
else {
if (engine.gamepad1.left_bumper) {
robot.backRightDrive.setPower(maxSpeed);
robot.frontRightDrive.setPower(-maxSpeed);
robot.backLeftDrive.setPower(-maxSpeed);
robot.frontLeftDrive.setPower(maxSpeed);
} else if (engine.gamepad1.right_bumper) {
robot.backRightDrive.setPower(-maxSpeed);
robot.frontRightDrive.setPower(maxSpeed);
robot.backLeftDrive.setPower(maxSpeed);
robot.frontLeftDrive.setPower(-maxSpeed);
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_up) {
robot.frontLeftDrive.setPower(-maxSpeed);
robot.backRightDrive.setPower(-maxSpeed);
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_up) {
robot.frontRightDrive.setPower(-maxSpeed);
robot.backLeftDrive.setPower(-maxSpeed);
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_down) {
robot.backLeftDrive.setPower(maxSpeed);
robot.frontRightDrive.setPower(maxSpeed);
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_down) {
robot.backRightDrive.setPower(maxSpeed);
robot.frontLeftDrive.setPower(maxSpeed);
} else {
robot.backRightDrive.setPower(engine.gamepad1.right_stick_y * maxSpeed) ;
robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y * maxSpeed);
robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y * maxSpeed);
robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y * maxSpeed);
}
}
}
}
package org.timecrafters.minibots.states;
import org.cyberarm.engine.V2.CyberarmState;
public class Mecanum_Robot_State extends CyberarmState {
// adb connect 192.168.43.1
private final MecanumRobot robot;
private float maxSpeed = 1;
private double halfSpeed = 0.5;
public Mecanum_Robot_State(MecanumRobot robot) {
this.robot = robot;
}
@Override
public void exec() {
if (engine.gamepad1.left_trigger > 0.5){
if (engine.gamepad1.left_bumper) {
robot.backRightDrive.setPower(halfSpeed);
robot.frontRightDrive.setPower(-halfSpeed);
robot.backLeftDrive.setPower(-halfSpeed);
robot.frontLeftDrive.setPower(halfSpeed);
} else if (engine.gamepad1.right_bumper) {
robot.backRightDrive.setPower(-halfSpeed);
robot.frontRightDrive.setPower(halfSpeed);
robot.backLeftDrive.setPower(halfSpeed);
robot.frontLeftDrive.setPower(-halfSpeed);
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_up) {
robot.frontLeftDrive.setPower(-halfSpeed);
robot.backRightDrive.setPower(-halfSpeed);
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_up) {
robot.frontRightDrive.setPower(-halfSpeed);
robot.backLeftDrive.setPower(-halfSpeed);
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_down) {
robot.backLeftDrive.setPower(halfSpeed);
robot.frontRightDrive.setPower(halfSpeed);
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_down) {
robot.backRightDrive.setPower(halfSpeed);
robot.frontLeftDrive.setPower(halfSpeed);
}
else {
robot.backRightDrive.setPower(engine.gamepad1.right_stick_y * halfSpeed) ;
robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y * halfSpeed);
robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y * halfSpeed);
robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y * halfSpeed);
}
}
else {
if (engine.gamepad1.left_bumper) {
robot.backRightDrive.setPower(maxSpeed);
robot.frontRightDrive.setPower(-maxSpeed);
robot.backLeftDrive.setPower(-maxSpeed);
robot.frontLeftDrive.setPower(maxSpeed);
} else if (engine.gamepad1.right_bumper) {
robot.backRightDrive.setPower(-maxSpeed);
robot.frontRightDrive.setPower(maxSpeed);
robot.backLeftDrive.setPower(maxSpeed);
robot.frontLeftDrive.setPower(-maxSpeed);
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_up) {
robot.frontLeftDrive.setPower(-maxSpeed);
robot.backRightDrive.setPower(-maxSpeed);
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_up) {
robot.frontRightDrive.setPower(-maxSpeed);
robot.backLeftDrive.setPower(-maxSpeed);
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_down) {
robot.backLeftDrive.setPower(maxSpeed);
robot.frontRightDrive.setPower(maxSpeed);
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_down) {
robot.backRightDrive.setPower(maxSpeed);
robot.frontLeftDrive.setPower(maxSpeed);
} else {
robot.backRightDrive.setPower(engine.gamepad1.right_stick_y * maxSpeed) ;
robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y * maxSpeed);
robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y * maxSpeed);
robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y * maxSpeed);
}
}
}
}

70
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/PingPongState.java → TeamCode/src/main/java/org/timecrafters/minibots/states/PingPongState.java
View File

@@ -1,35 +1,35 @@
package org.timecrafters.minibots.cyberarm.states;
import org.cyberarm.engine.V2.CyberarmState;
public class PingPongState extends CyberarmState {
private final MecanumRobot robot;
public PingPongState(MecanumRobot robot) {
this.robot = robot;
}
@Override
public void exec() {
if (engine.gamepad1.left_bumper) {
robot.frontLeftDrive.setPower(1);
robot.backLeftDrive.setPower(-1);
robot.backRightDrive.setPower(1);
robot.frontRightDrive.setPower(-1);
}
else if (engine.gamepad1.right_bumper) {
robot.frontLeftDrive.setPower(-1);
robot.backLeftDrive.setPower(1);
robot.frontRightDrive.setPower(1);
robot.backRightDrive.setPower(-1);
}
else {
robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y);
robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y);
robot.backRightDrive.setPower(engine.gamepad1.right_stick_y);
robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y);
}
}
}
package org.timecrafters.minibots.states;
import org.cyberarm.engine.V2.CyberarmState;
public class PingPongState extends CyberarmState {
private final MecanumRobot robot;
public PingPongState(MecanumRobot robot) {
this.robot = robot;
}
@Override
public void exec() {
if (engine.gamepad1.left_bumper) {
robot.frontLeftDrive.setPower(1);
robot.backLeftDrive.setPower(-1);
robot.backRightDrive.setPower(1);
robot.frontRightDrive.setPower(-1);
}
else if (engine.gamepad1.right_bumper) {
robot.frontLeftDrive.setPower(-1);
robot.backLeftDrive.setPower(1);
robot.frontRightDrive.setPower(1);
robot.backRightDrive.setPower(-1);
}
else {
robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y);
robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y);
robot.backRightDrive.setPower(engine.gamepad1.right_stick_y);
robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y);
}
}
}

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/pickle_teleop_state.java → TeamCode/src/main/java/org/timecrafters/minibots/states/pickle_teleop_state.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.minibots.cyberarm.states;
package org.timecrafters.minibots.states;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.cyberarm.pickle_minibot_general;
import org.timecrafters.minibots.pickle_minibot_general;
public class pickle_teleop_state extends CyberarmState {
private final pickle_minibot_general robot;

6
build.common.gradle
View File

@@ -47,9 +47,6 @@ android {
}
}
aaptOptions {
noCompress "tflite"
}
defaultConfig {
signingConfig signingConfigs.debug
@@ -111,9 +108,6 @@ android {
targetCompatibility JavaVersion.VERSION_1_8
}
packagingOptions {
pickFirst '**/*.so'
}
sourceSets.main {
jni.srcDirs = []
jniLibs.srcDir rootProject.file('libs')

2
build.dependencies.gradle
View File

@@ -4,6 +4,7 @@ repositories {
flatDir {
dirs rootProject.file('libs')
}
maven { url = 'https://maven.brott.dev/' }
}
dependencies {
@@ -18,5 +19,6 @@ dependencies {
implementation 'org.tensorflow:tensorflow-lite-task-vision:0.2.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation 'org.firstinspires.ftc:gameAssets-PowerPlay:1.0.0'
implementation 'com.acmerobotics.roadrunner:core:0.5.1'
}

2
build.gradle
View File

@@ -10,7 +10,7 @@ buildscript {
google()
}
dependencies {
classpath 'com.android.tools.build:gradle:7.3.0'
classpath 'com.android.tools.build:gradle:7.3.1'
}
}