Skip to content

Commit 46b33ed

Browse files
DriverStationComputerDriverStationComputer
committed
tested climbing
1 parent 42a991b commit 46b33ed

File tree

5 files changed

+45
-29
lines changed

5 files changed

+45
-29
lines changed

‎src/main/java/org/carlmontrobotics/Constants.java‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,8 @@ public static final class Armc {
128128
public static final double ROTATION_TO_RAD = 2 * Math.PI;
129129
public static final boolean ENCODER_INVERTED = true;
130130

131-
public static final double ENCODER_OFFSET_RAD = 0.15235 + 2.08; //- 0.6095;
131+
// difference between CoG and arm is .328 rad
132+
public static final double ENCODER_OFFSET_RAD = -0.08 + .328; //- 0.6095;
132133

133134
// TODO: finish understand why this is broken public static final Measure<Angle>
134135
// INTAKE_ANGLE = Degrees.to(-1);
@@ -383,11 +384,11 @@ public static final class Manipulator {
383384
public static final Axis AMP_AX = Axis.kLeftTrigger;
384385
public static final int SPEAKER_CLOSE = Button.kLeftBumper.value;
385386
public static final int SPEAKER_SAFE = Button.kRightBumper.value;
386-
public static final int SPEAKER_POS = Button.kA.value;
387+
public static final int SPEAKER_POS = Button.kX.value;
387388
//public static final int INTAKE_POS = Button.kX.value;
388-
public static final int EJECT_RPM = Button.kY.value;
389-
public static final int RAISE_CLIMBER = Button.kLeftStick.value;
390-
public static final int LOWER_CLIMBER = Button.kRightStick.value;
389+
public static final int EJECT_RPM = Button.kX.value;
390+
public static final int RAISE_CLIMBER = Button.kA.value;
391+
public static final int LOWER_CLIMBER = Button.kY.value;
391392
}
392393
public static final double JOY_THRESH = 0.01;
393394
public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;//woah, this is high.

‎src/main/java/org/carlmontrobotics/RobotContainer.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -193,7 +193,7 @@ private void setBindingsManipulatorENDEFF() {
193193
//MoveToPos(arm, GROUND_INTAKE_POS));
194194

195195
new JoystickButton(manipulatorController, RAISE_CLIMBER).onTrue(new MoveToPos(arm, Armc.UPPER_ANGLE_LIMIT_RAD));
196-
new JoystickButton(manipulatorController, LOWER_CLIMBER).onTrue(new MoveToPos(arm, Armc.HANG_ANGLE_RAD));
196+
new JoystickButton(manipulatorController, LOWER_CLIMBER).onTrue(new moveClimber(arm));
197197

198198
//NEW BINDINGS(easier for manipulator)
199199
//Xbox left joy Y axis -> raw Intake/Outtake control
@@ -248,6 +248,7 @@ private void setBindingsManipulatorENDEFF() {
248248
249249
*/
250250
//TODO: ask charles if passing in controller is okay
251+
SmartDashboard.putData(new moveClimber(arm));
251252
}
252253
// private void setBindingsManipulatorARM() {
253254
// //NEW BINDINGS(easier for manipulator)

‎src/main/java/org/carlmontrobotics/commands/moveClimber.java‎

Lines changed: 13 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -31,28 +31,31 @@ public moveClimber(Arm arm) {
3131
@Override
3232
public void initialize() {
3333
arm.callDrive = false;//turn normal arm periodic off
34-
arm.setArmTarget(goal);
34+
//arm.setArmTarget(goal);
35+
SmartDashboard.putNumber("climber volts", 0);
3536
}
3637

3738

3839
// Called every time the scheduler runs while the command is scheduled.
3940
@Override
4041
public void execute() {
41-
double err = goal - arm.getArmPos();
42-
arm.drivearm(//equivalent to a clamped P controller with KS
43-
Math.signum(err) * //direction control
44-
MathUtil.clamp(
45-
Math.abs(err)*2,//.5rad err -> 1 speed
46-
.2,
47-
.1
48-
)
49-
);
42+
arm.drivearm(SmartDashboard.getNumber("climber volts", 0));
43+
// double err = goal - arm.getArmPos();
44+
// arm.drivearm(//equivalent to a clamped P controller with KS
45+
// Math.signum(err) * //direction control
46+
// MathUtil.clamp(
47+
// Math.abs(err)*2,//.5rad err -> 1 speed
48+
// .2,
49+
// .1
50+
// )
51+
// );
5052
}
5153

5254
// Called once the command ends or is interrupted.
5355
@Override
5456
public void end(boolean interrupted) {
5557
arm.driveMotor(Volt.of(0));
58+
arm.setArmTarget(arm.getArmPos());
5659
arm.callDrive = true;
5760
}
5861

‎src/main/java/org/carlmontrobotics/subsystems/Arm.java‎

Lines changed: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -97,9 +97,11 @@ public Arm() {
9797
// Comment out when running sysid
9898
armMasterEncoder.setPositionConversionFactor(ROTATION_TO_RAD);
9999
armMasterEncoder.setVelocityConversionFactor(ROTATION_TO_RAD);
100+
armMasterEncoder.setInverted(ENCODER_INVERTED);
101+
100102
armMasterEncoder.setZeroOffset(ENCODER_OFFSET_RAD);
103+
101104
// ------------------------------------------------------------
102-
armMasterEncoder.setInverted(ENCODER_INVERTED);
103105

104106
armMotorFollower.follow(armMotorMaster, MOTOR_INVERTED_FOLLOWER);
105107

@@ -152,6 +154,8 @@ public Arm() {
152154
@Override
153155
public void periodic() {
154156

157+
armMotorMaster.setSmartCurrentLimit(50);
158+
armMotorFollower.setSmartCurrentLimit(50);
155159
if (DriverStation.isDisabled())
156160
resetGoal();
157161

@@ -187,6 +191,9 @@ public void periodic() {
187191
// armFeed = new ArmFeedforward(kS, SmartDashboard.getNumber("set kG", currG), kV, kA);
188192
// KG = currG;
189193
// }
194+
SmartDashboard.putNumber("Master RPM", armMotorMaster.getEncoder().getVelocity());
195+
SmartDashboard.putNumber("Follower RPM", armMotorFollower.getEncoder().getVelocity());
196+
190197

191198
// when the value is different
192199
double currentArmPos = getArmPos();
@@ -197,7 +204,7 @@ public void periodic() {
197204
isArmEncoderConnected = currTime - lastMeasuredTime < DISCONNECTED_ENCODER_TIMEOUT_SEC;
198205

199206
if (isArmEncoderConnected) {
200-
if (callDrive) {
207+
if (callDrive || true) {
201208
driveArm();
202209
}
203210
} else {
@@ -242,8 +249,8 @@ private void driveArm() {
242249
SmartDashboard.putNumber("pid volts", armMotorMaster.getBusVoltage() * armMotorMaster.getAppliedOutput() - armFeedVolts);
243250
}
244251

245-
public void drivearm(double throttle) {
246-
armMotorMaster.set(throttle);
252+
public void drivearm(double throttleVolts) {
253+
armMotorMaster.setVoltage(throttleVolts);
247254
}
248255

249256
public void stopArm() {
@@ -304,7 +311,8 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
304311

305312
public double getArmPos() {
306313
return MathUtil.inputModulus(armMasterEncoder.getPosition(), ARM_DISCONT_RAD,
307-
ARM_DISCONT_RAD + 2 * Math.PI);
314+
ARM_DISCONT_RAD + 2 * Math.PI);//armMasterEncoder.getPosition();//MathUtil.inputModulus(armMasterEncoder.getPosition(), ARM_DISCONT_RAD,
315+
// ARM_DISCONT_RAD + 2 * Math.PI);
308316
}
309317

310318
public double getArmVel() {

‎src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,8 @@
4141

4242
public class IntakeShooter extends SubsystemBase {
4343
private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO_550);
44-
private final CANSparkMax outakeMotor = MotorControllerFactory.createSparkMax(30, MotorConfig.NEO_550);
45-
private final CANSparkFlex outakeMotorVortex = new CANSparkFlex(10,MotorType.kBrushless);
44+
private final CANSparkMax outakeMotor = MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550);
45+
//private final CANSparkFlex outakeMotorVortex = new CANSparkFlex(10,MotorType.kBrushless);
4646
private final RelativeEncoder outakeEncoder = outakeMotor.getEncoder();
4747
private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder();
4848
private final SparkPIDController pidControllerOutake = outakeMotor.getPIDController();
@@ -73,7 +73,7 @@ public IntakeShooter() {
7373
SmartDashboard.putData("Intake Shooter",this);
7474
intakeEncoder.setAverageDepth(4);
7575
intakeEncoder.setMeasurementPeriod(8);
76-
SmartDashboard.putNumber("Vortex volts", 0);
76+
SmartDashboard.putNumber("intake volts", 0);
7777
// setMaxOutakeOverload(1);
7878

7979
}
@@ -133,15 +133,17 @@ public boolean outakeDetectsNote() {
133133

134134
@Override
135135
public void periodic() {
136-
count++;
137-
double volts = SmartDashboard.getNumber("Vortex volts", 0);
138-
outakeMotorVortex.set(volts);
136+
outakeMotor.set(SmartDashboard.getNumber("intake volts", 0));
137+
SmartDashboard.putNumber("outtake vel", outakeMotor.getEncoder().getVelocity());
138+
//count++;
139+
//double volts = SmartDashboard.getNumber("Vortex volts", 0);
140+
//outakeMotorVortex.set(volts);
139141

140142
//setMaxOutake();
141143

142144
}
143145
public void driveMotor(double volts) {
144-
outakeMotorVortex.set(volts);
146+
//outakeMotorVortex.set(volts);
145147
}
146148
public void setCurrentLimit(int limit) {
147149
intakeMotor.setSmartCurrentLimit(limit);
@@ -188,7 +190,8 @@ public double getIntakeRPM() {
188190
return intakeEncoder.getVelocity();
189191
}
190192
public double getVortexRPM() {
191-
return outakeMotorVortex.getEncoder().getVelocity();
193+
return 0;
194+
//return outakeMotorVortex.getEncoder().getVelocity();
192195
}
193196
@Override
194197
public void initSendable(SendableBuilder sendableBuilder) {

0 commit comments

Comments
 (0)