forked from BotNotFound/FreightFrenzyPractice
-
Notifications
You must be signed in to change notification settings - Fork 0
/
RobotHardware.java
167 lines (148 loc) · 7.47 KB
/
RobotHardware.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
/* Copyright (c) 2022 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
/*
* This file works in conjunction with the External Hardware Class sample called: ConceptExternalHardwareClass.java
* Please read the explanations in that Sample about how to use this class definition.
*
* This file defines a Java Class that performs all the setup and configuration for a sample robot's hardware (motors and sensors).
* It assumes three motors (left_drive, right_drive and arm) and two servos (left_hand and right_hand)
*
* This one file/class can be used by ALL of your OpModes without having to cut & paste the code each time.
*
* Where possible, the actual hardware objects are "abstracted" (or hidden) so the OpMode code just makes calls into the class,
* rather than accessing the internal hardware directly. This is why the objects are declared "private".
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with *exactly the same name*.
*
* Or... In OnBot Java, add a new file named RobotHardware.java, select this sample, and select Not an OpMode.
* Also add a new OpMode, select the sample ConceptExternalHardwareClass.java, and select TeleOp.
*
*/
public class RobotHardware {
/* Declare OpMode members. */
private LinearOpMode myOpMode = null; // gain access to methods in the calling OpMode.
// Define Motor and Servo objects (Make them private so they can't be accessed externally)
private DcMotor leftDrive = null;
private DcMotor rightDrive = null;
private DcMotor armMotor = null;
private Servo leftHand = null;
private Servo rightHand = null;
// Define Drive constants. Make them public so they CAN be used by the calling OpMode
public static final double MID_SERVO = 0.5 ;
public static final double HAND_SPEED = 0.02 ; // sets rate to move servo
public static final double ARM_UP_POWER = 0.45 ;
public static final double ARM_DOWN_POWER = -0.45 ;
// Define a constructor that allows the OpMode to pass a reference to itself.
public RobotHardware (LinearOpMode opmode) {
myOpMode = opmode;
}
/**
* Initialize all the robot's hardware.
* This method must be called ONCE when the OpMode is initialized.
* <p>
* All of the hardware devices are accessed via the hardware map, and initialized.
*/
public void init() {
// Define and Initialize Motors (note: need to use reference to actual OpMode).
leftDrive = myOpMode.hardwareMap.get(DcMotor.class, "left_drive");
rightDrive = myOpMode.hardwareMap.get(DcMotor.class, "right_drive");
armMotor = myOpMode.hardwareMap.get(DcMotor.class, "arm");
// To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
// Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive.
// Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);
// If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy
// leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Define and initialize ALL installed servos.
leftHand = myOpMode.hardwareMap.get(Servo.class, "left_hand");
rightHand = myOpMode.hardwareMap.get(Servo.class, "right_hand");
leftHand.setPosition(MID_SERVO);
rightHand.setPosition(MID_SERVO);
myOpMode.telemetry.addData(">", "Hardware Initialized");
myOpMode.telemetry.update();
}
/**
* Calculates the left/right motor powers required to achieve the requested
* robot motions: Drive (Axial motion) and Turn (Yaw motion).
* Then sends these power levels to the motors.
*
* @param Drive Fwd/Rev driving power (-1.0 to 1.0) +ve is forward
* @param Turn Right/Left turning power (-1.0 to 1.0) +ve is CW
*/
public void driveRobot(double Drive, double Turn) {
// Combine drive and turn for blended motion.
double left = Drive + Turn;
double right = Drive - Turn;
// Scale the values so neither exceed +/- 1.0
double max = Math.max(Math.abs(left), Math.abs(right));
if (max > 1.0)
{
left /= max;
right /= max;
}
// Use existing function to drive both wheels.
setDrivePower(left, right);
}
/**
* Pass the requested wheel motor powers to the appropriate hardware drive motors.
*
* @param leftWheel Fwd/Rev driving power (-1.0 to 1.0) +ve is forward
* @param rightWheel Fwd/Rev driving power (-1.0 to 1.0) +ve is forward
*/
public void setDrivePower(double leftWheel, double rightWheel) {
// Output the values to the motor drives.
leftDrive.setPower(leftWheel);
rightDrive.setPower(rightWheel);
}
/**
* Pass the requested arm power to the appropriate hardware drive motor
*
* @param power driving power (-1.0 to 1.0)
*/
public void setArmPower(double power) {
armMotor.setPower(power);
}
/**
* Send the two hand-servos to opposing (mirrored) positions, based on the passed offset.
*
* @param offset
*/
public void setHandPositions(double offset) {
offset = Range.clip(offset, -0.5, 0.5);
leftHand.setPosition(MID_SERVO + offset);
rightHand.setPosition(MID_SERVO - offset);
}
}