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

Worked On odometry localization and drive to coordinates

Browse Source
This commit is contained in:
SpencerPiha
2023-10-14 13:01:47 -05:00
parent 3261fd18a7
commit cc8c46a6b3
3 changed files with 152 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java
View File

@@ -7,6 +7,8 @@ import dev.cyberarm.engine.V2.CyberarmState;
public class AutoStateSample extends CyberarmState {
private final boolean stateDisabled;
private double newX;
private double newY;
PrototypeRobot robot;
public AutoStateSample(PrototypeRobot robot, String groupName, String actionName) {
this.robot = robot;
@@ -20,9 +22,6 @@ public class AutoStateSample extends CyberarmState {
@Override
public void exec() {
// do stuff
// -------------
// end:
setHasFinished(true);
}
}

82
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java Normal file
View File

@@ -0,0 +1,82 @@
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)
}
}

70
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
View File

@@ -1,9 +1,7 @@
package org.timecrafters.CenterStage.Common;
import com.arcrobotics.ftclib.drivebase.HDrive;
import com.arcrobotics.ftclib.hardware.RevIMU;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -29,10 +27,42 @@ public class PrototypeRobot extends Robot {
public float currentSetPosElbow;
private HardwareMap hardwareMap;
public MotorEx frontLeft, frontRight, backLeft, backRight, lift;
public DcMotor odometerR, odometerL, odometerA;
public IMU imu;
public Servo depositorShoulder, depositorElbow, depositor;
private HDrive xDrive;
private String string;
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;
public final double cm_per_tick = (2 * Math.PI * R) / N;
private CyberarmEngine engine;
public TimeCraftersConfiguration configuration;
@@ -118,4 +148,40 @@ public class PrototypeRobot extends Robot {
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;
}
}