Skip to content

Commit 5a92aed

Browse files
kevinfreikevinfrei
committed
added claw and arm hardware, setup, robot, controls and cmds code
1 parent 579fe6b commit 5a92aed

File tree

7 files changed

+216
-11
lines changed

7 files changed

+216
-11
lines changed

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java‎

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -21,16 +21,15 @@ public class Hardware implements Loggable {
2121

2222
public List<LynxModule> hubs;
2323

24-
public EncodedMotor<DcMotorEx> theMotor, flMotor, frMotor, rlMotor, rrMotor;
24+
public EncodedMotor<DcMotorEx> theMotor, flMotor, frMotor, rlMotor, rrMotor, arm;
2525
public Motor<DcMotorEx> placeholder1;
2626
public DcMotorEx liftMotor;
2727

2828
public Servo placeholder2;
29-
public Servo servo;
29+
public Servo servo, clawServo;
3030

3131
public IGyro imu;
3232
public Webcam camera;
33-
3433
public Rev2MDistanceSensor distanceSensor;
3534
public ColorDistanceSensor colorSensor;
3635

@@ -73,6 +72,14 @@ public Hardware(HardwareMap hwmap) {
7372
// this.colorSensor = new ColorDistanceSensor(Setup.HardwareNames.COLOR);
7473
// }
7574
}
75+
if (Setup.Connected.CLAWSUBSYSTEM) {
76+
if (Setup.Connected.CLAWSERVO) {
77+
this.clawServo = new Servo(Setup.HardwareNames.CLAWSERVO);
78+
}
79+
if (Setup.Connected.ARM) {
80+
this.arm = new EncodedMotor<>(Setup.HardwareNames.ARM);
81+
}
82+
}
7683
}
7784

7885
// We can read the voltage from the different hubs for fun...

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import com.technototes.library.logger.Loggable;
44
import com.technototes.library.util.Alliance;
55
import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition;
6+
import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem;
67
import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem;
78

89
public class Robot implements Loggable {
@@ -11,6 +12,7 @@ public class Robot implements Loggable {
1112
public Alliance alliance;
1213
public double initialVoltage;
1314
public DrivebaseSubsystem drivebaseSubsystem;
15+
public ClawSubsystem clawSubsystem;
1416

1517
public Robot(Hardware hw) {
1618
this.initialVoltage = hw.voltage();

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,11 @@ public static class Connected {
99

1010
public static boolean DRIVEBASE = true;
1111
public static boolean TESTSUBSYSTEM = false;
12+
public static boolean CLAWSUBSYSTEM = true;
1213
public static boolean MOTOR = false;
1314
public static boolean SERVO = false;
15+
public static boolean ARM = true;
16+
public static boolean CLAWSERVO = true;
1417
public static boolean DISTANCE_SENSOR = false;
1518
public static boolean COLOR_SENSOR = false;
1619
public static boolean FLYWHEEL = false;
@@ -33,6 +36,8 @@ public static class HardwareNames {
3336
public static String DISTANCE = "d";
3437
public static String COLOR = "c";
3538
public static String CAMERA = "camera";
39+
public static String ARM = "arm";
40+
public static String CLAWSERVO = "clawServo";
3641
}
3742

3843
@Config

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/ClawCmds.java‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
package org.firstinspires.ftc.ptechnodactyl.commands;
2+
3+
import com.technototes.library.command.Command;
4+
import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem;
5+
6+
public class ClawCmds {
7+
8+
public static class cmds {
9+
10+
public static Command OpenClaw(ClawSubsystem CS) {
11+
return Command.create(CS::openClaw);
12+
}
13+
14+
public static Command CloseClaw(ClawSubsystem CS) {
15+
return Command.create(CS::closeClaw);
16+
}
17+
}
18+
}

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
package org.firstinspires.ftc.ptechnodactyl.commands;
2+
3+
import com.technototes.library.command.Command;
4+
import com.technototes.library.control.Stick;
5+
import com.technototes.library.logger.Loggable;
6+
import java.util.function.DoubleSupplier;
7+
import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem;
8+
9+
public class JoystickIncDecCmd implements Command, Loggable {
10+
11+
public ClawSubsystem subsystem;
12+
public DoubleSupplier x;
13+
14+
public JoystickIncDecCmd(ClawSubsystem sub, Stick xStick) {
15+
addRequirements(sub);
16+
subsystem = sub;
17+
x = xStick.getXSupplier();
18+
}
19+
20+
// This will make the bot snap to an angle, if the 'straighten' button is pressed
21+
// Otherwise, it just reads the rotation value from the rotation stick
22+
23+
@Override
24+
public void execute() {
25+
double xvalue = -x.getAsDouble();
26+
subsystem.increment(xvalue);
27+
}
28+
29+
@Override
30+
public boolean isFinished() {
31+
return false;
32+
}
33+
}

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
package org.firstinspires.ftc.ptechnodactyl.controllers;
2+
3+
import com.technototes.library.command.Command;
4+
import com.technototes.library.command.CommandScheduler;
5+
import com.technototes.library.control.CommandButton;
6+
import com.technototes.library.control.CommandGamepad;
7+
import com.technototes.library.control.Stick;
8+
import org.firstinspires.ftc.ptechnodactyl.Robot;
9+
import org.firstinspires.ftc.ptechnodactyl.Setup;
10+
import org.firstinspires.ftc.ptechnodactyl.commands.ClawCmds;
11+
import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd;
12+
import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem;
13+
14+
public class OperatorController {
15+
16+
public Robot robot;
17+
public CommandGamepad gamepad;
18+
public CommandButton openClaw;
19+
public CommandButton closeClaw;
20+
public Stick armStick;
21+
22+
public OperatorController(CommandGamepad g, Robot r) {
23+
robot = r;
24+
gamepad = g;
25+
AssignNamedControllerButton();
26+
BindControls();
27+
}
28+
29+
private void AssignNamedControllerButton() {
30+
openClaw = gamepad.leftBumper;
31+
closeClaw = gamepad.rightBumper;
32+
armStick = gamepad.rightStick;
33+
}
34+
35+
public void BindControls() {
36+
if (Setup.Connected.CLAWSUBSYSTEM) {
37+
bindClawSubsystemControls();
38+
}
39+
}
40+
41+
public void bindClawSubsystemControls() {
42+
openClaw.whenPressed(ClawCmds.cmds.OpenClaw(robot.clawSubsystem));
43+
closeClaw.whenPressed(ClawCmds.cmds.CloseClaw(robot.clawSubsystem));
44+
CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick));
45+
}
46+
}

‎Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java‎

Lines changed: 102 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -12,18 +12,43 @@
1212
import com.technototes.library.subsystem.Subsystem;
1313
import org.firstinspires.ftc.ptechnodactyl.Hardware;
1414

15-
1615
@Config
1716
public class ClawSubsystem implements Subsystem, Loggable {
18-
private Servo clawServo, wristServo;
17+
18+
private Servo clawServo;
19+
private EncodedMotor<DcMotorEx> arm;
1920
private boolean isHardware;
2021

2122
@Log(name = "clawPosition")
2223
public double clawPosition = 0;
2324

24-
2525
public static double CLAW_OPEN_POSITION = 0.3;
2626
public static double CLAW_CLOSE_POSITION = 0.7;
27+
public static int INCREMENT_DECREMENT = 200;
28+
public static double FEEDFORWARD_COEFFICIENT = 0.00014;
29+
public static int ARM_VERTICAL = 3100;
30+
public static int ARM_HORIZONTAL = 1000;
31+
public static int INITIAL_POSITION = 150;
32+
public static int ARM_MAX = 0;
33+
public static int ARM_MIN = 0;
34+
35+
@Log(name = "armTarget")
36+
public int armTargetPos;
37+
38+
@Log(name = "armPos")
39+
public int armPos;
40+
41+
@Log(name = "armFdFwdVal")
42+
public double armFeedFwdValue;
43+
44+
private PIDFController armPidController;
45+
46+
private void setArmPos(int e) {
47+
armPidController.setTargetPosition(e);
48+
armTargetPos = e;
49+
}
50+
51+
public static PIDCoefficients armPID = new PIDCoefficients(0.0005, 0.0, 0.0001);
2752

2853
private void setClawPosition(double d) {
2954
if (isHardware) {
@@ -32,14 +57,83 @@ private void setClawPosition(double d) {
3257
}
3358
}
3459

35-
public void openClaw() {
36-
setClawPosition(CLAW_OPEN_POSITION);
60+
public ClawSubsystem(Hardware hw) {
61+
isHardware = true;
62+
clawServo = hw.clawServo;
63+
arm = hw.arm;
64+
armPidController = new PIDFController(
65+
armPID,
66+
0,
67+
0,
68+
0,
69+
/*
70+
71+
The function arguments for the Feed Forward function are Position (ticks) and
72+
Velocity (units?). So, for the arm, we want to check to see if which side of
73+
center we're on, and if the velocity is pushing us down, FF should probably be
74+
low (negative?) while if velocity is pushing us *up*, FF should be high (right?)
75+
Someone who's done physics and/or calculus recently should write some real equations
76+
77+
Braelyn got the math right
78+
79+
For angle T through this range where we start at zero:
80+
/
81+
/ T
82+
180 _____/_____ 0
83+
The downward torque due to gravity is cos(T) * Gravity (9.8m/s^2)
84+
85+
If we're moving from 0 to 180 degrees, then:
86+
While T is less than 90, the "downward torque" is working *against* the motor
87+
When T is greater than 90, the "downward torque" is working *with* the motor
88+
89+
*/
90+
91+
(ticks, velocity) -> {
92+
armFeedFwdValue = FEEDFORWARD_COEFFICIENT * Math.cos(getArmAngle(ticks));
93+
94+
// if (velocity > MIN_ANGULAR_VELOCITY) {
95+
// //increase armFeedFwdValue to avoid slamming or increase D in PID
96+
// armFeedFwdValue += ARM_SLAM_PREVENTION;
97+
// }
98+
if (Math.abs(armFeedFwdValue) < 0.1) {
99+
armFeedFwdValue = 0.0;
100+
}
101+
102+
return armFeedFwdValue;
103+
}
104+
);
105+
setArmPos(INITIAL_POSITION);
106+
}
107+
108+
public void increment(double value) {
109+
int newArmPos = (int) (armTargetPos + value * INCREMENT_DECREMENT);
110+
// if (newArmPos > ARM_MAX) {
111+
// newArmPos = 3150;
112+
// } else if (newArmPos < ARM_MIN) {
113+
// newArmPos = 0;
114+
// }
115+
setArmPos(newArmPos);
37116
}
38117

39-
public void closeClaw(){
40-
setClawPosition(CLAW_CLOSE_POSITION);
118+
public void incrementn() {
119+
increment(1.0);
41120
}
42121

122+
public void decrement() {
123+
increment(-1.0);
124+
}
43125

126+
private static double getArmAngle(double ticks) {
127+
// our horizontal value starts at ARM_HORIZONTAL, so we need to
128+
// subtract it
129+
return ((Math.PI / 2.0) * (ticks - ARM_HORIZONTAL)) / (ARM_VERTICAL - ARM_HORIZONTAL);
130+
}
44131

45-
}
132+
public void openClaw() {
133+
setClawPosition(CLAW_OPEN_POSITION);
134+
}
135+
136+
public void closeClaw() {
137+
setClawPosition(CLAW_CLOSE_POSITION);
138+
}
139+
}

0 commit comments

Comments
 (0)