-
Notifications
You must be signed in to change notification settings - Fork 1
/
RobotAutoDriveToLine_Linear.java
142 lines (120 loc) · 7.02 KB
/
RobotAutoDriveToLine_Linear.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
/* Copyright (c) 2017 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.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import com.qualcomm.robotcore.hardware.SwitchableLight;
/*
* This OpMode illustrates the concept of driving up to a line and then stopping.
* The code is structured as a LinearOpMode
*
* The Sensor used here can be a REV Color Sensor V2 or V3. Make sure the white LED is turned on.
* The sensor can be plugged into any I2C port, and must be named "sensor_color" in the active configuration.
*
* Depending on the height of your color sensor, you may want to set the sensor "gain".
* The higher the gain, the greater the reflected light reading will be.
* Use the SensorColor sample in this folder to determine the minimum gain value that provides an
* "Alpha" reading of 1.0 when you are on top of the white line. In this sample, we use a gain of 15
* which works well with a Rev V2 color sensor
*
* Setting the correct WHITE_THRESHOLD value is key to stopping correctly.
* This should be set halfway between the bare-tile, and white-line "Alpha" values.
* The reflected light value can be read on the screen once the OpMode has been INIT, but before it is STARTED.
* Move the sensor on and off the white line and note the min and max readings.
* Edit this code to make WHITE_THRESHOLD halfway between the min and max.
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*/
@Autonomous(name="Robot: Auto Drive To Line", group="Robot")
@Disabled
public class RobotAutoDriveToLine_Linear extends LinearOpMode {
/* Declare OpMode members. */
private DcMotor leftDrive = null;
private DcMotor rightDrive = null;
/** The variable to store a reference to our color sensor hardware object */
NormalizedColorSensor colorSensor;
static final double WHITE_THRESHOLD = 0.5; // spans between 0.0 - 1.0 from dark to light
static final double APPROACH_SPEED = 0.25;
@Override
public void runOpMode() {
// Initialize the drive system variables.
leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
// To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
// When run, this OpMode should start both motors driving 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);
// Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over
// ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while
// the values you get from ColorSensor are dependent on the specific sensor you're using.
colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
// If necessary, turn ON the white LED (if there is no LED switch on the sensor)
if (colorSensor instanceof SwitchableLight) {
((SwitchableLight)colorSensor).enableLight(true);
}
// Some sensors allow you to set your light sensor gain for optimal sensitivity...
// See the SensorColor sample in this folder for how to determine the optimal gain.
// A gain of 15 causes a Rev Color Sensor V2 to produce an Alpha value of 1.0 at about 1.5" above the floor.
colorSensor.setGain(15);
// Wait for driver to press PLAY)
// Abort this loop is started or stopped.
while (opModeInInit()) {
// Send telemetry message to signify robot waiting;
telemetry.addData("Status", "Ready to drive to white line."); //
// Display the light level while we are waiting to start
getBrightness();
}
// Start the robot moving forward, and then begin looking for a white line.
leftDrive.setPower(APPROACH_SPEED);
rightDrive.setPower(APPROACH_SPEED);
// run until the white line is seen OR the driver presses STOP;
while (opModeIsActive() && (getBrightness() < WHITE_THRESHOLD)) {
sleep(5);
}
// Stop all motors
leftDrive.setPower(0);
rightDrive.setPower(0);
}
// to obtain reflected light, read the normalized values from the color sensor. Return the Alpha channel.
double getBrightness() {
NormalizedRGBA colors = colorSensor.getNormalizedColors();
telemetry.addData("Light Level (0 to 1)", "%4.2f", colors.alpha);
telemetry.update();
return colors.alpha;
}
}