-
Notifications
You must be signed in to change notification settings - Fork 1
/
MainProgRoboteers.java
209 lines (183 loc) · 8.49 KB
/
MainProgRoboteers.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
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name = "0 Roboteers main program")
public class MainProgRoboteers extends LinearOpMode {
private DcMotor right_drive;
private DcMotor arm_drive;
private DcMotor left_drive;
private Servo grab_servo;
private DcMotor carousel_drive;
private DigitalChannel ms_up;
private DigitalChannel ms_dn;
private final double dir_scaling= 1.0;
private double pow_dif;
private double L_Pow_C, R_Pow_C;
//private double gamepadlsy;
/**
* This function is executed when this Op Mode is selected from the Driver Station.
*/
@Override
public void runOpMode() {
initRobot();
waitForStart();
if (opModeIsActive()) {
// Put run blocks here.
while (opModeIsActive()) {
// Put loop blocks here.
// The Y axis of a joystick ranges from -1 in its topmost position
// to +1 in its bottommost position. We negate this value so that
// the topmost position corresponds to maximum forward power.
if (gamepad1.a) {
arm_drive.setTargetPosition(310);
arm_drive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
//MotorIsBusy(left_drive,right_drive);
left_drive.setPower(0);
right_drive.setPower(0);
arm_drive.setPower(0.5);
while (arm_drive.isBusy()) {
// nothing to do here . ..
}
arm_drive.setPower(0);
} else if (gamepad1.b) {
left_drive.setPower(0);
right_drive.setPower(0);
arm_drive.setTargetPosition(620);
arm_drive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
//MotorIsBusy(left_drive,right_drive);
arm_drive.setPower(0.5);
while (arm_drive.isBusy()) {
// nothing to do here . ..
}
arm_drive.setPower(0);
} else if (gamepad1.y) {
left_drive.setPower(0);
right_drive.setPower(0);
arm_drive.setTargetPosition(900);
arm_drive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
//MotorIsBusy(left_drive,right_drive);
arm_drive.setPower(0.5);
while (arm_drive.isBusy()) {
// nothing to do here . ..
}
arm_drive.setPower(0);
//left_drive.setTargetPosition(left_drive.getCurrentPosition() + 200);
//right_drive.setTargetPosition(left_drive.getCurrentPosition() + 200);
//MotorIsBusy(left_drive,right_drive)
}
pow_dif = dir_scaling * gamepad1.left_stick_x;
// Front right turn
/*if (-gamepad1.left_stick_y>0 && gamepad1.left_stick_x>0) {
left_drive.setPower(-gamepad1.left_stick_y + pow_dif);
right_drive.setPower(-gamepad1.left_stick_y - pow_dif);
} // Front left turn
else if (-gamepad1.left_stick_y>0&&gamepad1.left_stick_x<0){
left_drive.setPower(-gamepad1.left_stick_y+pow_dif);
right_drive.setPower(-gamepad1.left_stick_y-pow_dif);
} else if (-gamepad1.left_stick_y>0&&gamepad1.left_stick_x<0){
left_drive.setPower(-gamepad1.left_stick_y+pow_dif);
right_drive.setPower(-gamepad1.left_stick_y-pow_dif);
}*/
if (-gamepad1.left_stick_y > 0) {
L_Pow_C = -gamepad1.left_stick_y + pow_dif;
R_Pow_C = -gamepad1.left_stick_y - pow_dif;
//Following 2 lines don't make any sense
// accidently put < instead
if(L_Pow_C>1){L_Pow_C=1; }
if(R_Pow_C>1){R_Pow_C=1; }
if(L_Pow_C<0){L_Pow_C=0; }
if(R_Pow_C<0){R_Pow_C=0; }
//New code added by Anmol
} else if (-gamepad1.left_stick_y < 0) {
L_Pow_C = -gamepad1.left_stick_y - pow_dif;
R_Pow_C = -gamepad1.left_stick_y + pow_dif;
if(L_Pow_C<-1){L_Pow_C=-1; }
if(R_Pow_C<-1){R_Pow_C=-1; }
if(L_Pow_C>0){L_Pow_C=0; }
if(R_Pow_C>0){R_Pow_C=0; }
} else {
L_Pow_C = -gamepad1.left_stick_y;
R_Pow_C = -gamepad1.left_stick_y;
}
left_drive.setPower(Math.pow(L_Pow_C, 3));
right_drive.setPower(Math.pow(R_Pow_C, 3));
// Code for magnetic switch
// If the up magnetic switch is closed (false) and the user is trying
// to go up, don't allow to proceed further.
ArmDrive();
CarouselDrive();
if (gamepad1.right_bumper) {
grab_servo.setPosition(0); // claw is closed
} else if (gamepad1.left_bumper) {
grab_servo.setPosition(0.5); //claw is open
}
telemetry.addData("Left Pow", L_Pow_C);
telemetry.addData("Right Pow", R_Pow_C);
telemetry.addData("LSY", -gamepad1.left_stick_y);
telemetry.addData("ARM Pow", arm_drive.getPower());
telemetry.addData("Servo Pos", grab_servo.getPosition());
telemetry.addData("Arm Pos", arm_drive.getCurrentPosition());
//telemetry.addData("ms up", !(ms_up.getState()));
//telemetry.addData("ms down", !(ms_dn.getState()));
telemetry.update();
// Use left stick to drive and right stick to turn
}
}
}
private void initRobot(){
right_drive = hardwareMap.get(DcMotor.class, "right_drive");
arm_drive = hardwareMap.get(DcMotor.class, "arm_drive");
left_drive = hardwareMap.get(DcMotor.class, "left_drive");
grab_servo = hardwareMap.get(Servo.class, "grab_servo");
carousel_drive = hardwareMap.get(DcMotor.class, "carousel_drive");
ms_dn = hardwareMap.get(DigitalChannel.class, "ms_dn");
ms_up = hardwareMap.get(DigitalChannel.class, "ms_up");
left_drive.setDirection(DcMotorSimple.Direction.REVERSE);
carousel_drive.setDirection(DcMotorSimple.Direction.FORWARD);
// You will have to determine which motor to reverse for your robot.
// In this example, the right motor was reversed so that positive
// applied power makes it move the robot in the forward direction.
//arm_drive.setDirection(DcMotorSimple.Direction.FORWARD);
// we want to set the 0 to ms_dn
//if (ms_dn.getState()) {
// arm_drive.setPower(-0.9);
}
//while(ms_dn.getState());
//arm_drive.setPower(0);
//sleep(1000);
//arm_drive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
//arm_drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
private void ArmDrive(){
arm_drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
if (!(ms_up.getState())&&(gamepad1.right_stick_y<0)) {
arm_drive.setPower(0);
return;
// Same case as above but this time the down magnetic switch is close
// and the user is trying to move further down. Don't allow that.
} else if (!(ms_dn.getState())&&(gamepad1.right_stick_y>0)) {
arm_drive.setPower(0);
return;
} else {
arm_drive.setPower(-gamepad1.right_stick_y);
return;
}
}
private void CarouselDrive(){
if(gamepad1.left_trigger > 0){
carousel_drive.setPower(-1);
}
else{
carousel_drive.setPower(0);
}
if(gamepad1.right_trigger > 0){
carousel_drive.setPower(1);
}
else {
carousel_drive.setPower(0);
}
}
}