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

last meetings code that I forgot to push

Browse Source
This commit is contained in:
SpencerPiha
2023-01-04 16:10:24 -06:00
parent d6652cc807
commit 0c5b5f4cc6
4 changed files with 73 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java
View File

@@ -53,6 +53,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
// 7 Drop bottom arm down on the junction to place cone
addState(new BottomArm(robot, "RightFourCone", "07-0"));
// 7-1 drive back slightly
addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-1"));
// 8 Drop cone as soon as arm is in position
addState(new CollectorState(robot, "RightFourCone", "08-0"));
@@ -61,12 +64,12 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
//
// // 10 Back up and bring lower arm down (parallel state)
addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0"));
// 11 Rotate towards stack
addState(new RotationState(robot, "RightFourCone", "11-0"));
//
// // 11 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
// addState(new TopArm(robot, "RightFourCone", "11-0"));
//
// // 12 Rotate towards stack
// //filled in as parallel state. in parallel with previous state
// // 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"));
//
// // 13 Drive at stack while collecting and check to see when we grab it
// addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));

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

@@ -1,5 +1,7 @@
package org.timecrafters.Autonomous.States;
import android.util.Log;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
@@ -91,6 +93,22 @@ public class DriverStateWithOdometer extends CyberarmState {
setHasFinished(true);
}
if (!getHasFinished()){
float angle = robot.imu.getAngularOrientation().firstAngle;
if (angle < 0){
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
if (angle > 0){
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
}
@@ -100,7 +118,15 @@ public class DriverStateWithOdometer 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("frontRightDrive", robot.frontRightDrive.getPower());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
@@ -109,5 +135,9 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
}
}

34
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java
View File

@@ -1,5 +1,7 @@
package org.timecrafters.Autonomous.States;
import android.util.Log;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
@@ -35,12 +37,14 @@ public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
addParallelState(new TopArm(robot, "RightFourCone", "04-1"));
}
@Override
public void exec() {
addParallelState(new TopArm(robot, "RightFourCone", "04-1"));
if (stateDisabled) {
setHasFinished(true);
return;
@@ -89,6 +93,22 @@ public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
if (!getHasFinished()){
float angle = robot.imu.getAngularOrientation().firstAngle;
if (angle < -0.25){
robot.backLeftDrive.setPower(drivePower * 0.25);
robot.backRightDrive.setPower(drivePower * 1.25);
robot.frontLeftDrive.setPower(drivePower * 0.25);
robot.frontRightDrive.setPower(drivePower * 1.25);
}
if (angle > 0.25){
robot.backLeftDrive.setPower(drivePower * 1.25);
robot.backRightDrive.setPower(drivePower * 0.25);
robot.frontLeftDrive.setPower(drivePower * 1.25);
robot.frontRightDrive.setPower(drivePower * 0.25);
}
}
}
@@ -99,7 +119,15 @@ public class DriverStateWithOdometerUpperArmParallelState 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("frontRightDrive", robot.frontRightDrive.getPower());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
@@ -108,5 +136,9 @@ public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
}
}

3
TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java
View File

@@ -53,7 +53,8 @@ public class TopArm extends CyberarmState {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance);
} if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
}
else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);