-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathRRBotHardware.java
184 lines (165 loc) · 7.92 KB
/
RRBotHardware.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
/**
* Hardware class, defines the robot's hardware and initializes it
* @author Andrew Hollabaugh
* @since 2017-09-10
*/
public class RRBotHardware
{
public DcMotor rearRightMotor = null;
public DcMotor rearLeftMotor = null;
public DcMotor frontRightMotor = null;
public DcMotor frontLeftMotor = null;
public DcMotor glyphArmMotor1 = null;
public DcMotor glyphWristMotor = null;
public DcMotor servoPowerModule = null;
public Servo jewelArmServo1 = null;
public Servo jewelArmServo2 = null;
public Servo grabber1Servo = null;
public Servo grabber2Servo = null;
public Servo grabberRotateServo = null;
public Servo relicInitServo = null;
public Servo relicGrabberServo = null;
public CRServo grabber1Belt = null;
public CRServo grabber2Belt = null;
public ColorSensor jewelArmColor = null;
public DigitalChannel allianceColorSwitch = null;
public DigitalChannel fieldPosSwitch = null;
public DigitalChannel glyphStartLimit = null;
public DigitalChannel glyphEndLimit = null;
public DigitalChannel glyphGrabberSwitch1 = null;
public DigitalChannel glyphGrabberSwitch2 = null;
HardwareMap hwMap = null;
private ElapsedTime period = new ElapsedTime();
public static final double JEWEL_ARM_SERVO_1_START_POS = 1;
public static final double JEWEL_ARM_SERVO_2_START_POS = 0.1;
//public static final double GRABBER_START_POS = 0.8;
//public static final double RELIC_INIT_SERVO_START_POS = 0.2;
//public static final double RELIC_GRABBER_SERVO_START_POS = 0.8;
/**
* Default constructor
*/
public RRBotHardware(){}
/**
* Defines the robot's hardware elements using hardwareMap.
* Initializes motors with runMode and direction, certain servos to init positions, and sets mode of digitalInputs
* @param ahwMap
*/
public void init(HardwareMap ahwMap)
{
hwMap = ahwMap;
//Initialize hardware
rearRightMotor = hwMap.dcMotor.get("rear_right");
rearLeftMotor = hwMap.dcMotor.get("rear_left");
frontRightMotor = hwMap.dcMotor.get("front_right");
frontLeftMotor = hwMap.dcMotor.get("front_left");
servoPowerModule = hwMap.dcMotor.get("servo_power");
glyphArmMotor1 = hwMap.dcMotor.get("glyph_arm_1");
glyphWristMotor = hwMap.dcMotor.get("glyph_wrist");
jewelArmServo1 = hwMap.servo.get("jewel_arm_1");
jewelArmServo2 = hwMap.servo.get("jewel_arm_2");
grabber1Servo = hwMap.servo.get("grabber_1");
grabber2Servo = hwMap.servo.get("grabber_2");
grabberRotateServo = hwMap.servo.get("grabber_rotate");
relicInitServo = hwMap.servo.get("relic_init");
relicGrabberServo = hwMap.servo.get("relic_grabber");
grabber1Belt = hwMap.crservo.get("grabber_1_belt");
grabber2Belt = hwMap.crservo.get("grabber_2_belt");
jewelArmColor = hwMap.colorSensor.get("arm_color");
allianceColorSwitch = hwMap.digitalChannel.get("alliance_color");
fieldPosSwitch = hwMap.digitalChannel.get("field_pos");
glyphStartLimit = hwMap.digitalChannel.get("glyph_start_limit");
glyphEndLimit = hwMap.digitalChannel.get("glyph_end_limit");
glyphGrabberSwitch1 = hwMap.digitalChannel.get("grabber_switch_1");
glyphGrabberSwitch2 = hwMap.digitalChannel.get("grabber_switch_2");
//set motors to drive forwards
rearRightMotor.setDirection(DcMotor.Direction.REVERSE);
rearLeftMotor.setDirection(DcMotor.Direction.FORWARD);
frontRightMotor.setDirection(DcMotor.Direction.REVERSE);
frontLeftMotor.setDirection(DcMotor.Direction.FORWARD);
glyphArmMotor1.setDirection(DcMotor.Direction.FORWARD);
glyphWristMotor.setDirection(DcMotor.Direction.FORWARD);
servoPowerModule.setDirection(DcMotor.Direction.FORWARD);
//set motors to zero power
rearRightMotor.setPower(0);
rearLeftMotor.setPower(0);
frontRightMotor.setPower(0);
frontLeftMotor.setPower(0);
glyphArmMotor1.setPower(0);
glyphWristMotor.setPower(0);
//servo power module is plugged into motor port, turn it on
servoPowerModule.setPower(1);
//set drive motors to run using encoder guidance and glyphArm motors to stop and reset encoders
rearRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rearLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
glyphArmMotor1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
glyphWristMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
servoPowerModule.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
//sets motors to brake mode and servo power module "motor" to coast mode
rearRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rearLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
glyphArmMotor1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
glyphWristMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
servoPowerModule.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
//set servos to start positions
jewelArmServo1.setPosition(JEWEL_ARM_SERVO_1_START_POS);
jewelArmServo2.setPosition(JEWEL_ARM_SERVO_2_START_POS);
//grabber1Servo.setPosition(GRABBER_START_POS);
//grabber2Servo.setPosition(GRABBER_START_POS);
grabberRotateServo.setPosition(RRBotGlyphArm.GRABBER_ROTATE_POS1);
relicInitServo.setPosition(RRBotGlyphArm.RELIC_INIT_SERVO_GLYPHMODE_POS);
relicGrabberServo.setPosition(RRBotGlyphArm.RELIC_GRABBER_CLOSE_POS);
//set continuous rotation servos to run forwards
grabber1Belt.setDirection(DcMotorSimple.Direction.FORWARD);
grabber2Belt.setDirection(DcMotorSimple.Direction.FORWARD);
//set cr servos to zero power
grabber1Belt.setPower(0);
grabber1Belt.setPower(0);
//set digital channels to input mode
allianceColorSwitch.setMode(DigitalChannel.Mode.INPUT);
fieldPosSwitch.setMode(DigitalChannel.Mode.INPUT);
glyphStartLimit.setMode(DigitalChannel.Mode.INPUT);
glyphEndLimit.setMode(DigitalChannel.Mode.INPUT);
glyphGrabberSwitch1.setMode(DigitalChannel.Mode.INPUT);
glyphGrabberSwitch2.setMode(DigitalChannel.Mode.INPUT);
}
/***
*
* waitForTick implements a periodic delay. However, this acts like a metronome with a regular
* periodic tick. This is used to compensate for varying processing times for each cycle.
* The function looks at the elapsed cycle time, and sleeps for the remaining time interval.
*
* @param periodMs Length of wait cycle in mSec.
*/
public void waitForTick(long periodMs)
{
long remaining = periodMs - (long)period.milliseconds();
// sleep for the remaining portion of the regular cycle period.
if (remaining > 0)
{
try
{
Thread.sleep(remaining);
}
catch (InterruptedException e)
{
Thread.currentThread().interrupt();
}
}
// Reset the cycle clock for the next pass.
period.reset();
}
}