Initial Version - CustomController recording system - TinyBot mode
This commit is contained in:
555
src/main.cpp
Normal file
555
src/main.cpp
Normal file
@ -0,0 +1,555 @@
|
||||
#pragma region VEXcode Generated Robot Configuration
|
||||
// Make sure all required headers are included.
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <fstream>
|
||||
|
||||
#include "vex.h"
|
||||
|
||||
using namespace vex;
|
||||
|
||||
// Brain should be defined by default
|
||||
brain Brain;
|
||||
|
||||
|
||||
// START V5 MACROS
|
||||
#define waitUntil(condition) \
|
||||
do { \
|
||||
wait(5, msec); \
|
||||
} while (!(condition))
|
||||
|
||||
#define repeat(iterations) \
|
||||
for (int iterator = 0; iterator < iterations; iterator++)
|
||||
// END V5 MACROS
|
||||
|
||||
|
||||
// Robot configuration code.
|
||||
controller vexController = controller(primary); // Instance of the VEX controller
|
||||
motor leftMotorA = motor(PORT19, ratio6_1, false);
|
||||
motor leftMotorB = motor(PORT20, ratio6_1, false);
|
||||
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB);
|
||||
motor rightMotorA = motor(PORT9, ratio6_1, true);
|
||||
motor rightMotorB = motor(PORT10, ratio6_1, true);
|
||||
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB);
|
||||
drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 40, mm, 1);
|
||||
|
||||
motor WingsMotorA = motor(PORT15, ratio18_1, true);
|
||||
motor WingsMotorB = motor(PORT5, ratio18_1, false);
|
||||
motor_group Wings = motor_group(WingsMotorA, WingsMotorB);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Forward declaration to resolve circular dependency
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Helper to make playing sounds from the V5 in VEXcode easier and
|
||||
// keeps the code cleaner by making it clear what is happening.
|
||||
void playVexcodeSound(const char *soundName) {
|
||||
printf("VEXPlaySound:%s\n", soundName);
|
||||
wait(5, msec);
|
||||
}
|
||||
|
||||
class CustomController {
|
||||
private:
|
||||
|
||||
bool recording = false;
|
||||
|
||||
struct ControllerState {
|
||||
uint32_t timestamp = 0;
|
||||
int8_t ButtonA = false;
|
||||
int8_t ButtonB = false;
|
||||
int8_t ButtonX = false;
|
||||
int8_t ButtonY = false;
|
||||
int8_t ButtonUp = false;
|
||||
int8_t ButtonDown = false;
|
||||
int8_t ButtonLeft = false;
|
||||
int8_t ButtonRight = false;
|
||||
int8_t ButtonL1 = false;
|
||||
int8_t ButtonL2 = false;
|
||||
int8_t ButtonR1 = false;
|
||||
int8_t ButtonR2 = false;
|
||||
int16_t Axis1 = 0;
|
||||
int16_t Axis2 = 0;
|
||||
int16_t Axis3 = 0;
|
||||
int16_t Axis4 = 0;
|
||||
};
|
||||
struct ControllerState state;
|
||||
struct ControllerState playbackstate;
|
||||
|
||||
struct RecordingData {
|
||||
uint32_t recordinglength = 0;
|
||||
struct ControllerState recording[100000];
|
||||
};
|
||||
struct RecordingData recordingtmp;
|
||||
timer recordingtimer;
|
||||
uint32_t recordingidx = 0;
|
||||
int filenum = 1;
|
||||
bool playback = false;
|
||||
|
||||
void saveRecording(const RecordingData& myStruct, const std::string& filename) {
|
||||
//std::remove(filename.c_str());
|
||||
std::ofstream file(filename, std::ios::binary);
|
||||
if (file.is_open()) {
|
||||
file.write(reinterpret_cast<const char*>(&myStruct), sizeof(RecordingData));
|
||||
file.close();
|
||||
}
|
||||
}
|
||||
|
||||
void loadRecording(RecordingData& myStruct, const std::string& filename) {
|
||||
std::ifstream file(filename, std::ios::binary);
|
||||
if (file.is_open()) {
|
||||
file.read(reinterpret_cast<char*>(&myStruct), sizeof(RecordingData));
|
||||
file.close();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public:
|
||||
// Constructor
|
||||
|
||||
CustomController() {
|
||||
}
|
||||
|
||||
bool startRecording() {
|
||||
if(!recording && !playback) {
|
||||
recording = true;
|
||||
recordingidx = 0;
|
||||
recordingtimer.clear();
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false; // already recording
|
||||
}
|
||||
}
|
||||
|
||||
bool stopRecording() {
|
||||
if(recording) {
|
||||
recording = false;
|
||||
recordingtmp.recordinglength = recordingidx + 1;
|
||||
char filename[1] = "";
|
||||
snprintf(filename, 2, "%d", filenum);
|
||||
std::string fn = filename;
|
||||
fn += ".txt";
|
||||
saveRecording(recordingtmp, fn);
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false; // not recording
|
||||
}
|
||||
}
|
||||
|
||||
bool startPlayback() {
|
||||
if(!playback && !recording) {
|
||||
char filename[1] = "";
|
||||
snprintf(filename, 2, "%d", filenum);
|
||||
std::string fn = filename;
|
||||
fn += ".txt";
|
||||
recordingidx = 0;
|
||||
loadRecording(recordingtmp, fn);
|
||||
recordingtimer.clear();
|
||||
playback = true;
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
inline int32_t record(int32_t value) {
|
||||
if(!recording) return value;
|
||||
state.timestamp = recordingtimer.time();
|
||||
recordingtmp.recording[recordingidx] = state;
|
||||
recordingidx++;
|
||||
wait(1,msec);
|
||||
return value;
|
||||
}
|
||||
|
||||
bool __update_struct() {
|
||||
if(!playback) return false;
|
||||
|
||||
if (recordingidx >= recordingtmp.recordinglength) {
|
||||
stopPlayback();
|
||||
return true;
|
||||
}
|
||||
else if ( recordingtmp.recording[recordingidx].timestamp <= recordingtimer.time() ) {
|
||||
recordingidx++;
|
||||
//playbackstate = recordingtmp.recording[recordingidx];
|
||||
|
||||
//wait(1,msec);
|
||||
return true;
|
||||
} else {
|
||||
playbackstate = recordingtmp.recording[recordingidx];
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void stopPlayback() {
|
||||
playback = false;
|
||||
vexController.Screen.setCursor(3,1);
|
||||
vexController.Screen.clearLine();
|
||||
vexController.Screen.clearScreen();
|
||||
vexController.Screen.print(filenum);
|
||||
}
|
||||
bool update_struct() {
|
||||
while (playback && __update_struct()) {
|
||||
wait(0, msec);
|
||||
}
|
||||
|
||||
/*vexController.Screen.clearLine();
|
||||
vexController.Screen.clearScreen();
|
||||
vexController.Screen.setCursor(2,1);
|
||||
vexController.Screen.print(recordingidx);
|
||||
vexController.Screen.setCursor(3,1);
|
||||
vexController.Screen.print(playbackstate.Axis1);*/
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void recordingloop() {
|
||||
if(vexController.ButtonLeft.pressing() && !recording && !playback) {
|
||||
wait(250, msec);
|
||||
startRecording();
|
||||
vexController.Screen.setCursor(3,1);
|
||||
vexController.Screen.clearLine();
|
||||
vexController.Screen.clearScreen();
|
||||
vexController.Screen.print("Recording");
|
||||
}
|
||||
if(vexController.ButtonLeft.pressing() && recording && !playback) {
|
||||
stopRecording();
|
||||
vexController.Screen.setCursor(3,1);
|
||||
vexController.Screen.clearLine();
|
||||
vexController.Screen.clearScreen();
|
||||
vexController.Screen.print(filenum);
|
||||
wait(250, msec);
|
||||
}
|
||||
if(vexController.ButtonRight.pressing() && !recording && !playback) {
|
||||
wait(250, msec);
|
||||
startPlayback();
|
||||
vexController.Screen.setCursor(3,1);
|
||||
vexController.Screen.clearLine();
|
||||
vexController.Screen.clearScreen();
|
||||
vexController.Screen.print("Playing");
|
||||
}
|
||||
if (vexController.ButtonUp.pressing()) {
|
||||
filenum ++;
|
||||
vexController.Screen.setCursor(3, 1);
|
||||
vexController.Screen.clearLine();
|
||||
vexController.Screen.clearScreen();
|
||||
vexController.Screen.print(filenum);
|
||||
wait(250, msec);
|
||||
}
|
||||
if (vexController.ButtonDown.pressing()) {
|
||||
filenum --;
|
||||
vexController.Screen.setCursor(3, 1);
|
||||
vexController.Screen.clearLine();
|
||||
vexController.Screen.clearScreen();
|
||||
vexController.Screen.print(filenum);
|
||||
wait(250, msec);
|
||||
}
|
||||
|
||||
}
|
||||
// Check if a button is pressed
|
||||
bool ButtonA() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonA;
|
||||
}
|
||||
return record(state.ButtonA = vexController.ButtonA.pressing());
|
||||
}
|
||||
|
||||
bool ButtonB() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonB;
|
||||
}
|
||||
return record(state.ButtonB = vexController.ButtonB.pressing());
|
||||
}
|
||||
|
||||
bool ButtonX() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonX;
|
||||
}
|
||||
return record(state.ButtonX = vexController.ButtonX.pressing());
|
||||
}
|
||||
|
||||
bool ButtonY() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonY;
|
||||
}
|
||||
return record(state.ButtonY = vexController.ButtonY.pressing());
|
||||
}
|
||||
|
||||
bool ButtonUp() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonUp;
|
||||
}
|
||||
return record(state.ButtonUp = vexController.ButtonUp.pressing());
|
||||
}
|
||||
|
||||
bool ButtonDown() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonDown;
|
||||
}
|
||||
return record(state.ButtonDown = vexController.ButtonDown.pressing());
|
||||
}
|
||||
|
||||
bool ButtonLeft() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonLeft;
|
||||
}
|
||||
return record(state.ButtonLeft = vexController.ButtonLeft.pressing());
|
||||
}
|
||||
|
||||
bool ButtonRight() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonRight;
|
||||
}
|
||||
return record(state.ButtonRight = vexController.ButtonRight.pressing());
|
||||
}
|
||||
|
||||
bool ButtonL1() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonL1;
|
||||
}
|
||||
return record(state.ButtonL1 = vexController.ButtonL1.pressing());
|
||||
}
|
||||
|
||||
bool ButtonL2() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonL2;
|
||||
}
|
||||
return record(state.ButtonL2 = vexController.ButtonL2.pressing());
|
||||
}
|
||||
|
||||
bool ButtonR1() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonR1;
|
||||
}
|
||||
return record(state.ButtonR1 = vexController.ButtonR1.pressing());
|
||||
}
|
||||
|
||||
bool ButtonR2() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.ButtonR2;
|
||||
}
|
||||
return record(state.ButtonR2 = vexController.ButtonR2.pressing());
|
||||
}
|
||||
|
||||
// Getters for Axis values
|
||||
int Axis1() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.Axis1;
|
||||
}
|
||||
return record(state.Axis1 = vexController.Axis1.position(percent));
|
||||
}
|
||||
|
||||
int Axis2() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.Axis2;
|
||||
}
|
||||
return record(state.Axis2 = vexController.Axis2.position(percent));
|
||||
}
|
||||
|
||||
int Axis3() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.Axis3;
|
||||
}
|
||||
return record(state.Axis3 = vexController.Axis3.position(percent));
|
||||
}
|
||||
|
||||
int Axis4() {
|
||||
if(playback) {
|
||||
update_struct();
|
||||
return playbackstate.Axis4;
|
||||
}
|
||||
return record(state.Axis4 = vexController.Axis4.position(percent));
|
||||
}
|
||||
|
||||
// Add more functionalities as needed
|
||||
};
|
||||
|
||||
CustomController Controller1;
|
||||
|
||||
// define variable for remote controller enable/disable
|
||||
bool RemoteControlCodeEnabled = true;
|
||||
// define variables used for controlling motors based on controller inputs
|
||||
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
|
||||
bool DrivetrainRNeedsToBeStopped_Controller1 = true;
|
||||
|
||||
// define a task that will handle monitoring inputs from Controller1
|
||||
int rc_auto_loop_function_Controller1() {
|
||||
// process the controller input every 20 milliseconds
|
||||
// update the motors based on the input values
|
||||
while(true) {
|
||||
|
||||
|
||||
if(RemoteControlCodeEnabled) {
|
||||
|
||||
}
|
||||
wait(5, msec);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);
|
||||
|
||||
#pragma endregion VEXcode Generated Robot Configuration
|
||||
// Include the V5 Library
|
||||
|
||||
|
||||
// Allows for easier use of the VEX Library
|
||||
using namespace vex;
|
||||
|
||||
competition Competition;
|
||||
|
||||
float myVariable;
|
||||
|
||||
// "when started" hat block
|
||||
int whenStarted1() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
void drive() {
|
||||
while (true) {
|
||||
//Controller1.recordingloop();
|
||||
// calculate the drivetrain motor velocities from the controller joystick axies
|
||||
// left = Axis3
|
||||
// right = Axis2
|
||||
int drivetrainLeftSideSpeed = Controller1.Axis3();
|
||||
int drivetrainRightSideSpeed = Controller1.Axis2();
|
||||
|
||||
// check if the value is inside of the deadband range
|
||||
if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
|
||||
// check if the left motor has already been stopped
|
||||
if (DrivetrainLNeedsToBeStopped_Controller1) {
|
||||
// stop the left drive motor
|
||||
LeftDriveSmart.stop();
|
||||
// tell the code that the left motor has been stopped
|
||||
DrivetrainLNeedsToBeStopped_Controller1 = false;
|
||||
}
|
||||
} else {
|
||||
// reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
|
||||
DrivetrainLNeedsToBeStopped_Controller1 = true;
|
||||
}
|
||||
// check if the value is inside of the deadband range
|
||||
if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
|
||||
// check if the right motor has already been stopped
|
||||
if (DrivetrainRNeedsToBeStopped_Controller1) {
|
||||
// stop the right drive motor
|
||||
RightDriveSmart.stop();
|
||||
// tell the code that the right motor has been stopped
|
||||
DrivetrainRNeedsToBeStopped_Controller1 = false;
|
||||
}
|
||||
} else {
|
||||
// reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
|
||||
DrivetrainRNeedsToBeStopped_Controller1 = true;
|
||||
}
|
||||
|
||||
// only tell the left drive motor to spin if the values are not in the deadband range
|
||||
if (DrivetrainLNeedsToBeStopped_Controller1) {
|
||||
LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
|
||||
LeftDriveSmart.spin(forward);
|
||||
}
|
||||
// only tell the right drive motor to spin if the values are not in the deadband range
|
||||
if (DrivetrainRNeedsToBeStopped_Controller1) {
|
||||
RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
|
||||
RightDriveSmart.spin(forward);
|
||||
}
|
||||
|
||||
if (Controller1.ButtonR1()) {
|
||||
Wings.setStopping(coast);
|
||||
Wings.setMaxTorque(100.0, percent);
|
||||
Wings.setVelocity(100.0, percent);
|
||||
Wings.spin(forward);
|
||||
//waitUntil((!Controller1.ButtonR1()));
|
||||
}
|
||||
else if (Controller1.ButtonR2()) {
|
||||
Wings.setStopping(coast);
|
||||
Wings.setMaxTorque(100.0, percent);
|
||||
Wings.setVelocity(100.0, percent);
|
||||
Wings.spin(reverse);
|
||||
//waitUntil((!Controller1.ButtonR2()));
|
||||
}
|
||||
else if(Controller1.ButtonX()) {
|
||||
Wings.stop();
|
||||
}
|
||||
wait(5,msec);
|
||||
// wait before repeating the process
|
||||
|
||||
}
|
||||
}
|
||||
// "when autonomous" hat block
|
||||
int onauton_autonomous_0() {
|
||||
Controller1.startPlayback();
|
||||
drive();
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// "when driver control" hat block
|
||||
int ondriver_drivercontrol_0() {
|
||||
Controller1.stopPlayback();
|
||||
drive();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void VEXcode_driver_task() {
|
||||
// Start the driver control tasks....
|
||||
vex::task drive0(ondriver_drivercontrol_0);
|
||||
while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
|
||||
drive0.stop();
|
||||
return;
|
||||
}
|
||||
|
||||
void VEXcode_auton_task() {
|
||||
// Start the auton control tasks....
|
||||
vex::task auto0(onauton_autonomous_0);
|
||||
while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}
|
||||
auto0.stop();
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int main() {
|
||||
vex::competition::bStopTasksBetweenModes = false;
|
||||
Competition.autonomous(VEXcode_auton_task);
|
||||
Competition.drivercontrol(VEXcode_driver_task);
|
||||
|
||||
// register event handlers
|
||||
|
||||
wait(15, msec);
|
||||
// post event registration
|
||||
|
||||
// set default print color to black
|
||||
printf("\033[30m");
|
||||
|
||||
// wait for rotation sensor to fully initialize
|
||||
wait(30, msec);
|
||||
|
||||
whenStarted1();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user