mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-15 16:52:35 +00:00
robot code for odometry is wrote, still needs to be tuned, and redid the entire robot structure for organization and my sanity
This commit is contained in:
SpencerPiha
@@ -0,0 +1,55 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
|
||||
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class DriveToCoordinatesState extends CyberarmState {
|
||||
|
||||
CompetitionRobotV1 robot;
|
||||
public double xTarget;
|
||||
public double yTarget;
|
||||
public double hTarget;
|
||||
public boolean posAchieved = false;
|
||||
|
||||
public DriveToCoordinatesState(CompetitionRobotV1 robot/*, String groupName, String actionName*/) {
|
||||
this.robot = robot;
|
||||
// this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value();
|
||||
// this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value();
|
||||
// this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
robot.hTarget = hTarget;
|
||||
robot.yTarget = yTarget;
|
||||
robot.xTarget = xTarget;
|
||||
|
||||
if (posAchieved){
|
||||
// setHasFinished(true);
|
||||
} else {
|
||||
robot.OdometryLocalizer();
|
||||
if (((robot.positionX > xTarget - 1) && (robot.positionX < xTarget + 1)) &&
|
||||
((robot.positionH > hTarget - 1) && (robot.positionH < hTarget + 1)) &&
|
||||
((robot.positionY > yTarget - 1) && (robot.positionY < yTarget + 1))){
|
||||
posAchieved = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("x pos", robot.positionX);
|
||||
engine.telemetry.addData("y pos", robot.positionY);
|
||||
engine.telemetry.addData("h pos odo", robot.positionH);
|
||||
engine.telemetry.addData("aux encoder", robot.odometerA.getCurrentPosition());
|
||||
engine.telemetry.addData("left encoder", robot.odometerL.getCurrentPosition());
|
||||
engine.telemetry.addData("right encoder", robot.odometerR.getCurrentPosition());
|
||||
engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.Engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
|
||||
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
@Autonomous(name = "odo test and tune")
|
||||
|
||||
public class OdometryTestEngine extends CyberarmEngine {
|
||||
|
||||
CompetitionRobotV1 robot;
|
||||
@Override
|
||||
public void setup() {
|
||||
this.robot = new CompetitionRobotV1("Autonomous odometry test");
|
||||
this.robot.setup();
|
||||
|
||||
addState(new DriveToCoordinatesState(robot/*,"odoTest", "00-1"*/));
|
||||
}
|
||||
}
|
||||
@@ -3,7 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.ProtoBotSodi;
|
||||
import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi;
|
||||
import org.timecrafters.CenterStage.Autonomous.SodiStates.ProtoBotStateSodi;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
|
||||
@@ -1,13 +1,9 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.Engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
|
||||
import org.timecrafters.CenterStage.Autonomous.States.AutoStateScrimmage;
|
||||
import org.timecrafters.CenterStage.Autonomous.States.DepositorArmPosState;
|
||||
import org.timecrafters.CenterStage.Autonomous.SodiStates.AutoStateScrimmage;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
import org.timecrafters.CenterStage.Common.ServoTestRobot;
|
||||
import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
|
||||
@@ -2,14 +2,10 @@ package org.timecrafters.CenterStage.Autonomous.Engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoArmState;
|
||||
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoFirstDriveState;
|
||||
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoSecDriveState;
|
||||
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState;
|
||||
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest;
|
||||
import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoFirstDriveState;
|
||||
import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoTurnState;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
@Autonomous(name = "Sodi's Pizza Box Bot Auto", group = "")
|
||||
public class SodiPizzaAutoRedRightEngine extends CyberarmEngine {
|
||||
|
||||
@@ -1,11 +1,8 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
/**BEGAN WITH 43 LINES**/
|
||||
|
||||
import com.arcrobotics.ftclib.hardware.motors.Motor;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.ProtoBotSodi;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
import org.timecrafters.CenterStage.Common.ServoTestRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
@@ -1,4 +1,4 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
@@ -1,9 +1,4 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
|
||||
import com.arcrobotics.ftclib.hardware.motors.Motor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.CenterStage.Common.ProtoBotSodi;
|
||||
@@ -1,4 +1,4 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import android.annotation.SuppressLint;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
|
||||
|
||||
@@ -1,10 +1,8 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
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;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
package org.timecrafters.CenterStage.Autonomous.SodiStates;
|
||||
|
||||
import android.annotation.SuppressLint;
|
||||
|
||||
@@ -1,82 +0,0 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.States;
|
||||
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class DriveToCoordinatesState extends CyberarmState {
|
||||
|
||||
private final boolean stateDisabled;
|
||||
PrototypeRobot robot;
|
||||
private PIDController driveXPidController;
|
||||
private PIDController driveYPidController;
|
||||
private PIDController driveHPidController;
|
||||
private double pidX;
|
||||
private double pidY;
|
||||
private double pidH;
|
||||
|
||||
public double xTarget;
|
||||
public double yTarget;
|
||||
public double hTarget;
|
||||
|
||||
public double currentVelocityX;
|
||||
public double currentVelocityY;
|
||||
public double currentVelocityH;
|
||||
|
||||
public double targetVelocityFL;
|
||||
public double targetVelocityBL;
|
||||
public double targetVelocityFR;
|
||||
public double targetVelocityBR;
|
||||
|
||||
|
||||
public static double Xp = 0, Xi = 0, Xd = 0; // p = 0.02 i = 0.09 d = 0
|
||||
public static double Yp = 0, Yi = 0, Yd = 0;// p = 0.03 i = 0.07 d = 0
|
||||
public static double Hp = 0, Hi = 0, Hd = 0;// p = 0.03 i = 0.07 d = 0
|
||||
|
||||
public DriveToCoordinatesState(PrototypeRobot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
driveXPidController = new PIDController(Xp, Xi, Xd);
|
||||
driveYPidController = new PIDController(-Yp, -Yi, -Yd);
|
||||
driveHPidController = new PIDController(-Hp, -Hi, -Hd);
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value();
|
||||
this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value();
|
||||
this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
//add variables that need to be reinitillized
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
// PID functions to come up with the velocity to get to the final point.
|
||||
driveXPidController.setPID(Xp, Xi, Xd);
|
||||
pidX = driveXPidController.calculate(robot.positionX, xTarget);
|
||||
driveYPidController.setPID(Yp, Yi, Yd);
|
||||
pidY = (driveYPidController.calculate(robot.positionY, yTarget));
|
||||
driveHPidController.setPID(Hp, Hi, Hd);
|
||||
pidH = (driveYPidController.calculate(robot.positionH, hTarget));
|
||||
|
||||
// PID robot Velocity Ratios
|
||||
currentVelocityX = robot.MaxVelocityForward * pidX;
|
||||
currentVelocityY = robot.MaxStrafeVelocity * pidY;
|
||||
currentVelocityH = robot.MaxRotationalVelocity * pidH;
|
||||
|
||||
// Kinematic Formulas with plugged in velocities to solve for wheel velocities.
|
||||
targetVelocityFL = currentVelocityX - currentVelocityY - ((2 * (robot.L / 2) * robot.MaxRotationalVelocity));
|
||||
targetVelocityBL = currentVelocityX + currentVelocityY - ((2 * (robot.L / 2) * robot.MaxRotationalVelocity));
|
||||
targetVelocityBR = currentVelocityX - currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity));
|
||||
targetVelocityFR = currentVelocityX + currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity));
|
||||
|
||||
// robot.frontLeft.setVelocity((targetVelocityFL / robot.R));
|
||||
// robot.backLeft.setVelocity((targetVelocityBL / robot.R));
|
||||
// robot.backRight.setVelocity((targetVelocityBR / robot.R));
|
||||
// robot.frontRight.setVelocity((targetVelocityFR / robot.R));
|
||||
|
||||
// setHasFinished(true)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,214 @@
|
||||
package org.timecrafters.CenterStage.Common;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.timecrafters.Library.Robot;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
|
||||
public class CompetitionRobotV1 extends Robot {
|
||||
// --------------------------------------------------------------------------------------------------- engine and state setup variables:
|
||||
private String string;
|
||||
private CyberarmEngine engine;
|
||||
|
||||
public TimeCraftersConfiguration configuration;
|
||||
|
||||
// HardwareMap setup
|
||||
|
||||
public DcMotor frontLeft, frontRight, backLeft, backRight, lift;
|
||||
public DcMotor odometerR, odometerL, odometerA;
|
||||
|
||||
public IMU imu;
|
||||
public Servo shoulder, elbow, leftClaw, rightClaw;
|
||||
|
||||
// ---------------------------------------------------------------------------------------------------- odometry variables:
|
||||
public static double Hp = 0, Hi = 0, Hd = 0;
|
||||
public static double Xp = 0, Xi = 0, Xd = 0;
|
||||
public static double Yp = 0, Yi = 0, Yd = 0;
|
||||
private double drivePower = 1;
|
||||
|
||||
public double xMultiplier = 1;
|
||||
public double yMultiplier = 1;
|
||||
public double positionX = 0;
|
||||
public double positionY = 0;
|
||||
public double positionH = 0;
|
||||
public double xTarget;
|
||||
public double yTarget;
|
||||
public double hTarget;
|
||||
|
||||
public int currentRightPosition = 0;
|
||||
public int currentLeftPosition = 0;
|
||||
public int currentAuxPosition = 0;
|
||||
public int oldRightPosition = 0;
|
||||
public int oldLeftPosition = 0;
|
||||
public int oldAuxPosition = 0;
|
||||
public double rx;
|
||||
public final static double L = 23.425; // distance between left and right encoder in cm
|
||||
final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
|
||||
public final static double R = 4.5; // wheel radius in cm
|
||||
final static double N = 8192; // encoder ticks per revolution (REV encoder)
|
||||
|
||||
// heading math variables for pid with imu
|
||||
public double integralSum = 0;
|
||||
public double targetHeading;
|
||||
public final double cm_per_tick = (2 * Math.PI * R) / N;
|
||||
private double lastError = 0;
|
||||
ElapsedTime timer = new ElapsedTime();
|
||||
|
||||
// ---------------------------------------------------------------------------------------------------- collector / depositor variables:
|
||||
|
||||
public float depositorPos;
|
||||
public float collectorPos;
|
||||
public boolean lbsVar2;
|
||||
public boolean rbsVar2;
|
||||
|
||||
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
|
||||
public int armPosition = 0;
|
||||
public long startOfSequencerTime;
|
||||
public int oldArmPosition = 0;
|
||||
public float DEPOSITOR_SHOULDER_IN;
|
||||
public float DEPOSITOR_SHOULDER_OUT;
|
||||
public float DEPOSITOR_ELBOW_IN;
|
||||
public float DEPOSITOR_ELBOW_OUT;
|
||||
public float COLLECTOR_SHOULDER_IN;
|
||||
public float COLLECTOR_SHOULDER_PASSIVE;
|
||||
public float COLLECTOR_SHOULDER_OUT;
|
||||
public float COLLECTOR_ELBOW_IN;
|
||||
public float COLLECTOR_ELBOW_PASSIVE;
|
||||
public float COLLECTOR_ELBOW_OUT;
|
||||
private HardwareMap hardwareMap;
|
||||
|
||||
|
||||
|
||||
public CompetitionRobotV1(String string) {
|
||||
this.engine = engine;
|
||||
setup();
|
||||
this.string = string;
|
||||
}
|
||||
|
||||
public void initConstants() {
|
||||
DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value();
|
||||
DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value();
|
||||
DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value();
|
||||
DEPOSITOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_OUT").value();
|
||||
COLLECTOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_IN").value();
|
||||
COLLECTOR_SHOULDER_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_PASSIVE").value();
|
||||
COLLECTOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_OUT").value();
|
||||
COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value();
|
||||
COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value();
|
||||
COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
System.out.println("Bacon: " + this.string);
|
||||
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
|
||||
this.engine = CyberarmEngine.instance;
|
||||
|
||||
imu = engine.hardwareMap.get(IMU.class, "imu");
|
||||
|
||||
//----------------------------------------------------------------------------------------------------------------------------MOTORS
|
||||
frontRight = engine.hardwareMap.dcMotor.get("frontRight");
|
||||
frontLeft = engine.hardwareMap.dcMotor.get("frontLeft");
|
||||
backRight = engine.hardwareMap.dcMotor.get("backRight");
|
||||
backLeft = engine.hardwareMap.dcMotor.get("backLeft");
|
||||
lift = engine.hardwareMap.dcMotor.get("Lift");
|
||||
odometerL = engine.hardwareMap.dcMotor.get("leftodo");
|
||||
odometerR = engine.hardwareMap.dcMotor.get("rightodo");
|
||||
odometerA = engine.hardwareMap.dcMotor.get("auxodo");
|
||||
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
backRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
lift.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
odometerR.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
odometerL.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
odometerA.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
odometerR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
odometerL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
odometerA.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
|
||||
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
configuration = new TimeCraftersConfiguration("Blue Crab");
|
||||
|
||||
initConstants();
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------------------------------------------------------SERVO
|
||||
shoulder = hardwareMap.servo.get("shoulder");
|
||||
elbow = hardwareMap.servo.get("elbow");
|
||||
leftClaw = hardwareMap.servo.get("leftClaw");
|
||||
rightClaw = hardwareMap.servo.get("leftClaw");
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void CollectorToggle() {
|
||||
boolean lbs2 = engine.gamepad2.left_stick_button;
|
||||
if (lbs2 && !lbsVar2) {
|
||||
if (collectorPos == 1F) {
|
||||
collectorPos = 0F;
|
||||
} else {
|
||||
collectorPos = 1F;
|
||||
}
|
||||
}
|
||||
lbsVar2 = lbs2;
|
||||
}
|
||||
|
||||
|
||||
public void DepositorToggle() {
|
||||
boolean rbs2 = engine.gamepad2.right_stick_button;
|
||||
if (rbs2 && !rbsVar2) {
|
||||
if (depositorPos == 0.6F) {
|
||||
depositorPos = 0F;
|
||||
} else {
|
||||
depositorPos = 0.6F;
|
||||
}
|
||||
}
|
||||
rbsVar2 = rbs2;
|
||||
}
|
||||
|
||||
public void OdometryLocalizer() {
|
||||
|
||||
// update positions
|
||||
oldRightPosition = currentRightPosition;
|
||||
oldLeftPosition = currentLeftPosition;
|
||||
oldAuxPosition = currentAuxPosition;
|
||||
|
||||
currentRightPosition = -odometerR.getCurrentPosition();
|
||||
currentLeftPosition = -odometerL.getCurrentPosition();
|
||||
currentAuxPosition = odometerA.getCurrentPosition();
|
||||
|
||||
int dnl1 = currentLeftPosition - oldLeftPosition;
|
||||
int dnr2 = currentRightPosition - oldRightPosition;
|
||||
int dna3 = currentAuxPosition - oldAuxPosition;
|
||||
|
||||
// the robot has turned and moved a tiny bit between two measurements
|
||||
double dtheta = cm_per_tick * (dnr2 - dnl1) / L;
|
||||
double dx = cm_per_tick * (dnl1 + dnr2) / 2.0;
|
||||
double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L);
|
||||
|
||||
// the small movement of the bot gets added to the field coordinates
|
||||
double theta = positionH + (dtheta / 2.0);
|
||||
positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier;
|
||||
positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier;
|
||||
positionH += dtheta;
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,5 +1,6 @@
|
||||
package org.timecrafters.CenterStage.Common;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.arcrobotics.ftclib.drivebase.HDrive;
|
||||
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
@@ -10,32 +11,21 @@ import com.qualcomm.robotcore.hardware.IMU;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.Library.Robot;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
|
||||
@Config
|
||||
public class PrototypeRobot extends Robot {
|
||||
|
||||
// public double integralSum = 0;
|
||||
// double Kp = 0;
|
||||
// double Ki = 0;
|
||||
// double Kd = 0;
|
||||
// public double lastError = 0;
|
||||
public double PIDrx;
|
||||
public double targetHeading;
|
||||
public boolean headingLock = false;
|
||||
public double collectLock = Math.toRadians(-90);
|
||||
public double backDropLock = Math.toRadians(90);
|
||||
ElapsedTime timer = new ElapsedTime();
|
||||
public int armPosition = 0;
|
||||
public boolean stateFinished;
|
||||
public long startOfSequencerTime;
|
||||
public int oldArmPosition = 0;
|
||||
public long waitTime;
|
||||
public double servoWaitTime;
|
||||
public double servoSecPerDeg = 0.14/60;
|
||||
public float DEPOSITOR_SHOULDER_IN;
|
||||
public float DEPOSITOR_SHOULDER_OUT;
|
||||
public float DEPOSITOR_ELBOW_IN;
|
||||
@@ -46,56 +36,56 @@ public class PrototypeRobot extends Robot {
|
||||
public float COLLECTOR_ELBOW_IN;
|
||||
public float COLLECTOR_ELBOW_PASSIVE;
|
||||
public float COLLECTOR_ELBOW_OUT;
|
||||
public float lastSetPosShoulder;
|
||||
public float lastSetPosElbow;
|
||||
public float currentSetPosShoulder;
|
||||
public float currentSetPosElbow;
|
||||
|
||||
// hardware map
|
||||
private HardwareMap hardwareMap;
|
||||
public DcMotor frontLeft, frontRight, backLeft, backRight, lift;
|
||||
public DcMotor odometerR, odometerL, odometerA;
|
||||
public IMU imu;
|
||||
public Servo depositorShoulder, depositorElbow, collectorShoulder, collectorElbow, depositor, collector;
|
||||
private HDrive xDrive;
|
||||
private String string;
|
||||
|
||||
// robot geometry constants for odometry -----------------------------------------------------------------------------------------------
|
||||
public static double Hp = 0, Hi = 0, Hd = 0;
|
||||
public static double Xp = 0, Xi = 0, Xd = 0;
|
||||
public static double Yp = 0, Yi = 0, Yd = 0;
|
||||
private double drivePower = 1;
|
||||
|
||||
public double xMultiplier = 1;
|
||||
public double yMultiplier = 1;
|
||||
public double positionX;
|
||||
public double positionY;
|
||||
public double positionH;
|
||||
public double positionX = 0;
|
||||
public double positionY = 0;
|
||||
public double positionH = 0;
|
||||
public double xTarget;
|
||||
public double yTarget;
|
||||
public double hTarget;
|
||||
|
||||
// robot geometry constants for odometry -----------------------------------------------------------------------------------------------
|
||||
public int currentRightPosition = 0;
|
||||
public int currentLeftPosition = 0;
|
||||
public int currentAuxPosition = 0;
|
||||
public int oldRightPosition = 0;
|
||||
public int oldLeftPosition = 0;
|
||||
public int oldAuxPosition = 0;
|
||||
public double globalPositionX;
|
||||
public double globalPositionY;
|
||||
public double globalPositionH;
|
||||
public double localPositionX;
|
||||
public double localPositionY;
|
||||
public double localPositionH;
|
||||
|
||||
public double rx;
|
||||
public final static double L = 23.425; // distance between left and right encoder in cm
|
||||
final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
|
||||
public final static double R = 4.5; // wheel radius in cm
|
||||
final static double N = 8192; // encoder ticks per revolution (REV encoder)
|
||||
|
||||
public final double MaxVelocityForward = 40;
|
||||
public final double MaxStrafeVelocity = 34;
|
||||
public final double MaxRotationalVelocity = 20;
|
||||
// heading math variables for pid with imu
|
||||
public double integralSum = 0;
|
||||
public double targetHeading;
|
||||
|
||||
private double lastError = 0;
|
||||
ElapsedTime timer = new ElapsedTime();
|
||||
|
||||
|
||||
// collector / depositor variables
|
||||
private boolean lbsVar1;
|
||||
private boolean lbsVar2;
|
||||
private boolean rbsVar2;
|
||||
public float depositorPos;
|
||||
public float collectorPos;
|
||||
public double rx;
|
||||
|
||||
|
||||
|
||||
|
||||
public final double cm_per_tick = (2 * Math.PI * R) / N;
|
||||
private CyberarmEngine engine;
|
||||
@@ -108,7 +98,7 @@ public class PrototypeRobot extends Robot {
|
||||
this.string = string;
|
||||
}
|
||||
|
||||
public void initConstants(){
|
||||
public void initConstants() {
|
||||
DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value();
|
||||
DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value();
|
||||
DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value();
|
||||
@@ -119,7 +109,7 @@ public class PrototypeRobot extends Robot {
|
||||
COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value();
|
||||
COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value();
|
||||
COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
@@ -135,7 +125,9 @@ public class PrototypeRobot extends Robot {
|
||||
backRight = engine.hardwareMap.dcMotor.get("backRight");
|
||||
backLeft = engine.hardwareMap.dcMotor.get("backLeft");
|
||||
lift = engine.hardwareMap.dcMotor.get("Lift");
|
||||
odometerR = engine.hardwareMap.dcMotor.get("odometerR");
|
||||
odometerL = engine.hardwareMap.dcMotor.get("leftodo");
|
||||
odometerR = engine.hardwareMap.dcMotor.get("rightodo");
|
||||
odometerA = engine.hardwareMap.dcMotor.get("auxodo");
|
||||
|
||||
configuration = new TimeCraftersConfiguration("Blue Crab");
|
||||
|
||||
@@ -146,8 +138,14 @@ public class PrototypeRobot extends Robot {
|
||||
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
lift.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
odometerR.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
odometerL.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
odometerA.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
|
||||
odometerR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
odometerL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
odometerA.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
|
||||
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
@@ -166,8 +164,6 @@ public class PrototypeRobot extends Robot {
|
||||
//SERVO
|
||||
depositorShoulder = hardwareMap.servo.get("depositor_shoulder");
|
||||
depositorElbow = hardwareMap.servo.get("depositor_elbow");
|
||||
collectorShoulder = hardwareMap.servo.get("collector_shoulder");
|
||||
collectorElbow = hardwareMap.servo.get("collector_elbow");
|
||||
depositor = hardwareMap.servo.get("depositor");
|
||||
collector = hardwareMap.servo.get("collector");
|
||||
|
||||
@@ -176,305 +172,166 @@ public class PrototypeRobot extends Robot {
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN);
|
||||
collector.setPosition(0F);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void driveTrainTeleOp() {
|
||||
boolean lbs1 = engine.gamepad1.left_stick_button;
|
||||
if (lbs1 && !lbsVar1) {
|
||||
if (drivePower == 1) {
|
||||
drivePower = 0.5;
|
||||
} else {
|
||||
drivePower = 1;
|
||||
}
|
||||
}
|
||||
lbsVar1 = lbs1;
|
||||
|
||||
double y = -engine.gamepad1.left_stick_y;
|
||||
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
|
||||
|
||||
if (headingLock){
|
||||
;
|
||||
} else if (headingLock == false){
|
||||
rx = engine.gamepad1.right_stick_x;
|
||||
}
|
||||
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
|
||||
// angle math to make things field oriented
|
||||
double heading = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
double rotX = x * Math.cos(heading) - y * Math.sin(heading);
|
||||
double rotY = x * Math.sin(heading) + y * Math.cos(heading);
|
||||
|
||||
// joystick math to find the approximate power across each wheel for a movement
|
||||
double frontLeftPower = (rotY + rotX + rx) / denominator;
|
||||
double backLeftPower = (rotY - rotX + rx) / denominator;
|
||||
double frontRightPower = (rotY + rotX - rx) / denominator;
|
||||
double backRightPower = (rotY - rotX - rx) / denominator;
|
||||
|
||||
// setting each power determined previously from the math above
|
||||
// as well as multiplying it by a drive power that can be changed.
|
||||
backLeft.setPower(backLeftPower * drivePower);
|
||||
backRight.setPower(backRightPower * drivePower);
|
||||
frontLeft.setPower(frontLeftPower * drivePower);
|
||||
frontRight.setPower(frontRightPower * drivePower);
|
||||
}
|
||||
|
||||
public void ShoulderServoWaitTime(){
|
||||
|
||||
servoWaitTime = servoSecPerDeg * (Math.abs(lastSetPosShoulder - currentSetPosShoulder));
|
||||
|
||||
}
|
||||
|
||||
public void ElbowServoWaitTime(){
|
||||
|
||||
servoWaitTime = 1000 * (servoSecPerDeg * (Math.abs(lastSetPosElbow - currentSetPosElbow)));
|
||||
|
||||
}
|
||||
public void OdometryLocalizer(){
|
||||
|
||||
if (Math.toDegrees(positionH) > 360){
|
||||
positionH -= 360;
|
||||
}
|
||||
|
||||
globalPositionX = localPositionX;
|
||||
globalPositionY = localPositionY;
|
||||
globalPositionH = localPositionH;
|
||||
|
||||
|
||||
// update positions
|
||||
oldRightPosition = currentRightPosition;
|
||||
oldLeftPosition = currentLeftPosition;
|
||||
oldAuxPosition = currentAuxPosition;
|
||||
|
||||
currentRightPosition = -odometerR.getCurrentPosition();
|
||||
currentLeftPosition = -odometerL.getCurrentPosition();
|
||||
currentAuxPosition = odometerA.getCurrentPosition();
|
||||
|
||||
int dnl1 = currentLeftPosition - oldLeftPosition;
|
||||
int dnr2 = currentRightPosition - oldRightPosition;
|
||||
int dna3 = currentAuxPosition - oldAuxPosition;
|
||||
|
||||
// the robot has turned and moved a tiny bit between two measurements
|
||||
double dtheta = cm_per_tick * (dnr2 - dnl1) / L;
|
||||
double dx = cm_per_tick * (dnl1 + dnr2) / 2.0;
|
||||
double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L);
|
||||
|
||||
// the small movement of the bot gets added to the field coordinates
|
||||
double theta = positionH + (dtheta / 2.0);
|
||||
positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier;
|
||||
positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier;
|
||||
positionH += dtheta;
|
||||
|
||||
}
|
||||
|
||||
// public double PIDControl(double reference, double current){
|
||||
// double error = reference - current;
|
||||
// integralSum += error * timer.seconds();
|
||||
// double derivative = (error - lastError) / timer.seconds();
|
||||
//
|
||||
// timer.reset();
|
||||
//
|
||||
// double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki);
|
||||
// return output;
|
||||
// }
|
||||
|
||||
public void CollectorToggle(){
|
||||
boolean lbs2 = engine.gamepad2.left_stick_button;
|
||||
if (lbs2 && !lbsVar2) {
|
||||
if (collectorPos == 1F) {
|
||||
collectorPos = 0F;
|
||||
} else {
|
||||
collectorPos = 1F;
|
||||
}
|
||||
}
|
||||
lbsVar2 = lbs2;
|
||||
}
|
||||
public void DepositorToggle(){
|
||||
boolean rbs2 = engine.gamepad2.right_stick_button;
|
||||
if (rbs2 && !rbsVar2) {
|
||||
if (depositorPos == 0.6F) {
|
||||
depositorPos = 0F;
|
||||
} else {
|
||||
depositorPos = 0.6F;
|
||||
}
|
||||
}
|
||||
rbsVar2 = rbs2;
|
||||
}
|
||||
public void ArmSequences(){
|
||||
switch (armPosition) {
|
||||
case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos
|
||||
switch (oldArmPosition) {
|
||||
case 0:
|
||||
// transfer
|
||||
case 1:
|
||||
// driving
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
oldArmPosition = 0;
|
||||
}
|
||||
}
|
||||
case 2:
|
||||
// collect
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
oldArmPosition = 0;
|
||||
}
|
||||
}
|
||||
case 3:
|
||||
// deposit
|
||||
depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 1600) {
|
||||
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
|
||||
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
|
||||
oldArmPosition = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
public void driveTrainTeleOp () {
|
||||
boolean lbs1 = engine.gamepad1.left_stick_button;
|
||||
if (lbs1 && !lbsVar1) {
|
||||
if (drivePower == 1) {
|
||||
drivePower = 0.5;
|
||||
} else {
|
||||
drivePower = 1;
|
||||
}
|
||||
}
|
||||
lbsVar1 = lbs1;
|
||||
|
||||
// case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos
|
||||
// switch (oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
// oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// // drive pos
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2100) {
|
||||
// oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 1600) {
|
||||
// collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
|
||||
// oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos
|
||||
// switch (oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
// oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// // driving
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
// oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
// oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_ELBOW_OUT);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
|
||||
// oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
double y = -engine.gamepad1.left_stick_y;
|
||||
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
|
||||
|
||||
// case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos
|
||||
// switch (oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
// oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// // driving
|
||||
// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
|
||||
// oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
|
||||
// oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
|
||||
// angle math to make things field oriented
|
||||
double heading = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
double rotX = x * Math.cos(heading) - y * Math.sin(heading);
|
||||
double rotY = x * Math.sin(heading) + y * Math.cos(heading);
|
||||
|
||||
// joystick math to find the approximate power across each wheel for a movement
|
||||
double frontLeftPower = (rotY + rotX + rx) / denominator;
|
||||
double backLeftPower = (rotY - rotX + rx) / denominator;
|
||||
double frontRightPower = (rotY + rotX - rx) / denominator;
|
||||
double backRightPower = (rotY - rotX - rx) / denominator;
|
||||
|
||||
// setting each power determined previously from the math above
|
||||
// as well as multiplying it by a drive power that can be changed.
|
||||
backLeft.setPower(backLeftPower * drivePower);
|
||||
backRight.setPower(backRightPower * drivePower);
|
||||
frontLeft.setPower(frontLeftPower * drivePower);
|
||||
frontRight.setPower(frontRightPower * drivePower);
|
||||
}
|
||||
|
||||
public void OdometryLocalizer () {
|
||||
|
||||
// update positions
|
||||
oldRightPosition = currentRightPosition;
|
||||
oldLeftPosition = currentLeftPosition;
|
||||
oldAuxPosition = currentAuxPosition;
|
||||
|
||||
currentRightPosition = -odometerR.getCurrentPosition();
|
||||
currentLeftPosition = -odometerL.getCurrentPosition();
|
||||
currentAuxPosition = odometerA.getCurrentPosition();
|
||||
|
||||
int dnl1 = currentLeftPosition - oldLeftPosition;
|
||||
int dnr2 = currentRightPosition - oldRightPosition;
|
||||
int dna3 = currentAuxPosition - oldAuxPosition;
|
||||
|
||||
// the robot has turned and moved a tiny bit between two measurements
|
||||
double dtheta = cm_per_tick * (dnr2 - dnl1) / L;
|
||||
double dx = cm_per_tick * (dnl1 + dnr2) / 2.0;
|
||||
double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L);
|
||||
|
||||
// the small movement of the bot gets added to the field coordinates
|
||||
double theta = positionH + (dtheta / 2.0);
|
||||
positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier;
|
||||
positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier;
|
||||
positionH += dtheta;
|
||||
|
||||
}
|
||||
|
||||
public void CollectorToggle () {
|
||||
boolean lbs2 = engine.gamepad2.left_stick_button;
|
||||
if (lbs2 && !lbsVar2) {
|
||||
if (collectorPos == 1F) {
|
||||
collectorPos = 0F;
|
||||
} else {
|
||||
collectorPos = 1F;
|
||||
}
|
||||
}
|
||||
lbsVar2 = lbs2;
|
||||
}
|
||||
|
||||
public void DepositorToggle () {
|
||||
boolean rbs2 = engine.gamepad2.right_stick_button;
|
||||
if (rbs2 && !rbsVar2) {
|
||||
if (depositorPos == 0.6F) {
|
||||
depositorPos = 0F;
|
||||
} else {
|
||||
depositorPos = 0.6F;
|
||||
}
|
||||
}
|
||||
rbsVar2 = rbs2;
|
||||
}
|
||||
|
||||
public double angleWrap ( double radians){
|
||||
while (radians > Math.PI) {
|
||||
radians -= 2 * Math.PI;
|
||||
}
|
||||
while (radians < -Math.PI) {
|
||||
radians += 2 * Math.PI;
|
||||
}
|
||||
return radians;
|
||||
}
|
||||
|
||||
public double HeadingPIDControl ( double reference, double current){
|
||||
double error = angleWrap(current - reference);
|
||||
integralSum += error * timer.seconds();
|
||||
double derivative = (error - lastError) / timer.seconds();
|
||||
|
||||
timer.reset();
|
||||
|
||||
double output = (error * Hp) + (derivative * Hd) + (integralSum * Hi);
|
||||
return output;
|
||||
}
|
||||
|
||||
public double XPIDControl ( double reference, double current){
|
||||
double error = (current - reference);
|
||||
integralSum += error * timer.seconds();
|
||||
double derivative = (error - lastError) / timer.seconds();
|
||||
|
||||
timer.reset();
|
||||
|
||||
double output = (error * Xp) + (derivative * Xd) + (integralSum * Xi);
|
||||
return output;
|
||||
}
|
||||
|
||||
public double YPIDControl ( double reference, double current){
|
||||
double error = (current - reference);
|
||||
integralSum += error * timer.seconds();
|
||||
double derivative = (error - lastError) / timer.seconds();
|
||||
|
||||
timer.reset();
|
||||
|
||||
double output = (error * Yp) + (derivative * Yd) + (integralSum * Yi);
|
||||
return output;
|
||||
}
|
||||
|
||||
public void DriveToCoordinates () {
|
||||
|
||||
// determine the velocities needed for each direction
|
||||
// this uses PID to adjust needed Power for robot to move to target
|
||||
double heading = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
|
||||
double pidX = XPIDControl(xTarget, positionX);
|
||||
double pidY = YPIDControl(yTarget, positionY);
|
||||
|
||||
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
|
||||
|
||||
// field oriented math, (rotating the global field to the relative field)
|
||||
double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading);
|
||||
double rotX = pidY * Math.sin(heading) + pidX * Math.cos(heading);
|
||||
|
||||
|
||||
// finding approximate power for each wheel.
|
||||
double frontLeftPower = (rotY + rotX + rx) / denominator;
|
||||
double backLeftPower = (rotY - rotX + rx) / denominator;
|
||||
double frontRightPower = (rotY + rotX - rx) / denominator;
|
||||
double backRightPower = (rotY - rotX - rx) / denominator;
|
||||
|
||||
// apply my powers
|
||||
frontLeft.setPower(frontLeftPower);
|
||||
backLeft.setPower(backLeftPower);
|
||||
backRight.setPower(backRightPower);
|
||||
frontRight.setPower(frontRightPower);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,168 @@
|
||||
package org.timecrafters.CenterStage.TeleOp.States;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
@Config
|
||||
|
||||
public class CompetitionTeleOpState extends CyberarmState {
|
||||
private PrototypeRobot robot;
|
||||
private int maxExtension = 2000;
|
||||
private int minExtension = 0;
|
||||
public double integralSum = 0;
|
||||
private double targetHeading;
|
||||
|
||||
public double power;
|
||||
private double currentHeading;
|
||||
private boolean headingLock = false;
|
||||
|
||||
public static double Kp = 0;
|
||||
public static double Ki = 0;
|
||||
public static double Kd = 0;
|
||||
private double lastError = 0;
|
||||
ElapsedTime timer = new ElapsedTime();
|
||||
|
||||
|
||||
private long lastCheckedTime;
|
||||
|
||||
public CompetitionTeleOpState(PrototypeRobot robot) {
|
||||
this.robot = robot;
|
||||
|
||||
}
|
||||
|
||||
// }
|
||||
// --------------------------------------------------------------------------------------------------------- Slider control function
|
||||
private void SliderTeleOp() {
|
||||
if (engine.gamepad2.right_trigger != 0) {
|
||||
if (robot.lift.getCurrentPosition() >= maxExtension) {
|
||||
robot.lift.setPower(0);
|
||||
} else if (robot.lift.getCurrentPosition() >= maxExtension - 200) {
|
||||
robot.lift.setPower(0.35);
|
||||
} else {
|
||||
robot.lift.setPower(engine.gamepad2.right_trigger);
|
||||
}
|
||||
} else if (engine.gamepad2.left_trigger != 0) {
|
||||
|
||||
if (robot.lift.getCurrentPosition() <= minExtension) {
|
||||
robot.lift.setPower(0);
|
||||
} else if (robot.lift.getCurrentPosition() < 350) {
|
||||
robot.lift.setPower(-0.3);
|
||||
} else {
|
||||
robot.lift.setPower(-engine.gamepad2.left_trigger);
|
||||
}
|
||||
} else {
|
||||
robot.lift.setPower(0);
|
||||
}
|
||||
}
|
||||
public double angleWrap(double radians) {
|
||||
while (radians > Math.PI) {
|
||||
radians -= 2 * Math.PI;
|
||||
}
|
||||
while (radians < -Math.PI){
|
||||
radians += 2 * Math.PI;
|
||||
}
|
||||
return radians;
|
||||
}
|
||||
|
||||
public double HeadingPIDControl(double reference, double current){
|
||||
double error = angleWrap(current - reference);
|
||||
integralSum += error * timer.seconds();
|
||||
double derivative = (error - lastError) / timer.seconds();
|
||||
|
||||
timer.reset();
|
||||
|
||||
double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki);
|
||||
return output;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
|
||||
if (engine.gamepad1.right_stick_button) {
|
||||
robot.imu.resetYaw();
|
||||
}
|
||||
|
||||
if (headingLock){
|
||||
robot.rx = HeadingPIDControl(targetHeading, currentHeading);
|
||||
} else {
|
||||
robot.rx = engine.gamepad1.right_stick_x;
|
||||
|
||||
}
|
||||
|
||||
// drivetrain
|
||||
robot.driveTrainTeleOp();
|
||||
|
||||
if (engine.gamepad1.b){
|
||||
headingLock = true;
|
||||
targetHeading = robot.backDropLock;
|
||||
}
|
||||
if (engine.gamepad1.x){
|
||||
headingLock = true;
|
||||
targetHeading = robot.collectLock;
|
||||
}
|
||||
if (engine.gamepad1.a){
|
||||
headingLock = true;
|
||||
targetHeading = currentHeading;
|
||||
}
|
||||
if (engine.gamepad1.right_stick_x != 0){
|
||||
headingLock = false;
|
||||
}
|
||||
|
||||
// if (engine.gamepad2.a) {
|
||||
// robot.armPosition = 0;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// } else if (engine.gamepad2.x) {
|
||||
// robot.armPosition = 1;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// } else if (engine.gamepad2.b) {
|
||||
// robot.armPosition = 2;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// } else if (engine.gamepad2.y) {
|
||||
// robot.armPosition = 3;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// }
|
||||
//
|
||||
// robot.depositor.setPosition(robot.depositorPos);
|
||||
// robot.collector.setPosition(robot.collectorPos);
|
||||
//
|
||||
//
|
||||
// // drivetrain
|
||||
// robot.driveTrainTeleOp();
|
||||
// lift
|
||||
// SliderTeleOp();
|
||||
// collector depositor
|
||||
// robot.CollectorToggle();
|
||||
// depositor toggle
|
||||
// robot.DepositorToggle();
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry () {
|
||||
engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition());
|
||||
engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
|
||||
engine.telemetry.addData("arm Pos", robot.armPosition);
|
||||
engine.telemetry.addData("old arm pos", robot.oldArmPosition);
|
||||
engine.telemetry.addData("depositor pos", robot.depositorPos);
|
||||
engine.telemetry.addData("collector pos", robot.collectorPos);
|
||||
engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime);
|
||||
engine.telemetry.addData("pid power", power);
|
||||
engine.telemetry.addData("heading Lock?", headingLock);
|
||||
engine.telemetry.addData("Kp", Kp);
|
||||
engine.telemetry.addData("Ki", Ki);
|
||||
engine.telemetry.addData("Kd", Kd);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
package org.timecrafters.CenterStage.TeleOp.States;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
@@ -7,18 +8,26 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
@Config
|
||||
|
||||
public class PrototypeRobotDrivetrainState extends CyberarmState {
|
||||
private PrototypeRobot robot;
|
||||
private int maxExtension = 2000;
|
||||
private int minExtension = 0;
|
||||
public double integralSum = 0;
|
||||
double Kp = 0;
|
||||
double Ki = 0;
|
||||
double Kd = 0;
|
||||
private double targetHeading;
|
||||
|
||||
private double lastError = 0;
|
||||
ElapsedTime timer = new ElapsedTime();
|
||||
|
||||
public double power;
|
||||
private double currentHeading;
|
||||
private boolean headingLock = false;
|
||||
|
||||
public static double Kp = 0;
|
||||
public static double Ki = 0;
|
||||
public static double Kd = 0;
|
||||
|
||||
|
||||
private long lastCheckedTime;
|
||||
|
||||
@@ -61,12 +70,8 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
|
||||
return radians;
|
||||
}
|
||||
|
||||
private void HeadingLock(){
|
||||
double currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
|
||||
|
||||
}
|
||||
public double HeadingPIDControl(double reference, double current){
|
||||
double error = angleWrap(reference - current);
|
||||
double error = angleWrap(current - reference);
|
||||
integralSum += error * timer.seconds();
|
||||
double derivative = (error - lastError) / timer.seconds();
|
||||
|
||||
@@ -83,36 +88,64 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
|
||||
if (engine.gamepad1.right_stick_button) {
|
||||
robot.imu.resetYaw();
|
||||
}
|
||||
|
||||
if (engine.gamepad2.a) {
|
||||
robot.armPosition = 0;
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
} else if (engine.gamepad2.x) {
|
||||
robot.armPosition = 1;
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
} else if (engine.gamepad2.b) {
|
||||
robot.armPosition = 2;
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
} else if (engine.gamepad2.y) {
|
||||
robot.armPosition = 3;
|
||||
robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
if (headingLock){
|
||||
robot.rx = HeadingPIDControl(targetHeading, currentHeading);
|
||||
} else {
|
||||
robot.rx = engine.gamepad1.right_stick_x;
|
||||
|
||||
}
|
||||
|
||||
robot.depositor.setPosition(robot.depositorPos);
|
||||
robot.collector.setPosition(robot.collectorPos);
|
||||
// drivetrain
|
||||
robot.driveTrainTeleOp();
|
||||
|
||||
if (engine.gamepad1.b){
|
||||
headingLock = true;
|
||||
targetHeading = robot.backDropLock;
|
||||
}
|
||||
if (engine.gamepad1.x){
|
||||
headingLock = true;
|
||||
targetHeading = robot.collectLock;
|
||||
}
|
||||
if (engine.gamepad1.a){
|
||||
headingLock = true;
|
||||
targetHeading = currentHeading;
|
||||
}
|
||||
if (engine.gamepad1.right_stick_x != 0){
|
||||
headingLock = false;
|
||||
}
|
||||
|
||||
// drivetrain
|
||||
robot.driveTrainTeleOp();
|
||||
// if (engine.gamepad2.a) {
|
||||
// robot.armPosition = 0;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// } else if (engine.gamepad2.x) {
|
||||
// robot.armPosition = 1;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// } else if (engine.gamepad2.b) {
|
||||
// robot.armPosition = 2;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// } else if (engine.gamepad2.y) {
|
||||
// robot.armPosition = 3;
|
||||
// robot.startOfSequencerTime = System.currentTimeMillis();
|
||||
// }
|
||||
//
|
||||
// robot.depositor.setPosition(robot.depositorPos);
|
||||
// robot.collector.setPosition(robot.collectorPos);
|
||||
//
|
||||
//
|
||||
// // drivetrain
|
||||
// robot.driveTrainTeleOp();
|
||||
// lift
|
||||
SliderTeleOp();
|
||||
// SliderTeleOp();
|
||||
// collector depositor
|
||||
robot.CollectorToggle();
|
||||
// robot.CollectorToggle();
|
||||
// depositor toggle
|
||||
robot.DepositorToggle();
|
||||
// robot.DepositorToggle();
|
||||
|
||||
|
||||
}
|
||||
@@ -126,6 +159,14 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
|
||||
engine.telemetry.addData("depositor pos", robot.depositorPos);
|
||||
engine.telemetry.addData("collector pos", robot.collectorPos);
|
||||
engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime);
|
||||
engine.telemetry.addData("heading Lock?", headingLock);
|
||||
engine.telemetry.addData("Kp", Kp);
|
||||
engine.telemetry.addData("Ki", Ki);
|
||||
engine.telemetry.addData("Kd", Kd);
|
||||
engine.telemetry.addData("front left motor", robot.frontLeft.getCurrentPosition());
|
||||
engine.telemetry.addData("front right motor", robot.frontRight.getCurrentPosition());
|
||||
engine.telemetry.addData("back left motor", robot.backLeft.getCurrentPosition());
|
||||
engine.telemetry.addData("back right motor", robot.backRight.getCurrentPosition());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -33,13 +33,10 @@ public class XDrivetrainRobotState extends CyberarmState {
|
||||
private double ArmlastError = 0;
|
||||
private boolean headingLock = false;
|
||||
|
||||
private long lastCheckedTime;
|
||||
|
||||
public XDrivetrainRobotState(XDrivetrainBot robot) {
|
||||
this.robot = robot;
|
||||
|
||||
}
|
||||
|
||||
// }
|
||||
// --------------------------------------------------------------------------------------------------------- Slider control function
|
||||
public double angleWrap(double radians) {
|
||||
@@ -149,7 +146,7 @@ public class XDrivetrainRobotState extends CyberarmState {
|
||||
engine.telemetry.addData("heading lock?", headingLock);
|
||||
engine.telemetry.addData("pid heading power", HeadingPIDControl(targetHeading, currentHeading));
|
||||
engine.telemetry.addData("robot arm current pos ", robot.armMotor.getCurrentPosition());
|
||||
engine.telemetry.addData("arm pid power ", power);
|
||||
engine.telemetry.addData("arm pid power ", HeadingPIDControl(targetHeading, currentHeading));
|
||||
engine.telemetry.addData("p", ArmP);
|
||||
engine.telemetry.addData("i", ArmI);
|
||||
engine.telemetry.addData("d", ArmD);
|
||||
|
||||
@@ -54,7 +54,7 @@ public class headingLockTeleOp extends CyberarmState {
|
||||
timer.reset();
|
||||
|
||||
double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki);
|
||||
robot.PIDrx = output;
|
||||
// robot.PIDrx = output;
|
||||
}
|
||||
|
||||
// drivetrain
|
||||
@@ -82,7 +82,7 @@ public class headingLockTeleOp extends CyberarmState {
|
||||
public void telemetry () {
|
||||
engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
|
||||
engine.telemetry.addData("rx", robot.rx);
|
||||
engine.telemetry.addData("PIDrx", robot.PIDrx);
|
||||
// engine.telemetry.addData("PIDrx", robot.PIDrx);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user