karx/FtcRobotController
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Add ability to record and then replay autonomous + fix warnings that annoy me when commiting

Browse source
This commit is contained in:
4a656666 2021-12-04 21:52:45 -06:00
parent 07c314797f
commit 34c40cf46c
4 changed files with 213 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/API.java
View file

@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
public class API {
@ -33,7 +34,8 @@ public class API {
*/
public static void pause(double seconds) {
double time = opMode.getRuntime() + seconds;
while (opMode.getRuntime()<time);
//noinspection StatementWithEmptyBody
while (opMode.getRuntime()<time); /* no empty while loop here! */
}
/**
@ -118,6 +120,10 @@ public class API {
this.power = power;
}
public double getPower() {
return this.power;
}
/**
* Sets the direction the motor should move in, without starting the motor
*

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gamepad.java
View file

@ -58,7 +58,9 @@ public class Gamepad extends OpMode {
ms-=System.currentTimeMillis();
if (ms>5) try {
Thread.sleep(ms);
} catch (InterruptedException ie) {}
} catch (InterruptedException ie) {
API.print("Why are we here? Just to suffer?");
}
}
// private void move(double power, double turn, double strafe, double speed, boolean verbose) {

103
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GamepadRecord.java Normal file
View file

@ -0,0 +1,103 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ReadWriteFile;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import org.json.JSONException;
import org.json.JSONObject;
import java.io.File;
@TeleOp(name="Gamepad Recorder v1.0.0")
public class GamepadRecord extends OpMode {
private double speed = 1;
private API.Motor intakeMotor = API.Motor.M4;
private API.Motor liftMotor = API.Motor.M5;
private API.Motor carouselMotor = API.Motor.M6;
private String outputFileName = "recording1.json";
private File outputFile;
private JSONObject outputJSON;
private boolean prevGuide = false;
private int currentIndex = 0;
@Override
public void init() {
API.init(this);
API.print("Status", "Initializing, please wait");
API.pause(1);
MovementAPI.init(API.Motor.M0, API.Motor.M1, API.Motor.M2, API.Motor.M3);
API.clear();
API.print("Press play to start");
}
@Override
public void start() {
API.clear();
API.imu.reset();
intakeMotor.setDirection(API.Direction.REVERSE);
outputFile = AppUtil.getInstance().getSettingsFile(outputFileName);
try {
outputJSON = new JSONObject("{ \"recordedData\": [] }");
} catch (JSONException e) {
API.print("Why are we here? Just to suffer?");
}
}
@Override
public void loop() {
long ms = System.currentTimeMillis()+15;
MovementAPI.move(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x, speed, false);
API.print(
"Speed: " + speed + System.lineSeparator() +
"Time: " + this.time
);
if (gamepad1.right_bumper) speed = Math.min(speed+0.01, 1);
else if (gamepad1.left_bumper) speed = Math.max(speed-0.01, 0.2);
intakeMotor.controlWithTwoButtons(gamepad2.a, gamepad2.b);
liftMotor.controlWithTwoButtons(gamepad2.dpad_up, gamepad2.dpad_down, 0.25);
carouselMotor.controlWithTwoButtons(gamepad2.x, gamepad2.y);
try {
JSONObject currentData = new JSONObject()
.put("movement",
new JSONObject()
.put("x", gamepad1.left_stick_x)
.put("y", -gamepad1.left_stick_y)
.put("turn", gamepad1.right_stick_x)
.put("speed", speed)
)
.put("intake", intakeMotor.getPower())
.put("lift", liftMotor.getPower())
.put("carousel", carouselMotor.getPower())
.put("time", this.getRuntime() * 1000);
if (!currentData.toString().equals(outputJSON.getJSONArray("recordedData").get(currentIndex - 1).toString()))
outputJSON.getJSONArray("recordedData").put(currentIndex++, currentData);
} catch (JSONException e) {
API.print("Why are we here? Just to suffer?");
}
ms-=System.currentTimeMillis();
if (ms>5) try {
Thread.sleep(ms);
} catch (InterruptedException ie) {
API.print("Why are we here? Just to suffer?");
}
if (gamepad1.guide && !prevGuide) {
ReadWriteFile.writeFile(outputFile, outputJSON.toString());
API.print("Wrote file to '" + outputFile.getName() + "'!");
}
prevGuide = gamepad1.guide;
}
}

100
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Replay.java Normal file
View file

@ -0,0 +1,100 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ReadWriteFile;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import org.json.JSONException;
import org.json.JSONObject;
import java.io.File;
@Autonomous(name="Autonomous Replay v1.0.0", preselectTeleOp = "AAAAAAAAA")
public class Replay extends OpMode {
private API.Motor intakeMotor = API.Motor.M4;
private API.Motor liftMotor = API.Motor.M5;
private API.Motor carouselMotor = API.Motor.M6;
private String recordingFileName = "recording1.json";
private File recordingFile;
private JSONObject recordingJSON;
private int nextIndex = 0;
@Override
public void init() {
API.init(this);
API.print("Status", "Initializing, please wait");
API.pause(1);
MovementAPI.init(API.Motor.M0, API.Motor.M1, API.Motor.M2, API.Motor.M3);
API.clear();
API.print("Press play to start");
}
@Override
public void start() {
API.clear();
API.imu.reset();
intakeMotor.setDirection(API.Direction.REVERSE);
recordingFile = AppUtil.getInstance().getSettingsFile(recordingFileName);
try {
recordingJSON = new JSONObject(ReadWriteFile.readFile(recordingFile));
} catch (JSONException e) {
API.print("Why are we here? Just to suffer?");
}
}
@Override
public void loop() {
if (nextIndex == -1) return;
try {
JSONObject currentData = recordingJSON.getJSONArray("recordingData").getJSONObject(nextIndex);
if (this.getRuntime() * 1000 < currentData.getDouble("time")) return;
JSONObject currentMovementData = currentData.getJSONObject("movement");
MovementAPI.move(
currentMovementData.getDouble("y"),
currentMovementData.getDouble("x"),
currentMovementData.getDouble("turn"),
currentMovementData.getDouble("speed"),
true
);
intakeMotor.start(currentData.getDouble("intake"));
liftMotor.start(currentData.getDouble("lift"));
carouselMotor.start(currentData.getDouble("carousel"));
nextIndex++;
if(nextIndex >= recordingJSON.getJSONArray("recordingData").length()){
this.requestOpModeStop();
nextIndex = -1;
}
} catch (JSONException e) {
API.print("Why are we here? Just to suffer?");
}
// try {
// outputJSON.getJSONArray("recordedData").put(
// new JSONObject()
// .put("movement",
// new JSONObject()
// .put("x", gamepad1.left_stick_x)
// .put("y", -gamepad1.left_stick_y)
// .put("turn", gamepad1.right_stick_x)
// .put("speed", speed)
// )
// .put("intake", intakeMotor.getPower())
// .put("lift", liftMotor.getPower())
// .put("carousel", carouselMotor.getPower())
// .put("time", this.getRuntime() * 1000)
// );
// } catch (JSONException e) {
// API.print("Why are we here? Just to suffer?");
// }
}
}