mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
sum changes
This commit is contained in:
aarav18
@@ -197,10 +197,10 @@ public final class Constants {
|
||||
public static final int LEFT_CLAW_ID = 44;
|
||||
public static final int RIGHT_CLAW_ID = 45;
|
||||
|
||||
public static final double OPEN_POSITION = 0;
|
||||
public static final double CLOSE_POSITION = 0;
|
||||
public static final double OPEN_POSITION = 0; // TODO: find actual position
|
||||
public static final double CLOSE_POSITION = 0; // TODO: find actual position
|
||||
|
||||
public static final double THRESHOLD = 0;
|
||||
public static final double THRESHOLD = 0; // TODO: find actual threshold
|
||||
|
||||
public static final double CALIBRATION_SPEED = 0;
|
||||
|
||||
|
||||
@@ -57,7 +57,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.subsystems.Claws;
|
||||
import frc4388.robot.commands.RunClaw;
|
||||
import frc4388.robot.subsystems.Climber;
|
||||
import frc4388.robot.subsystems.ClimberRewrite;
|
||||
import frc4388.robot.subsystems.Claws.ClawType;
|
||||
import frc4388.robot.commands.AimToCenter;
|
||||
import frc4388.robot.commands.Shoot;
|
||||
@@ -92,7 +92,7 @@ public class RobotContainer {
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
|
||||
private final ClimberRewrite m_robotClimber= new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
|
||||
|
||||
private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
|
||||
|
||||
|
||||
@@ -8,6 +8,7 @@ package frc4388.robot;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
@@ -63,7 +64,7 @@ public class RobotMap {
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
|
||||
public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID);
|
||||
public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule leftBack;
|
||||
@@ -171,8 +172,10 @@ public class RobotMap {
|
||||
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
|
||||
|
||||
/* Hooks Subsystem */
|
||||
public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless);
|
||||
public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless);
|
||||
// public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless);
|
||||
// public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless);
|
||||
public final Servo leftClaw = new Servo(1); // TODO: find actual channel
|
||||
public final Servo rightClaw = new Servo(1); // TODO: find actual channel
|
||||
|
||||
// Shooter Config
|
||||
/* Boom Boom Subsystem */
|
||||
|
||||
@@ -47,6 +47,7 @@ public class RunClaw extends CommandBase {
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_claws.checkSwitchAndCurrent(clawType);
|
||||
// return m_claws.checkSwitchAndCurrent(clawType);
|
||||
return false; // TODO: real return
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,18 +7,23 @@ import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.SparkMaxLimitSwitch;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.Servo;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.ClawConstants;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
|
||||
public class Claws extends SubsystemBase {
|
||||
public CANSparkMax m_leftClaw;
|
||||
public CANSparkMax m_rightClaw;
|
||||
|
||||
private SparkMaxLimitSwitch m_leftLimitSwitchF;
|
||||
private SparkMaxLimitSwitch m_rightLimitSwitchF;
|
||||
private SparkMaxLimitSwitch m_leftLimitSwitchR;
|
||||
private SparkMaxLimitSwitch m_rightLimitSwitchR;
|
||||
public Servo m_leftClaw;
|
||||
public Servo m_rightClaw;
|
||||
|
||||
// public CANSparkMax m_leftClaw;
|
||||
// public CANSparkMax m_rightClaw;
|
||||
|
||||
// private SparkMaxLimitSwitch m_leftLimitSwitchF;
|
||||
// private SparkMaxLimitSwitch m_rightLimitSwitchF;
|
||||
// private SparkMaxLimitSwitch m_leftLimitSwitchR;
|
||||
// private SparkMaxLimitSwitch m_rightLimitSwitchR;
|
||||
|
||||
private double m_leftOffset;
|
||||
private double m_rightOffset;
|
||||
@@ -26,21 +31,22 @@ public class Claws extends SubsystemBase {
|
||||
private boolean m_open;
|
||||
public static enum ClawType {LEFT, RIGHT}
|
||||
|
||||
public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) {
|
||||
public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) {
|
||||
m_leftClaw = leftClaw;
|
||||
m_rightClaw = rightClaw;
|
||||
|
||||
m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol
|
||||
m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
// m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
// m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
// m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol
|
||||
// m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
|
||||
m_leftLimitSwitchF.enableLimitSwitch(true);
|
||||
m_rightLimitSwitchF.enableLimitSwitch(true);
|
||||
m_leftLimitSwitchR.enableLimitSwitch(true);
|
||||
m_rightLimitSwitchR.enableLimitSwitch(true);
|
||||
leftClaw.setInverted(true);
|
||||
rightClaw.setInverted(true);
|
||||
// m_leftLimitSwitchF.enableLimitSwitch(true);
|
||||
// m_rightLimitSwitchF.enableLimitSwitch(true);
|
||||
// m_leftLimitSwitchR.enableLimitSwitch(true);
|
||||
// m_rightLimitSwitchR.enableLimitSwitch(true);
|
||||
|
||||
// m_leftClaw.setInverted(true);
|
||||
// m_rightClaw.setInverted(true);
|
||||
|
||||
m_open = false;
|
||||
|
||||
@@ -48,11 +54,6 @@ public class Claws extends SubsystemBase {
|
||||
// m_rightClaw.set(ClawConstants.CALIBRATION_SPEED);
|
||||
}
|
||||
|
||||
public void setSpeed(double speed){
|
||||
m_leftClaw.set(speed);
|
||||
m_rightClaw.set(speed);
|
||||
}
|
||||
|
||||
/**
|
||||
* Run a specific claw to open or close.
|
||||
* @param which Which claw to run.
|
||||
@@ -66,38 +67,33 @@ public class Claws extends SubsystemBase {
|
||||
|
||||
// double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset;
|
||||
// m_leftClaw.getEncoder().setPosition(setPos);
|
||||
m_leftClaw.set(direction * 0.1);
|
||||
m_leftClaw.setSpeed(direction * 0.1);
|
||||
|
||||
} else if (which == Claws.ClawType.RIGHT) {
|
||||
|
||||
// double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset;
|
||||
// m_rightClaw.getEncoder().setPosition(setPos);
|
||||
m_rightClaw.set(direction * 0.1);
|
||||
m_rightClaw.setSpeed(direction * 0.1);
|
||||
}
|
||||
|
||||
m_open = open;
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void runClaw(ClawType which, double input) {
|
||||
|
||||
|
||||
if (which == Claws.ClawType.LEFT) {
|
||||
m_leftClaw.set(input);
|
||||
m_leftClaw.setSpeed(input);
|
||||
|
||||
} else if (which == Claws.ClawType.RIGHT) {
|
||||
m_rightClaw.set(input);
|
||||
m_rightClaw.setSpeed(input);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void runClaws(double input)
|
||||
{
|
||||
m_leftClaw.set(input);
|
||||
m_rightClaw.set(input);
|
||||
m_leftClaw.setSpeed(input);
|
||||
m_rightClaw.setSpeed(input);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the state of both hooks
|
||||
* @param open The state of the hooks
|
||||
@@ -114,8 +110,6 @@ public class Claws extends SubsystemBase {
|
||||
m_leftClaw.set(-0.1);
|
||||
m_rightClaw.set(-0.1);
|
||||
}
|
||||
|
||||
// m_open = open;
|
||||
}
|
||||
|
||||
public double[] getOffsets() {
|
||||
@@ -123,12 +117,12 @@ public class Claws extends SubsystemBase {
|
||||
}
|
||||
|
||||
public boolean fullyOpen() {
|
||||
return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
|
||||
Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
|
||||
return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
|
||||
Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
|
||||
|
||||
public boolean fullyClosed() {
|
||||
return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
|
||||
Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
|
||||
return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
|
||||
Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -137,26 +131,33 @@ public class Claws extends SubsystemBase {
|
||||
* @param limit The current limit.
|
||||
* @return Whether to interrupt the RunClaw command or not.
|
||||
*/
|
||||
public boolean checkSwitchAndCurrent(ClawType which) {
|
||||
if (which == ClawType.LEFT) {
|
||||
if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
else if (which == ClawType.RIGHT) {
|
||||
if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// public boolean checkSwitchAndCurrent(ClawType which) {
|
||||
// if (which == ClawType.LEFT) {
|
||||
// if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
|
||||
// return true;
|
||||
// }
|
||||
// }
|
||||
// else if (which == ClawType.RIGHT) {
|
||||
// if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
|
||||
// return true;
|
||||
// }
|
||||
// }
|
||||
// return false;
|
||||
// }
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
|
||||
m_leftOffset = m_leftClaw.getEncoder().getPosition();
|
||||
if (fullyOpen() || fullyClosed()) {
|
||||
m_leftClaw.setSpeed(0.0);
|
||||
m_rightClaw.setSpeed(0.0);
|
||||
}
|
||||
|
||||
// if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
|
||||
// // m_leftOffset = m_leftClaw.getEncoder().getPosition();
|
||||
// m_leftOffset = m_leftClaw.getPosition();
|
||||
|
||||
if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
|
||||
m_rightOffset = m_rightClaw.getEncoder().getPosition();
|
||||
// if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
|
||||
// // m_rightOffset = m_rightClaw.getEncoder().getPosition();
|
||||
// m_rightOffset = m_rightClaw.getPosition();
|
||||
}
|
||||
}
|
||||
@@ -1,279 +1,279 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
// // Copyright (c) FIRST and other WPILib contributors.
|
||||
// // Open Source Software; you can modify and/or share it under the terms of
|
||||
// // the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
/*
|
||||
Safety
|
||||
Hooks
|
||||
Add
|
||||
*/
|
||||
// /*
|
||||
// Safety
|
||||
// Hooks
|
||||
// Add
|
||||
// */
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
// package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
// import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
// import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
// import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
|
||||
import org.opencv.core.Point;
|
||||
// import org.opencv.core.Point;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
// import edu.wpi.first.math.Matrix;
|
||||
// import edu.wpi.first.math.geometry.Rotation2d;
|
||||
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
// import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
// import frc4388.robot.Constants;
|
||||
// import frc4388.robot.Constants.ClimberConstants;
|
||||
|
||||
public class Climber extends SubsystemBase {
|
||||
public WPI_TalonFX m_shoulder;
|
||||
public WPI_TalonFX m_elbow;
|
||||
// public class Climber extends SubsystemBase {
|
||||
// public WPI_TalonFX m_shoulder;
|
||||
// public WPI_TalonFX m_elbow;
|
||||
|
||||
private double m_shoulderOffset;
|
||||
private double m_elbowOffset;
|
||||
// private double m_shoulderOffset;
|
||||
// private double m_elbowOffset;
|
||||
|
||||
private WPI_PigeonIMU m_gyro;
|
||||
private boolean m_groundRelative;
|
||||
private double m_robotAngle;
|
||||
private double m_robotPosition;
|
||||
// private WPI_PigeonIMU m_gyro;
|
||||
// private boolean m_groundRelative;
|
||||
// private double m_robotAngle;
|
||||
// private double m_robotPosition;
|
||||
|
||||
private Point m_position = new Point(ClimberConstants.MIN_ARM_LENGTH, 0.d);
|
||||
// private Point m_position = new Point(ClimberConstants.MIN_ARM_LENGTH, 0.d);
|
||||
|
||||
public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) {
|
||||
m_shoulder = shoulder;
|
||||
m_shoulder.configFactoryDefault();
|
||||
m_shoulder.setNeutralMode(NeutralMode.Brake);
|
||||
// public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) {
|
||||
// m_shoulder = shoulder;
|
||||
// m_shoulder.configFactoryDefault();
|
||||
// m_shoulder.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
m_elbow = elbow;
|
||||
m_elbow.configFactoryDefault();
|
||||
m_elbow.setNeutralMode(NeutralMode.Brake);
|
||||
// m_elbow = elbow;
|
||||
// m_elbow.configFactoryDefault();
|
||||
// m_elbow.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
setClimberGains();
|
||||
// setClimberGains();
|
||||
|
||||
m_shoulderOffset = m_shoulder.getSelectedSensorPosition();
|
||||
m_elbowOffset = m_elbow.getSelectedSensorPosition();
|
||||
// m_shoulderOffset = m_shoulder.getSelectedSensorPosition();
|
||||
// m_elbowOffset = m_elbow.getSelectedSensorPosition();
|
||||
|
||||
m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
|
||||
m_elbow.configForwardSoftLimitEnable(false);
|
||||
m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
|
||||
m_elbow.configReverseSoftLimitEnable(false);
|
||||
// m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
|
||||
// m_elbow.configForwardSoftLimitEnable(false);
|
||||
// m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
|
||||
// m_elbow.configReverseSoftLimitEnable(false);
|
||||
|
||||
m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD);
|
||||
m_shoulder.configForwardSoftLimitEnable(false);
|
||||
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
|
||||
m_shoulder.configReverseSoftLimitEnable(false);
|
||||
// m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD);
|
||||
// m_shoulder.configForwardSoftLimitEnable(false);
|
||||
// m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
|
||||
// m_shoulder.configReverseSoftLimitEnable(false);
|
||||
|
||||
if(groundRelative)
|
||||
m_gyro = gyro;
|
||||
// if(groundRelative)
|
||||
// m_gyro = gyro;
|
||||
|
||||
m_groundRelative = groundRelative;
|
||||
}
|
||||
// m_groundRelative = groundRelative;
|
||||
// }
|
||||
|
||||
/* getJointAngles
|
||||
* Gets joint angles for climber arm
|
||||
* targetPoint.x and targetPoint.y are set in the xy plane of the climber, accounting for the
|
||||
* rotation of the robot. Both parameters should be in cm.
|
||||
* returns [shoulderAngle, elbowAngle] in radians
|
||||
* Does not set the motors automatically
|
||||
*
|
||||
* IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */
|
||||
public static double[] getJointAngles(Point targetPoint, double tiltAngle) {
|
||||
double [] angles = new double[2];
|
||||
// /* getJointAngles
|
||||
// * Gets joint angles for climber arm
|
||||
// * targetPoint.x and targetPoint.y are set in the xy plane of the climber, accounting for the
|
||||
// * rotation of the robot. Both parameters should be in cm.
|
||||
// * returns [shoulderAngle, elbowAngle] in radians
|
||||
// * Does not set the motors automatically
|
||||
// *
|
||||
// * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */
|
||||
// public static double[] getJointAngles(Point targetPoint, double tiltAngle) {
|
||||
// double [] angles = new double[2];
|
||||
|
||||
double mag = Math.hypot(targetPoint.x, targetPoint.y);
|
||||
if(mag >= ClimberConstants.MAX_ARM_LENGTH) {
|
||||
targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH;
|
||||
targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH;
|
||||
mag = ClimberConstants.MAX_ARM_LENGTH;
|
||||
} else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) {
|
||||
targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH;
|
||||
targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH;
|
||||
mag = ClimberConstants.MIN_ARM_LENGTH;
|
||||
} else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
|
||||
targetPoint.x = ClimberConstants.MIN_ARM_LENGTH;
|
||||
targetPoint.y = 0.d;
|
||||
mag = ClimberConstants.MIN_ARM_LENGTH;
|
||||
}
|
||||
// double mag = Math.hypot(targetPoint.x, targetPoint.y);
|
||||
// if(mag >= ClimberConstants.MAX_ARM_LENGTH) {
|
||||
// targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH;
|
||||
// targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH;
|
||||
// mag = ClimberConstants.MAX_ARM_LENGTH;
|
||||
// } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) {
|
||||
// targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH;
|
||||
// targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH;
|
||||
// mag = ClimberConstants.MIN_ARM_LENGTH;
|
||||
// } else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
|
||||
// targetPoint.x = ClimberConstants.MIN_ARM_LENGTH;
|
||||
// targetPoint.y = 0.d;
|
||||
// mag = ClimberConstants.MIN_ARM_LENGTH;
|
||||
// }
|
||||
|
||||
// The angle to the target point
|
||||
double theta;
|
||||
if(targetPoint.x == 0.d) {
|
||||
theta = Math.PI/2.d; // TODO rename variable
|
||||
} else {
|
||||
theta = Math.atan(targetPoint.y / targetPoint.x);
|
||||
}
|
||||
theta += tiltAngle;
|
||||
// // The angle to the target point
|
||||
// double theta;
|
||||
// if(targetPoint.x == 0.d) {
|
||||
// theta = Math.PI/2.d; // TODO rename variable
|
||||
// } else {
|
||||
// theta = Math.atan(targetPoint.y / targetPoint.x);
|
||||
// }
|
||||
// theta += tiltAngle;
|
||||
|
||||
if(targetPoint.x < 0.d)
|
||||
theta += Math.PI;
|
||||
// if(targetPoint.x < 0.d)
|
||||
// theta += Math.PI;
|
||||
|
||||
|
||||
// Correct target position for tilt
|
||||
targetPoint.x = Math.cos(theta) * mag;
|
||||
targetPoint.y = Math.sin(theta) * mag;
|
||||
// // Correct target position for tilt
|
||||
// targetPoint.x = Math.cos(theta) * mag;
|
||||
// targetPoint.y = Math.sin(theta) * mag;
|
||||
|
||||
// Law and Order: Cosines edition
|
||||
double shoulderAngle;
|
||||
if(mag == 0) {
|
||||
shoulderAngle = 0;
|
||||
} else {
|
||||
shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
|
||||
(2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
|
||||
}
|
||||
shoulderAngle = theta - shoulderAngle;
|
||||
// // Law and Order: Cosines edition
|
||||
// double shoulderAngle;
|
||||
// if(mag == 0) {
|
||||
// shoulderAngle = 0;
|
||||
// } else {
|
||||
// shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
|
||||
// (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
|
||||
// }
|
||||
// shoulderAngle = theta - shoulderAngle;
|
||||
|
||||
double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) /
|
||||
(2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
|
||||
//elbowAngle = Math.PI - elbowAngle;
|
||||
// System.out.println("sa1: " + shoulderAngle);
|
||||
// System.out.println("ea1: " + elbowAngle);
|
||||
// double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) /
|
||||
// (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
|
||||
// //elbowAngle = Math.PI - elbowAngle;
|
||||
// // System.out.println("sa1: " + shoulderAngle);
|
||||
// // System.out.println("ea1: " + elbowAngle);
|
||||
|
||||
// If the climber is resting on the robot base, rotate the upper arm in the direction of the target
|
||||
if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
|
||||
shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
|
||||
double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH;
|
||||
// System.out.println("xDiff: " + xDiff);
|
||||
// // If the climber is resting on the robot base, rotate the upper arm in the direction of the target
|
||||
// if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
|
||||
// shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
|
||||
// double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH;
|
||||
// // System.out.println("xDiff: " + xDiff);
|
||||
|
||||
elbowAngle = Math.atan(-targetPoint.y / xDiff);
|
||||
// System.out.println("ea2: " + elbowAngle);
|
||||
if(elbowAngle < 0) {
|
||||
elbowAngle = Math.PI - Math.abs(elbowAngle);
|
||||
}
|
||||
// elbowAngle = Math.atan(-targetPoint.y / xDiff);
|
||||
// // System.out.println("ea2: " + elbowAngle);
|
||||
// if(elbowAngle < 0) {
|
||||
// elbowAngle = Math.PI - Math.abs(elbowAngle);
|
||||
// }
|
||||
|
||||
if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
|
||||
elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
|
||||
// System.out.println("ea3: " + elbowAngle);
|
||||
// if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
|
||||
// elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
|
||||
// // System.out.println("ea3: " + elbowAngle);
|
||||
|
||||
// Deal with negative wraparound
|
||||
// if(xDiff >= 0.d)
|
||||
// elbowAngle += Math.PI;
|
||||
// System.out.println("ea4: " + elbowAngle);
|
||||
}
|
||||
// // Deal with negative wraparound
|
||||
// // if(xDiff >= 0.d)
|
||||
// // elbowAngle += Math.PI;
|
||||
// // System.out.println("ea4: " + elbowAngle);
|
||||
// }
|
||||
|
||||
angles[0] = shoulderAngle;
|
||||
angles[1] = elbowAngle;
|
||||
return angles;
|
||||
}
|
||||
// angles[0] = shoulderAngle;
|
||||
// angles[1] = elbowAngle;
|
||||
// return angles;
|
||||
// }
|
||||
|
||||
public void setMotors(double shoulderOutput, double elbowOutput) {
|
||||
m_shoulder.set(shoulderOutput);
|
||||
m_elbow.set(elbowOutput);
|
||||
}
|
||||
// public void setMotors(double shoulderOutput, double elbowOutput) {
|
||||
// m_shoulder.set(shoulderOutput);
|
||||
// m_elbow.set(elbowOutput);
|
||||
// }
|
||||
|
||||
/* Rotation Matrix
|
||||
R = [cos(0) -sin(0) \n sin(0) cos(0)]
|
||||
Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)]
|
||||
Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix
|
||||
Rotation Matrix video: https://youtu.be/Ta8cKqltPfU
|
||||
*/
|
||||
public double getRobotTilt() {
|
||||
double[] ypr = new double[3];
|
||||
m_gyro.getYawPitchRoll(ypr);
|
||||
double theta = 0;
|
||||
// /* Rotation Matrix
|
||||
// R = [cos(0) -sin(0) \n sin(0) cos(0)]
|
||||
// Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)]
|
||||
// Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix
|
||||
// Rotation Matrix video: https://youtu.be/Ta8cKqltPfU
|
||||
// */
|
||||
// public double getRobotTilt() {
|
||||
// double[] ypr = new double[3];
|
||||
// m_gyro.getYawPitchRoll(ypr);
|
||||
// double theta = 0;
|
||||
|
||||
// double sin;
|
||||
// double cos;
|
||||
// xsin = 0; //placeholder for sin, cos
|
||||
// ysin = 0;
|
||||
// xcos = 0;
|
||||
// ycos = 0;
|
||||
double[][] rotMax = {
|
||||
{Math.cos(theta) - Math.sin(theta), 0 },
|
||||
{Math.sin(theta) + Math.cos(theta), 0},
|
||||
{0, 0, 1}
|
||||
};
|
||||
// // double sin;
|
||||
// // double cos;
|
||||
// // xsin = 0; //placeholder for sin, cos
|
||||
// // ysin = 0;
|
||||
// // xcos = 0;
|
||||
// // ycos = 0;
|
||||
// double[][] rotMax = {
|
||||
// {Math.cos(theta) - Math.sin(theta), 0 },
|
||||
// {Math.sin(theta) + Math.cos(theta), 0},
|
||||
// {0, 0, 1}
|
||||
// };
|
||||
|
||||
if (m_robotPosition != m_robotAngle){
|
||||
setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition);
|
||||
}
|
||||
// if (m_robotPosition != m_robotAngle){
|
||||
// setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition);
|
||||
// }
|
||||
|
||||
return Math.toRadians(ypr[1]); // Pitch
|
||||
// multiply by pie and then divide by 180
|
||||
// return Math.toRadians(ypr[1]); // Pitch
|
||||
// // multiply by pie and then divide by 180
|
||||
|
||||
}
|
||||
// }
|
||||
|
||||
public void setClimberGains() {
|
||||
// shoulder PIDs
|
||||
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
|
||||
m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
// public void setClimberGains() {
|
||||
// // shoulder PIDs
|
||||
// m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
|
||||
// m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
// m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
// m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
// m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
|
||||
// elbow PIDs
|
||||
m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
|
||||
m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
}
|
||||
// // elbow PIDs
|
||||
// m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
|
||||
// m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
// m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
// m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
// m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
|
||||
// }
|
||||
|
||||
public void setJointAngles(double[] angles) {
|
||||
System.out.println(angles);
|
||||
setJointAngles(angles[0], angles[1]);
|
||||
}
|
||||
// public void setJointAngles(double[] angles) {
|
||||
// System.out.println(angles);
|
||||
// setJointAngles(angles[0], angles[1]);
|
||||
// }
|
||||
|
||||
public void setJointAngles(double shoulderAngle, double elbowAngle) {
|
||||
shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
|
||||
elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
|
||||
// public void setJointAngles(double shoulderAngle, double elbowAngle) {
|
||||
// shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
|
||||
// elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
|
||||
|
||||
shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
|
||||
elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
|
||||
// shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
|
||||
// elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
|
||||
|
||||
// Convert radians to ticks
|
||||
System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
|
||||
// // Convert radians to ticks
|
||||
// System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
|
||||
|
||||
double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
|
||||
double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
|
||||
// double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
|
||||
// double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
|
||||
|
||||
shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO;
|
||||
elbowAngle *= ClimberConstants.GEAR_BOX_RATIO;
|
||||
// shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO;
|
||||
// elbowAngle *= ClimberConstants.GEAR_BOX_RATIO;
|
||||
|
||||
shoulderPosition += m_shoulderOffset;
|
||||
elbowPosition += m_elbowOffset;
|
||||
// shoulderPosition += m_shoulderOffset;
|
||||
// elbowPosition += m_elbowOffset;
|
||||
|
||||
m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
|
||||
m_elbow.set(TalonFXControlMode.Position, elbowPosition);
|
||||
}
|
||||
// m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
|
||||
// m_elbow.set(TalonFXControlMode.Position, elbowPosition);
|
||||
// }
|
||||
|
||||
public void controlWithInput(double xInput, double yInput) {
|
||||
m_position.x += xInput * ClimberConstants.MOVE_SPEED;
|
||||
m_position.y += yInput * ClimberConstants.MOVE_SPEED;
|
||||
// public void controlWithInput(double xInput, double yInput) {
|
||||
// m_position.x += xInput * ClimberConstants.MOVE_SPEED;
|
||||
// m_position.y += yInput * ClimberConstants.MOVE_SPEED;
|
||||
|
||||
System.out.println("x: " + m_position.x + " y: " + m_position.y);
|
||||
// System.out.println("x: " + m_position.x + " y: " + m_position.y);
|
||||
|
||||
double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d;
|
||||
// double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d;
|
||||
|
||||
// double[] testAngles = getJointAngles(0, 0, 0);
|
||||
// System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]);
|
||||
// // double[] testAngles = getJointAngles(0, 0, 0);
|
||||
// // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]);
|
||||
|
||||
// double[] testAngles2 = getJointAngles(5000, 5000, 0);
|
||||
// System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]);
|
||||
// // double[] testAngles2 = getJointAngles(5000, 5000, 0);
|
||||
// // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]);
|
||||
|
||||
// double[] testAngles3 = getJointAngles(0, 75, 0);
|
||||
// System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]);
|
||||
// // double[] testAngles3 = getJointAngles(0, 75, 0);
|
||||
// // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]);
|
||||
|
||||
// double[] testAngles4 = getJointAngles(75, 0, 0);
|
||||
// System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]);
|
||||
// // double[] testAngles4 = getJointAngles(75, 0, 0);
|
||||
// // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]);
|
||||
|
||||
// double[] testAngles5 = getJointAngles(-75, 0, 0);
|
||||
// System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]);
|
||||
// // double[] testAngles5 = getJointAngles(-75, 0, 0);
|
||||
// // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]);
|
||||
|
||||
// double[] testAngles6 = getJointAngles(60, 25, 0);
|
||||
// System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]);
|
||||
// // double[] testAngles6 = getJointAngles(60, 25, 0);
|
||||
// // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]);
|
||||
|
||||
double[] jointAngles = getJointAngles(m_position, tiltAngle);
|
||||
setJointAngles(jointAngles);
|
||||
}
|
||||
// double[] jointAngles = getJointAngles(m_position, tiltAngle);
|
||||
// setJointAngles(jointAngles);
|
||||
// }
|
||||
|
||||
public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) {
|
||||
m_robotPosition = robotPosition;
|
||||
m_robotAngle = robotAngle;
|
||||
m_robotAngle = 45; //45 is placeholder
|
||||
}
|
||||
// public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) {
|
||||
// m_robotPosition = robotPosition;
|
||||
// m_robotAngle = robotAngle;
|
||||
// m_robotAngle = 45; //45 is placeholder
|
||||
// }
|
||||
|
||||
@Override
|
||||
public void periodic(){
|
||||
SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
|
||||
SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
|
||||
}
|
||||
}
|
||||
// @Override
|
||||
// public void periodic(){
|
||||
// SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
|
||||
// }
|
||||
// }
|
||||
|
||||
Reference in New Issue
Block a user