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

finished Heading Lock, and started adding the PID controller for the robot to move the arm

Browse Source
This commit is contained in:
SpencerPiha
2023-11-30 20:32:26 -06:00
parent 77fc170ca7
commit d9ca39cbef
4 changed files with 25 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/XDrivetrainBot.java
View File

@@ -17,10 +17,9 @@ 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);
public double collectLock = Math.toRadians(-90);
ElapsedTime timer = new ElapsedTime();
public int armPosition = 0;
public boolean stateFinished;
@@ -28,7 +27,7 @@ public class XDrivetrainBot extends Robot {
public int oldArmPosition = 0;
public long waitTime;
private HardwareMap hardwareMap;
public DcMotor frontLeft, frontRight, backLeft, backRight;
public DcMotor frontLeft, frontRight, backLeft, backRight, armMotor;
public DcMotor odometerR, odometerL, odometerA;
public IMU imu;
private String string;
@@ -69,9 +68,6 @@ public class XDrivetrainBot extends Robot {
public float collectorPos;
public double rx;
public final double cm_per_tick = (2 * Math.PI * R) / N;
private CyberarmEngine engine;
@@ -96,6 +92,7 @@ public class XDrivetrainBot extends Robot {
frontLeft = engine.hardwareMap.dcMotor.get("frontLeft");
backRight = engine.hardwareMap.dcMotor.get("backRight");
backLeft = engine.hardwareMap.dcMotor.get("backLeft");
armMotor = engine.hardwareMap.dcMotor.get("arm");
// configuration = new TimeCraftersConfiguration("Blue Crab");
@@ -105,6 +102,7 @@ public class XDrivetrainBot extends Robot {
backRight.setDirection(DcMotorSimple.Direction.FORWARD);
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
armMotor.setDirection(DcMotorSimple.Direction.FORWARD);
@@ -113,6 +111,7 @@ public class XDrivetrainBot extends Robot {
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
//IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
@@ -193,19 +192,4 @@ public class XDrivetrainBot extends Robot {
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;
// }
}

4
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java
View File

@@ -12,8 +12,6 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
private PrototypeRobot robot;
private int maxExtension = 2000;
private int minExtension = 0;
private PIDController HeadingPidController;
private double targetHeading;
public double integralSum = 0;
double Kp = 0;
double Ki = 0;
@@ -111,7 +109,7 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
robot.driveTrainTeleOp();
if (engine.gamepad1.b){
targetHeading = robot.backDropLock;
// targetHeading = robot.backDropLock;
}
// lift

21
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java
View File

@@ -12,11 +12,12 @@ import dev.cyberarm.engine.V2.CyberarmState;
@Config
public class XDrivetrainRobotState extends CyberarmState {
private XDrivetrainBot robot;
private PIDController HeadingPidController;
private PIDController ArmPidController;
public static double ArmP, ArmI, ArmD;
private double targetHeading;
private double currentHeading;
public double integralSum = 0;
public static double Kp = 0;
public static double Kp = 1;
public static double Ki = 0;
public static double Kd = 0;
ElapsedTime timer = new ElapsedTime();
@@ -27,6 +28,8 @@ public class XDrivetrainRobotState extends CyberarmState {
public XDrivetrainRobotState(XDrivetrainBot robot) {
this.robot = robot;
ArmPidController = new PIDController(ArmP, ArmI, ArmD);
}
@@ -42,8 +45,12 @@ public class XDrivetrainRobotState extends CyberarmState {
return radians;
}
public void ArmControl(){
}
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();
@@ -78,6 +85,14 @@ public class XDrivetrainRobotState extends CyberarmState {
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;
}

13
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java
View File

@@ -15,7 +15,7 @@ public class headingLockTeleOp extends CyberarmState {
private PrototypeRobot robot;
private PIDController HeadingPidController;
public double integralSum = 0;
public static double Kp = 0;
public static double Kp = 1;
public static double Ki = 0;
public static double Kd = 0;
private double lastError = 0;
@@ -38,17 +38,6 @@ public class headingLockTeleOp extends CyberarmState {
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() {
robot.headingLock = false;