mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-15 16:52:35 +00:00
tested code for PID heading lock in tele op.. got the robot to move
This commit is contained in:
SpencerPiha
@@ -0,0 +1,211 @@
|
||||
package org.timecrafters.CenterStage.Common;
|
||||
|
||||
import com.arcrobotics.ftclib.drivebase.HDrive;
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
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.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.Library.Robot;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
|
||||
public class XDrivetrainBot extends Robot {
|
||||
public double PIDrx;
|
||||
public double targetHeading;
|
||||
public boolean headingLock = false;
|
||||
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;
|
||||
private HardwareMap hardwareMap;
|
||||
public DcMotor frontLeft, frontRight, backLeft, backRight;
|
||||
public DcMotor odometerR, odometerL, odometerA;
|
||||
public IMU imu;
|
||||
private String string;
|
||||
private double drivePower = 1;
|
||||
|
||||
public double xMultiplier = 1;
|
||||
public double yMultiplier = 1;
|
||||
public double positionX;
|
||||
public double positionY;
|
||||
public double positionH;
|
||||
|
||||
// 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 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;
|
||||
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;
|
||||
|
||||
public TimeCraftersConfiguration configuration;
|
||||
|
||||
public XDrivetrainBot(String string) {
|
||||
this.engine = engine;
|
||||
setup();
|
||||
this.string = string;
|
||||
}
|
||||
|
||||
@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");
|
||||
|
||||
|
||||
// configuration = new TimeCraftersConfiguration("Blue Crab");
|
||||
|
||||
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
backRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
|
||||
|
||||
|
||||
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
//IMU
|
||||
IMU.Parameters parameters = new IMU.Parameters(
|
||||
new RevHubOrientationOnRobot(
|
||||
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.UP));
|
||||
|
||||
imu.initialize(parameters);
|
||||
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
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(){
|
||||
|
||||
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;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package org.timecrafters.CenterStage.TeleOp.Engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
import org.timecrafters.CenterStage.Common.XDrivetrainBot;
|
||||
import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState;
|
||||
import org.timecrafters.CenterStage.TeleOp.States.XDrivetrainRobotState;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
@TeleOp(name = "PID Heading Lock Robot", group = "PROTOTYPE")
|
||||
public class XDriveTrainRobotEngine extends CyberarmEngine {
|
||||
private XDrivetrainBot robot;
|
||||
@Override
|
||||
public void setup() {
|
||||
this.robot = new XDrivetrainBot("Hello World");
|
||||
this.robot.setup();
|
||||
|
||||
addState(new XDrivetrainRobotState(robot));
|
||||
}
|
||||
}
|
||||
@@ -107,192 +107,6 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
|
||||
robot.collector.setPosition(robot.collectorPos);
|
||||
|
||||
|
||||
// arm sequencer
|
||||
// robot.ArmSequences();
|
||||
// switch (robot.armPosition) {
|
||||
// case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos
|
||||
// switch (robot.oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// robot.oldArmPosition = 0;
|
||||
// break;
|
||||
// case 1:
|
||||
// // driving
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
// robot.oldArmPosition = 0;
|
||||
// }
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
// robot.oldArmPosition = 0;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the out position
|
||||
// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) {
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { // wait to move till time is met
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) {
|
||||
// robot.oldArmPosition = 0;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
// case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos
|
||||
// switch (robot.oldArmPosition) {
|
||||
// case 0:
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
// robot.oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// // drive pos
|
||||
// robot.oldArmPosition = 1;
|
||||
//
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 900) { // wait to move till time is met
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
// robot.oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT - 0.1); // drive the shoulder to the out position
|
||||
// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met
|
||||
// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) {
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
// robot.oldArmPosition = 1;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos
|
||||
// switch (robot.oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) {
|
||||
// robot.oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// // driving
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
// robot.oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) {
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 700) {
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1000) {
|
||||
// robot.oldArmPosition = 2;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos
|
||||
// switch (robot.oldArmPosition) {
|
||||
// case 0:
|
||||
// // transfer
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) {
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){
|
||||
// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){
|
||||
// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){
|
||||
// robot.oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 1:
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 760) { // wait to move till time is met
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) {
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){
|
||||
// robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){
|
||||
// robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){
|
||||
// robot.oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 2:
|
||||
// // collect
|
||||
// robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met
|
||||
// robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
|
||||
// if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) {
|
||||
// robot.oldArmPosition = 3;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case 3:
|
||||
// // deposit
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
|
||||
// }
|
||||
|
||||
// drivetrain
|
||||
robot.driveTrainTeleOp();
|
||||
|
||||
|
||||
@@ -0,0 +1,99 @@
|
||||
package org.timecrafters.CenterStage.TeleOp.States;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
import org.timecrafters.CenterStage.Common.XDrivetrainBot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
@Config
|
||||
public class XDrivetrainRobotState extends CyberarmState {
|
||||
private XDrivetrainBot robot;
|
||||
private PIDController HeadingPidController;
|
||||
private double targetHeading;
|
||||
private double currentHeading;
|
||||
public double integralSum = 0;
|
||||
public static double Kp = 0;
|
||||
public static double Ki = 0;
|
||||
public static double Kd = 0;
|
||||
ElapsedTime timer = new ElapsedTime();
|
||||
private double lastError = 0;
|
||||
private boolean headingLock = false;
|
||||
|
||||
private long lastCheckedTime;
|
||||
|
||||
public XDrivetrainRobotState(XDrivetrainBot robot) {
|
||||
this.robot = robot;
|
||||
|
||||
}
|
||||
|
||||
// }
|
||||
// --------------------------------------------------------------------------------------------------------- Slider control function
|
||||
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(reference - current);
|
||||
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.right_stick_x != 0){
|
||||
headingLock = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry () {
|
||||
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("heading lock?", headingLock);
|
||||
engine.telemetry.addData("pid heading power", HeadingPIDControl(targetHeading, currentHeading));
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user