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

Why won't TurnState turn?

Browse Source
This commit is contained in:
NerdyBirdy460
2023-11-30 20:37:54 -06:00
parent 79e06c3c10
commit 749b0d34df
3 changed files with 19 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java
View File

@@ -2,6 +2,7 @@ package org.timecrafters.CenterStage.Autonomous.States;
import android.annotation.SuppressLint;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import dev.cyberarm.engine.V2.CyberarmState;
@@ -39,6 +40,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
robot.leftFront.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn);
engine.telemetry.addData("Distance Sensor Reading", robot.distSensor.getDistance(DistanceUnit.MM));
}

14
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.States;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
@@ -40,8 +41,14 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("Target Pos", robot.leftFront.getTargetPosition());
engine.telemetry.addData("Target Rotation", targetRot);
engine.telemetry.addData("Power", robot.leftFront.getPower());
engine.telemetry.addData("Distance Sensor Reading", robot.distSensor.getDistance(DistanceUnit.MM));
engine.telemetry.addLine();
engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn);
engine.telemetry.addData("Engine Ready To Turn Value", engine.blackboardGetInt("readyToTurn"));
}
@Override
@@ -56,7 +63,8 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightBack.setPower(turnSpeed);
neededRot = (targetRot - robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
// robot.imu.resetYaw();
engine.blackboardSet("readyToTurn", 0);
}
@@ -67,7 +75,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
currentRot = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) {
if (readyToTurn == 1 && Math.abs(neededRot) > 10) {
targetRot = -90;

7
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
View File

@@ -1,9 +1,12 @@
package org.timecrafters.CenterStage.Common;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
@@ -18,6 +21,7 @@ public class SodiPizzaMinibotObject extends Robot {
public DcMotor leftFront, rightFront, leftBack, rightBack;
public Servo shoulder, gripper;
public IMU imu;
public Rev2mDistanceSensor distSensor;
private String string;
public static double GRIPPER_CLOSED = 0.333; // ~90 degrees
@@ -69,8 +73,9 @@ public class SodiPizzaMinibotObject extends Robot {
shoulder = engine.hardwareMap.servo.get("arm");
gripper = engine.hardwareMap.servo.get("gripper");
//readyToTurn
//Distance Sensor
distSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "distSensor");
}
}