Skip to content
This repository was archived by the owner on Sep 9, 2024. It is now read-only.

Commit a6da73f

Browse files
amjadjamjadj
authored
Merge pull request #7 from 13201Hazmat/dev0.2_subsystems
Merged from all subsystems and RR Field and align to point working
2 parents d9e10eb + 19bd67f commit a6da73f

File tree

11 files changed

+1674
-0
lines changed

11 files changed

+1674
-0
lines changed

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SubSystems/Arm.java‎

Lines changed: 188 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,188 @@
1+
package org.firstinspires.ftc.teamcode.SubSystems;
2+
/*
3+
An arm with two grip mechanisms and rotatable on a motor to almost 225 degrees.
4+
Arm is controlled by trigger button and has fast motion from parked position to
5+
position for dropping ring in low goal, and from there slow motion to position
6+
for dropping ring on wobble goal, picking wobble goal position and picking ring
7+
from floor position
8+
*/
9+
10+
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
11+
import com.qualcomm.robotcore.hardware.DcMotor;
12+
import com.qualcomm.robotcore.hardware.DcMotorSimple;
13+
import com.qualcomm.robotcore.hardware.HardwareMap;
14+
import com.qualcomm.robotcore.hardware.Servo;
15+
16+
public class Arm {
17+
18+
public DcMotor armMotor;
19+
//Gobilda 5202 Series Yellow Jacket Planetary Gear Motor (26.9:1 Ratio, 223 RPM, 3.3 - 5V Encoder)
20+
//Encoder count : 753.2 - mounted on a 2:1 gear ratio
21+
22+
public Servo armGripServo;
23+
24+
public enum ARM_POSITIONS {
25+
ARM_PARKED_POSITION,
26+
ARM_DROP_WOBBLE_GOAL_POSITION,
27+
ARM_HOLD_WOBBLE_GOAL_POSITION,
28+
ARM_RING_POSITION
29+
}
30+
31+
public static int ARM_PARKED_POSITION_COUNT = 0; //TODO : AMJAD : Test and fix value
32+
public static int ARM_DROP_WOBBLE_GOAL_POSITION_COUNT = 600 ; //TODO : AMJAD : Test and fix value
33+
public static int ARM_HOLD_WOBBLE_GOAL_POSITION_COUNT = 800 ; //TODO : AMJAD : Test and fix value
34+
public static int ARM_RING_POSITION_COUNT = 1000 ; //TODO : AMJAD : Test and fix value
35+
36+
public ARM_POSITIONS currentArmPosition = ARM_POSITIONS.ARM_PARKED_POSITION;
37+
public int initialArmPositionCount;
38+
39+
public enum GRIP_SERVO_STATE {OPENED, CLOSED};
40+
41+
public static final double GRIP_OPEN = 0.2, GRIP_CLOSE = 0.7; //TODO : AMJAD : Test and fix value
42+
43+
public GRIP_SERVO_STATE gripServoState = GRIP_SERVO_STATE.OPENED ;
44+
45+
public Arm(HardwareMap hardwareMap) {
46+
armMotor = hardwareMap.dcMotor.get("arm_rotate");
47+
armGripServo = hardwareMap.servo.get("arm_grip");
48+
}
49+
50+
LinearOpMode opModepassed;
51+
52+
public void initArm(LinearOpMode opModepassed1){
53+
this.opModepassed = opModepassed1;
54+
resetArm();
55+
armMotor.setDirection(DcMotorSimple.Direction.FORWARD);
56+
armMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
57+
moveArmParkedPosition();
58+
turnArmBrakeModeOff();
59+
initGrip();
60+
}
61+
62+
public void resetArm(){
63+
DcMotor.RunMode runMode = armMotor.getMode();
64+
armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
65+
armMotor.setMode(runMode);
66+
}
67+
68+
/**
69+
* Method to set Arm brake mode to ON when Zero (0.0) power is applied. <BR>
70+
* To be used when arm is above groundlevel
71+
* setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE)
72+
*/
73+
public void turnArmBrakeModeOn(){
74+
armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
75+
armMotor.setPower(0.0);
76+
}
77+
78+
/**
79+
* Method to set Arm brake mode to OFF when Zero (0.0) power is applied. <BR>
80+
* To be used when arm is on groundlevel or blockLevel[0]
81+
* setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE)
82+
*/
83+
public void turnArmBrakeModeOff(){
84+
armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
85+
armMotor.setPower(0.0);
86+
}
87+
88+
89+
/**
90+
* Method to run motor to set to the set position
91+
*/
92+
public void runArmToLevel() {
93+
//armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
94+
//Turn Motors on
95+
armMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
96+
int sign = armMotor.getCurrentPosition() < armMotor.getTargetPosition() ? -1 : 1;
97+
armMotor.setPower(sign * 0.2);
98+
while(sign*(armMotor.getTargetPosition() - armMotor.getCurrentPosition()) < 0 ){
99+
opModepassed.telemetry.addData("armMotor.getCurrentPosition()", armMotor.getTargetPosition());
100+
opModepassed.telemetry.addData("armMotor.getCurrentPosition()", armMotor.getCurrentPosition());
101+
opModepassed.telemetry.update();
102+
}
103+
}
104+
105+
public void moveArmParkedPosition() {
106+
armMotor.setTargetPosition(ARM_PARKED_POSITION_COUNT);
107+
runArmToLevel();
108+
resetArm();
109+
turnArmBrakeModeOff();
110+
currentArmPosition = ARM_POSITIONS.ARM_PARKED_POSITION;
111+
}
112+
113+
public void moveArmDropWobbleGoalPosition() {
114+
armMotor.setTargetPosition(ARM_DROP_WOBBLE_GOAL_POSITION_COUNT);
115+
runArmToLevel();
116+
turnArmBrakeModeOn();
117+
currentArmPosition = ARM_POSITIONS.ARM_DROP_WOBBLE_GOAL_POSITION;
118+
}
119+
120+
public void moveArmHoldWobbleRingPosition() {
121+
armMotor.setTargetPosition(ARM_HOLD_WOBBLE_GOAL_POSITION_COUNT);
122+
runArmToLevel();
123+
turnArmBrakeModeOn();
124+
currentArmPosition = ARM_POSITIONS.ARM_HOLD_WOBBLE_GOAL_POSITION;
125+
}
126+
127+
public void moveArmRingPosition() {
128+
armMotor.setTargetPosition(ARM_RING_POSITION_COUNT);
129+
runArmToLevel();
130+
turnArmBrakeModeOn();
131+
currentArmPosition = ARM_POSITIONS.ARM_RING_POSITION;
132+
}
133+
134+
public int triggerPositionCount;
135+
136+
public void moveArmByTrigger(double triggerPosition, LinearOpMode opModepassed) {
137+
if ((triggerPosition < 0.1) && (currentArmPosition!= ARM_POSITIONS.ARM_PARKED_POSITION)) {
138+
moveArmParkedPosition();
139+
} else if ((triggerPosition > 0.1) && (triggerPosition < 0.5) &&
140+
(currentArmPosition!= ARM_POSITIONS.ARM_DROP_WOBBLE_GOAL_POSITION)) {
141+
moveArmDropWobbleGoalPosition();
142+
} else if ((triggerPosition > 0.5) && (triggerPosition < 0.9) &&
143+
(currentArmPosition!= ARM_POSITIONS.ARM_HOLD_WOBBLE_GOAL_POSITION)) {
144+
moveArmHoldWobbleRingPosition();
145+
} else if ((triggerPosition > 0.9) &&
146+
(currentArmPosition!= ARM_POSITIONS.ARM_RING_POSITION)) {
147+
moveArmRingPosition();
148+
}
149+
150+
/*opModepassed.telemetry.addData("initialArmPositionCount", initialArmPositionCount);
151+
opModepassed.telemetry.addData("armMotor.getCurrentPosition()", armMotor.getCurrentPosition());
152+
opModepassed.telemetry.addData("triggerPosition", triggerPosition);
153+
opModepassed.telemetry.addData("armGripServo.getCurrentPosition()", armGripServo.getPosition());
154+
opModepassed.telemetry.update();
155+
156+
*/
157+
}
158+
159+
//**** Grip Methods ****
160+
public void initGrip() {
161+
// On init close grip - In Autonomous mode, this will be used by drive to make robot hold the wobble goal
162+
armGripServo.setPosition(GRIP_CLOSE);
163+
164+
// AMJAD : Changed hardware design to use on one servo for both wobble goal and grip
165+
//armRingGripServo.setPosition(GRIP_CLOSE);
166+
gripServoState = GRIP_SERVO_STATE.CLOSED;
167+
}
168+
169+
public void openGrip() {
170+
armGripServo.setPosition(GRIP_OPEN);
171+
gripServoState = GRIP_SERVO_STATE.OPENED;
172+
}
173+
174+
public void closeGrip() {
175+
armGripServo.setPosition(GRIP_CLOSE);
176+
gripServoState = GRIP_SERVO_STATE.CLOSED;
177+
}
178+
179+
// grips ring with hand of the arm by running armRingGripServo
180+
181+
public ARM_POSITIONS getCurrentArmPosition() {
182+
return currentArmPosition;
183+
}
184+
185+
public GRIP_SERVO_STATE getGripServoState() {
186+
return gripServoState;
187+
}
188+
}

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SubSystems/Configuration‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
2+
Hardware configuration declarations :
3+
4+
frontLeft = hardwareMap.dcMotor.get("flmotor");
5+
frontRight = hardwareMap.dcMotor.get("frmotor");
6+
backLeft = hardwareMap.dcMotor.get("blmotor");
7+
backRight = hardwareMap.dcMotor.get("brmotor");
8+
9+
armRotateMotor = hardwareMap.dcMotor.get("arm_rotate");
10+
armWobbleGoalGripServo = hardwareMap.servo.get("arm_grip");
11+
12+
intakeMotor = hardwareMap.dcMotor.get("intake_motor");
13+
14+
launchControllerColorBeacon = hardwareMap.i2cDevice.get("launch_beacon");
15+
16+
launcherRingPlungerServo = hardwareMap.servo.get("launch__servo");
17+
launcherFlyWheelMotor = hardwareMap.dcMotor.get("launch_motor");
18+
19+
magazineServo = hardwareMap.crservo.get("mgz_servo");
20+
magazineRingSensor = hardwareMap.get(NormalizedColorSensor.class, "mgz_ring_sensor");
21+
22+
magazineColorBeacon = hardwareMap.i2cDevice.get("mgz_beacon");
23+
24+
magazineLaunchTouchSensor = hardwareMap.get(DigitalChannel.class, "mgz_launch_ts");
25+
magazineLaunchTouchSensor = hardwareMap.get(DigitalChannel.class, "mgz_collect_ts");

0 commit comments

Comments
 (0)