Skip to content

Commit c184e5b

Browse files
nbabspnbabsp
committed
added Main and Mini Bot examples
1 parent 180aa09 commit c184e5b

21 files changed

+1104
-0
lines changed

‎assemblies/MainBot/autonomous/AutoDriveLinearOp.java

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
package org.firstinspires.ftc.teamcode.MainBot.autonomous;
2+
3+
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
4+
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
5+
import com.qualcomm.robotcore.hardware.DcMotor;
6+
import com.qualcomm.robotcore.hardware.DcMotorSimple;
7+
8+
import org.firstinspires.ftc.teamcode.MainBot.teleop.CrossCommunicator;
9+
10+
@Autonomous(name = "Delay Move")
11+
public class AutoDriveLinearOp extends LinearOpMode {
12+
13+
private DcMotor frontLeftMotor;
14+
private DcMotor frontRightMotor;
15+
private DcMotor backLeftMotor;
16+
private DcMotor backRightMotor;
17+
18+
@Override
19+
public void runOpMode() throws InterruptedException {
20+
frontLeftMotor = hardwareMap.dcMotor.get(CrossCommunicator.Drive.MOTOR_FRONT_LEFT);
21+
frontLeftMotor.resetDeviceConfigurationForOpMode();
22+
frontLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
23+
24+
frontRightMotor = hardwareMap.dcMotor.get(CrossCommunicator.Drive.MOTOR_FRONT_RIGHT);
25+
frontRightMotor.resetDeviceConfigurationForOpMode();
26+
frontRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
27+
frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
28+
29+
backLeftMotor = hardwareMap.dcMotor.get(CrossCommunicator.Drive.MOTOR_BACK_LEFT);
30+
backLeftMotor.resetDeviceConfigurationForOpMode();
31+
backLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
32+
33+
backRightMotor = hardwareMap.dcMotor.get(CrossCommunicator.Drive.MOTOR_BACK_RIGHT);
34+
backRightMotor.resetDeviceConfigurationForOpMode();
35+
backRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
36+
backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
37+
38+
telemetry.addData("Status", "Initialized and ready!");
39+
telemetry.update();
40+
41+
waitForStart();
42+
43+
double waitUntil = time + 5;
44+
while(time < waitUntil){
45+
telemetry.addData("Time left", (waitUntil - time));
46+
telemetry.update();
47+
idle();
48+
}
49+
50+
// Drive forward for 5 seconds
51+
frontLeftMotor.setPower(1);
52+
frontRightMotor.setPower(1);
53+
backLeftMotor.setPower(1);
54+
backRightMotor.setPower(1);
55+
56+
waitUntil = time + 1;
57+
while(time < waitUntil)
58+
idle();
59+
frontLeftMotor.setPower(0);
60+
frontRightMotor.setPower(0);
61+
backLeftMotor.setPower(0);
62+
backRightMotor.setPower(0);
63+
}
64+
}

‎assemblies/MainBot/teleop/CrossCommunicator.java

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
package org.firstinspires.ftc.teamcode.MainBot.teleop;
2+
3+
public class CrossCommunicator {
4+
public static double mainBotThrottleFactor = 1.0;
5+
6+
public static class Drive {
7+
public static final String MOTOR_FRONT_LEFT = "front_left";
8+
public static final String MOTOR_FRONT_RIGHT = "front_right";
9+
public static final String MOTOR_BACK_LEFT = "back_left";
10+
public static final String MOTOR_BACK_RIGHT = "back_right";
11+
}
12+
13+
public static class Elevator {
14+
public static final String MOTOR = "elevator";
15+
public static final String SERVO = "elevator_retainer";
16+
17+
public static void setIsExtended(boolean val) {
18+
mainBotThrottleFactor = val ? 0.25 : 1.0;
19+
}
20+
}
21+
22+
public static class Feeder {
23+
public static final String SERVO = "ball_holder";
24+
}
25+
26+
public static class Intake {
27+
public static final String MOTOR = "intake";
28+
}
29+
30+
public static class Pinball {
31+
public static final String MOTOR = "pinball_motor";
32+
public static final String SERVO_FLAG = "semaphore";
33+
public static final String SERVO_CLAMP = "pinball_servo";
34+
public static final String SENSOR_TOUCH = "pinball_touch";
35+
}
36+
}

‎assemblies/MainBot/teleop/DriveAssembly/DriveController.java

Lines changed: 149 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,149 @@
1+
package org.firstinspires.ftc.teamcode.MainBot.teleop.DriveAssembly;
2+
3+
import com.qualcomm.robotcore.hardware.DcMotor;
4+
import com.qualcomm.robotcore.hardware.DcMotorSimple;
5+
import com.qualcomm.robotcore.hardware.Gamepad;
6+
import com.qualcomm.robotcore.hardware.HardwareMap;
7+
import com.qualcomm.robotcore.util.Range;
8+
9+
import org.firstinspires.ftc.robotcore.external.Func;
10+
import org.firstinspires.ftc.robotcore.external.Telemetry;
11+
import org.firstinspires.ftc.teamcode.MainBot.teleop.CrossCommunicator;
12+
13+
public class DriveController {
14+
private DcMotor frontLeftMotor;
15+
private DcMotor frontRightMotor;
16+
private DcMotor backLeftMotor;
17+
private DcMotor backRightMotor;
18+
19+
public void init(Telemetry telemetry, HardwareMap hardwareMap) {
20+
frontLeftMotor = hardwareMap.dcMotor.get(CrossCommunicator.Drive.MOTOR_FRONT_LEFT);
21+
frontLeftMotor.resetDeviceConfigurationForOpMode();
22+
frontLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
23+
24+
telemetry.addData("Front Left Power", new Func<Double>() {
25+
@Override
26+
public Double value() {
27+
return frontLeftMotor.getPower();
28+
}
29+
});
30+
telemetry.addData("Front Left Position", new Func<Integer>() {
31+
@Override
32+
public Integer value() {
33+
return frontLeftMotor.getCurrentPosition();
34+
}
35+
});
36+
telemetry.addData("Front Left Target", new Func<Integer>() {
37+
@Override
38+
public Integer value() {
39+
return frontLeftMotor.getTargetPosition();
40+
}
41+
});
42+
43+
frontRightMotor = hardwareMap.dcMotor.get(CrossCommunicator.Drive.MOTOR_FRONT_RIGHT);
44+
frontRightMotor.resetDeviceConfigurationForOpMode();
45+
frontRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
46+
frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
47+
48+
telemetry.addData("Front Right Power", new Func<Double>() {
49+
@Override
50+
public Double value() {
51+
return frontRightMotor.getPower();
52+
}
53+
});
54+
telemetry.addData("Front Right Position", new Func<Integer>() {
55+
@Override
56+
public Integer value() {
57+
return frontRightMotor.getCurrentPosition();
58+
}
59+
});
60+
telemetry.addData("Front Right Target", new Func<Integer>() {
61+
@Override
62+
public Integer value() {
63+
return frontRightMotor.getTargetPosition();
64+
}
65+
});
66+
67+
backLeftMotor = hardwareMap.dcMotor.get(CrossCommunicator.Drive.MOTOR_BACK_LEFT);
68+
backLeftMotor.resetDeviceConfigurationForOpMode();
69+
backLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
70+
71+
telemetry.addData("Back Left Power", new Func<Double>() {
72+
@Override
73+
public Double value() {
74+
return backLeftMotor.getPower();
75+
}
76+
});
77+
telemetry.addData("Back Left Position", new Func<Integer>() {
78+
@Override
79+
public Integer value() {
80+
return backLeftMotor.getCurrentPosition();
81+
}
82+
});
83+
telemetry.addData("Back Left Target", new Func<Integer>() {
84+
@Override
85+
public Integer value() {
86+
return backLeftMotor.getTargetPosition();
87+
}
88+
});
89+
90+
backRightMotor = hardwareMap.dcMotor.get(CrossCommunicator.Drive.MOTOR_BACK_RIGHT);
91+
backRightMotor.resetDeviceConfigurationForOpMode();
92+
backRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
93+
backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
94+
95+
telemetry.addData("Back Right Power", new Func<Double>() {
96+
@Override
97+
public Double value() {
98+
return backRightMotor.getPower();
99+
}
100+
});
101+
telemetry.addData("Back Right Position", new Func<Integer>() {
102+
@Override
103+
public Integer value() {
104+
return backRightMotor.getCurrentPosition();
105+
}
106+
});
107+
telemetry.addData("Back Right Target", new Func<Integer>() {
108+
@Override
109+
public Integer value() {
110+
return backRightMotor.getTargetPosition();
111+
}
112+
});
113+
}
114+
115+
public void start() {
116+
}
117+
118+
public void loop(Gamepad gamepad1, Gamepad gamepad2) {
119+
float joyLX = scaleInput(-gamepad1.left_stick_x);
120+
float joyLY = scaleInput(-gamepad1.left_stick_y);
121+
float joyRX = scaleInput(-gamepad1.right_stick_x);
122+
float joyRY = scaleInput(-gamepad1.right_stick_y);
123+
float modX = -(joyLX + joyRX) / 2;
124+
125+
frontLeftMotor.setPower(CrossCommunicator.mainBotThrottleFactor * Range.clip(modX + joyLY, -1F, 1F));
126+
frontRightMotor.setPower(CrossCommunicator.mainBotThrottleFactor * Range.clip(joyRY - modX, -1F, 1F));
127+
backLeftMotor.setPower(CrossCommunicator.mainBotThrottleFactor * Range.clip(joyLY - modX, -1F, 1F));
128+
backRightMotor.setPower(CrossCommunicator.mainBotThrottleFactor * Range.clip(modX + joyRY, -1F, 1F));
129+
}
130+
131+
public void stop() {
132+
}
133+
134+
private float scaleInput(float input) {
135+
double[] scaleArray = {0.0, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24, 0.30, 0.36, 0.43,
136+
0.50, 0.60, 0.72, 0.85, 1.0, 1.0};
137+
int index = (int) (input * 16);
138+
if (index < 0)
139+
index = -index;
140+
else if (index > 16)
141+
index = 16;
142+
float scaled;
143+
if (input < 0)
144+
scaled = (float) -scaleArray[index];
145+
else
146+
scaled = (float) scaleArray[index];
147+
return scaled / 2;
148+
}
149+
}

‎assemblies/MainBot/teleop/DriveAssembly/DriveTeleop.java

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
package org.firstinspires.ftc.teamcode.MainBot.teleop.DriveAssembly;
2+
3+
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
4+
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
5+
6+
@TeleOp(name = "Main Bot Drive", group = "Main Bot")
7+
public class DriveTeleop extends OpMode {
8+
9+
private DriveController controller;
10+
11+
public DriveTeleop() {
12+
super();
13+
controller = new DriveController();
14+
}
15+
16+
@Override
17+
public void init() {
18+
controller.init(telemetry, hardwareMap);
19+
}
20+
21+
@Override
22+
public void start() {
23+
controller.start();
24+
}
25+
26+
@Override
27+
public void loop() {
28+
controller.loop(gamepad1, gamepad2);
29+
30+
telemetry.update();
31+
}
32+
33+
@Override
34+
public void stop() {
35+
controller.stop();
36+
}
37+
}

‎assemblies/MainBot/teleop/ElevatorAssembly/ElevatorController.java

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
package org.firstinspires.ftc.teamcode.MainBot.teleop.ElevatorAssembly;
2+
3+
import com.qualcomm.robotcore.hardware.DcMotor;
4+
import com.qualcomm.robotcore.hardware.DcMotorSimple;
5+
import com.qualcomm.robotcore.hardware.Gamepad;
6+
import com.qualcomm.robotcore.hardware.HardwareMap;
7+
import com.qualcomm.robotcore.hardware.Servo;
8+
9+
import org.firstinspires.ftc.robotcore.external.Func;
10+
import org.firstinspires.ftc.robotcore.external.Telemetry;
11+
import org.firstinspires.ftc.teamcode.MainBot.teleop.CrossCommunicator;
12+
13+
public class ElevatorController {
14+
15+
private static final double SERVO_POSITION_CLOSED = 0;
16+
private static final double SERVO_POSITION_OPEN = 1;
17+
private DcMotor motor;
18+
private Servo servo;
19+
20+
public void init(Telemetry telemetry, HardwareMap hardwareMap) {
21+
motor = hardwareMap.dcMotor.get(CrossCommunicator.Elevator.MOTOR);
22+
motor.resetDeviceConfigurationForOpMode();
23+
motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
24+
motor.setDirection(DcMotorSimple.Direction.REVERSE);
25+
26+
telemetry.addData("Elevator Power", new Func<Double>() {
27+
@Override
28+
public Double value() {
29+
return motor.getPower();
30+
}
31+
});
32+
telemetry.addData("Elevator Position", new Func<Integer>() {
33+
@Override
34+
public Integer value() {
35+
return motor.getCurrentPosition();
36+
}
37+
});
38+
telemetry.addData("Elevator Target", new Func<Integer>() {
39+
@Override
40+
public Integer value() {
41+
return motor.getTargetPosition();
42+
}
43+
});
44+
45+
servo = hardwareMap.servo.get(CrossCommunicator.Elevator.SERVO);
46+
servo.setDirection(Servo.Direction.REVERSE);
47+
servo.setPosition(SERVO_POSITION_CLOSED);
48+
}
49+
50+
public void start() {
51+
}
52+
53+
public void loop(Gamepad gamepad1, Gamepad gamepad2) {
54+
if (gamepad1.right_bumper || gamepad2.dpad_up) {
55+
servo.setPosition(SERVO_POSITION_OPEN);
56+
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
57+
motor.setPower(1);
58+
} else if (gamepad1.left_bumper || gamepad2.dpad_down) {
59+
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
60+
motor.setPower(-1);
61+
} else {
62+
motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
63+
motor.setTargetPosition(motor.getCurrentPosition());
64+
}
65+
66+
if (gamepad1.b || gamepad2.dpad_right) {
67+
servo.setPosition(SERVO_POSITION_OPEN);
68+
} else if (gamepad1.y || gamepad2.dpad_left) {
69+
servo.setPosition(SERVO_POSITION_CLOSED);
70+
}
71+
if (Math.abs(motor.getCurrentPosition()) > 20000) {
72+
CrossCommunicator.Elevator.setIsExtended(true);
73+
} else {
74+
CrossCommunicator.Elevator.setIsExtended(false);
75+
}
76+
}
77+
78+
public void stop() {
79+
}
80+
}

0 commit comments

Comments
 (0)