3631Z Replay-ized

ArkRoyal.c

struct controlState {
	signed char leftDrive;	/* Raw Ch3 from stick */
	signed char rightDrive;	/* Raw Ch2 from stick */

	bool linSysUp;			/* Button 7U */
	bool linSysDown;		/* Button 7D */

	bool armUp;				/* Button 6U */
	bool armDown;			/* Button 6D */

	bool clawOpen;			/* Button 5U */
	bool clawClose;			/* Button 5D */
};
controlState currentState;
replayData replay;

/* Reset state (for when switching from auto->driver) */
void resetState() {
	currentState.leftDrive = 0;
	currentState.rightDrive = 0;

	currentState.linSysUp = false;
	currentState.linSysDown = false;
	currentState.armUp = false;
	currentState.armDown = false;
	currentState.clawOpen = false;
	currentState.clawClose = false;
}

/* Completely initialize state (from preauto->auto) */
void initState() {
	resetState();
}

void clearReplay() {
	for(unsigned int i=0;i<10802;i++) {
		replay.streamData[i] = 0;
	}

	replay.streamIndex = 0;
	replay.streamSize = 0;
}


#define TEST_BIT(x, i) (((x)&(1<<(i))) > 0)

void replayToControlState() {
	currentState.leftDrive = (signed char)readNextByte(&replay);
	currentState.rightDrive = (signed char)readNextByte(&replay);

	/* Bit | Button
	 *  0  | Linear System Up (7U)
	 *  1  | Linear System Down (7D)
	 *  2  | Arm Up (6U)
	 *  3  | Arm Down (6D)
	 *  4  | Claw Open (5U)
	 *  5  | Claw Closed (5D)
	 *  6  | <reserved>
	 *  7  | <reserved>
	 */
	unsigned char buttonState = readNextByte(&replay);

	currentState.linSysUp = TEST_BIT(buttonState, 0);
	currentState.linSysDown = TEST_BIT(buttonState, 1);
	currentState.armUp = TEST_BIT(buttonState, 2);
	currentState.armDown = TEST_BIT(buttonState, 3);
	currentState.clawOpen = TEST_BIT(buttonState, 4);
	currentState.clawClose = TEST_BIT(buttonState, 5);
}

void controllerToControlState() {
	currentState.leftDrive = vexRT[Ch3];
	currentState.rightDrive = vexRT[Ch2];

	currentState.linSysUp = (vexRT[Btn7U] > 0);
	currentState.linSysDown = (vexRT[Btn7D] > 0);
	currentState.armUp = (vexRT[Btn6U] > 0);
	currentState.armDown = (vexRT[Btn6D] > 0);
	currentState.clawOpen = (vexRT[Btn5U] > 0);
	currentState.clawClose = (vexRT[Btn5D] > 0);
}

void controlStateToReplay() {
	writeByte(&replay, (unsigned char)currentState.leftDrive);
	writeByte(&replay, (unsigned char)currentState.rightDrive);

	unsigned char buttonState = 0;
	buttonState |= (currentState.linSysUp ? 1 : 0);
	buttonState |= (currentState.linSysDown ? 1 : 0) << 1;
	buttonState |= (currentState.armUp ? 1 : 0) << 2;
	buttonState |= (currentState.armDown ? 1 : 0) << 3;
	buttonState |= (currentState.clawOpen ? 1 : 0) << 4;
	buttonState |= (currentState.clawClose ? 1 : 0) << 5;

	writeByte(&replay, buttonState);
}

void stopAllMotorsCustom() {
		motor[LBdrive] = 0;
		motor[LFdrive] = 0;
		motor[RBdrive] = 0;
		motor[RFdrive] = 0;

		motor [LBarm] = 0;
		motor [LTarm]= 0;
		motor [RBarm]= 0;
		motor [RTarm]= 0;

		motor [slide]= 0;

		motor[claw] = 0;
}

int speedLimit = 115; // this is the max speed of the base

void driveControl() {
	if(abs(currentState.leftDrive) <= speedLimit) {
		motor[LBdrive] = currentState.leftDrive;
		motor[LFdrive] = currentState.leftDrive;
	} else {
		motor[LBdrive] = sgn(currentState.leftDrive) * speedLimit;
		motor[LFdrive] = sgn(currentState.leftDrive) * speedLimit;
	}

	if(abs(currentState.rightDrive) <= speedLimit) {
		motor[RBdrive] = currentState.rightDrive;
		motor[RFdrive] = currentState.rightDrive;
	} else {
		motor[RBdrive] = sgn(currentState.rightDrive) * speedLimit;
		motor[RFdrive] = sgn(currentState.rightDrive) * speedLimit;
	}
}

void armControl() {
	if(currentState.armUp) {  //arm lifts
		motor [LBarm] = 50;
		motor [LTarm] = 50;
		motor [RBarm] = 50;
		motor [RTarm] = 50;
	} else if(currentState.armDown) { //arm decends
		motor [LBarm] = -50;
		motor [LTarm] = -50;
		motor [RBarm] = -50;
		motor [RTarm] = -50;
	} else {
		motor [LBarm] = 0;
		motor [LTarm]= 0;
		motor [RBarm]= 0;
		motor [RTarm]= 0;
	}
}

void clawControl() {
	if(currentState.clawOpen) {  //claw opens
		motor[claw] = -30;
	} else if(currentState.clawClose) {//claw closes
		motor[claw] = 30;
	} else {
		motor[claw] = 0;
	}
}

void linSysControl() {
	if(currentState.linSysUp) { //linear system lifts
		motor [slide] = 127;
	} else if(currentState.linSysDown) { //linear system decends
		motor [slide] = -127;
	} else {
		motor [slide]= 0;
	}
}


void controlLoopIteration() {
	driveControl();
	armControl();
	clawControl();
	linSysControl();
}

CompetitionControl.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_7,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_8,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           LBdrive,       tmotorVex393HighSpeed_HBridge, openLoop, driveLeft, encoderPort, I2C_1)
#pragma config(Motor,  port2,           LFdrive,       tmotorVex393HighSpeed_MC29, openLoop, driveLeft, encoderPort, I2C_2)
#pragma config(Motor,  port3,           LBarm,         tmotorVex393_MC29, openLoop, encoderPort, I2C_3)
#pragma config(Motor,  port4,           LTarm,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           claw,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           RTarm,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           RBarm,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           slide,         tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_6)
#pragma config(Motor,  port9,           RFdrive,       tmotorVex393HighSpeed_MC29, openLoop, reversed, driveRight, encoderPort, I2C_7)
#pragma config(Motor,  port10,          RBdrive,       tmotorVex393HighSpeed_HBridge, openLoop, reversed, driveRight, encoderPort, I2C_8)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

#include "./Enterprise.c"
#include "./ArkRoyal.c"
/* Competition control stub. */

void loadAutonomous(replayData* replay) {
		writeDebugStreamLine("Loading: slot1");
		loadReplayFromFile("slot1", replay);
}

/*
void loadAutonomous(replayData* replay) {
	int pos = sensorValue[autoSelector];

	if(pos < 727) {		// Illuminati Skills
		writeDebugStreamLine("Loading: ilmskills");
		loadReplayFromFile("ilmskills", replay);
	} else if(pos < 1920) {	// Illuminati routine
		writeDebugStreamLine("Loading: ilmroutine");
		loadReplayFromFile("ilmroutine", replay);
	} else if(pos < 2678) {	// Off
		return;
	} else if(pos < 3200) {	// A1
		writeDebugStreamLine("Loading: slot1");
		loadReplayFromFile("slot1", replay);
	} else if(pos < 3768) { // A2
		writeDebugStreamLine("Loading: slot2");
		loadReplayFromFile("slot2", replay);
	} else if(pos > 4080) {	// A3
		writeDebugStreamLine("Loading: slot3");
		loadReplayFromFile("slot3", replay);
	}

	clearLCDLine(0);
	displayLCDCenteredString(0, "Load done.");
	writeDebugStreamLine("Loading done.");
}
*/

void pre_auton()
{
}

task autonomous() {
  clearReplay();
  loadAutonomous(&replay);

	initState();
	while(replay.streamIndex < replay.streamSize) {
		replayToControlState();
		controlLoopIteration();
		sleep((int)deltaT);
	}

	stopAllMotorsCustom();
}

task usercontrol()
{
	resetState();

	while (true)
	{
		controllerToControlState();
		controlLoopIteration();
		sleep((int)deltaT);
	}
}

Enterprise.c

#define MAX_FLASH_FILE_SIZE 10810
#include "./rcfs-master/FlashLib.h"

const float snapshotFreq = 30; // Hz
const float deltaT = (1/snapshotFreq) * 1000; // time between snapshots in milliseconds

struct replayData {
	unsigned char streamData[10802];
	unsigned int streamIndex;
	unsigned int streamSize;
	bool loaded;
};

void initReplayData(replayData* data) {
	data->streamIndex = 0;
	data->streamSize = 0;
	data->loaded = false;
}

unsigned char readNextByte(replayData* data) {
	unsigned char ret = data->streamData[data->streamIndex];
	data->streamIndex += 1;
	return ret;
}

void writeByte(replayData* data, unsigned char dat) {
	data->streamData[data->streamIndex] = dat;
	data->streamIndex += 1;
}

bool findFile(char* name, flash_file* out) {
	flash_file cur;
	bool foundAFile = false;
	int currentSlot = 0;
	int selectedSlot = -1;

	RCFS_FileInit(&cur);

	if(RCFS_FindFirstFile(&cur) < 0)
		return false;

	do {
		if( strcmp(name, &(cur.name[0])) == 0 ) {
			// found a file, write it out
			/* *out = cur; */
			memcpy(out, &cur, FLASH_FILE_HEADER_SIZE);
			out->addr = cur.addr;
			out->data = cur.data;
			out->datalength = cur.datalength;

			foundAFile = true;
			selectedSlot = currentSlot;
		}
		currentSlot = RCFS_FindNextFile(&cur);
#ifdef DEBUG
		writeDebugStreamLine("Inspecting file %i", currentSlot);
#endif
	} while(currentSlot >= 0);

	if(!foundAFile) {
		RCFS_FileInit(out); // clear the out datastructure
	}	else {
#ifdef DEBUG
		writeDebugStreamLine("Selecting file in slot %i", selectedSlot);
#endif
		RCFS_ReadHeader(out);
	}

	return foundAFile;
}

/*
 * Stream on-flash file format (current):
 *  2 bytes: stream size in bytes
 *  n bytes: stream data
 */

void saveReplayToFile(char* name, replayData* repSt) {
	// discarded parameters to RCFS_GetFile
	unsigned char* tmp1;
	int tmp2;

#ifdef DEBUG
	writeDebugStreamLine("Killed motors, now finding file:");
	writeDebugStreamLine(name);
#endif

	repSt->streamSize = repSt->streamIndex+1;

	//flash_file fHandle;
	//findFile(name, &fHandle);
	//RCFS_FileInit(&fHandle);
	//if(fHandle.addr == NULL) {
#ifdef DEBUG
		writeDebugStreamLine("Adding to FS.");
#endif

		repSt->streamData[0] = (repSt->streamSize & 0xFF);
		repSt->streamData[1] = ((repSt->streamSize & 0xFF00) >> 8) & 0xFF;

		clearLCDLine(0);
		clearLCDLine(1);
		displayLCDCenteredString(0, "! WRITING !");
#ifdef DEBUG
		writeDebugStreamLine("BEGINNING WRITE!");
#endif
		signed int err = 0;
		if((err = RCFS_AddFile((unsigned char*)repSt->streamData, 10802, name)) < 0) {
			clearLCDLine(0);
			displayLCDCenteredString(0, "Write failed!");
#ifdef DEBUG
			writeDebugStreamLine("Write failed, code: %d", err);
#endif
		}
	//}
	/*else {
#ifdef DEBUG
		writeDebugStreamLine("Found existing file.");
#endif
		fHandle.data[0] = (repSt->streamSize & 0xFF);
		fHandle.data[1] = ((repSt->streamSize & 0xFF00) >> 8) & 0xFF;

		for(unsigned int i=2;i<repSt->streamSize;i++) {
			fHandle.data[i] = repSt->streamData[i];
		}

		clearLCDLine(0);
		clearLCDLine(1);
		displayLCDCenteredString(0, "! WRITING !");
#ifdef DEBUG
		writeDebugStreamLine("BEGINNING WRITE!");
#endif
		RCFS_Write(&fHandle);

		clearLCDLine(0);
		clearLCDLine(1);
		displayLCDCenteredString(0, "Write complete.");
	}
	*/
	clearLCDLine(0);
	clearLCDLine(1);
	displayLCDCenteredString(0, "Write complete.");
#ifdef DEBUG
	writeDebugStreamLine("Write complete.");
#endif
}

void loadReplayFromFile(const char* name, replayData* repSt) {
	clearLCDLine(0);
	clearLCDLine(1);
#ifdef DEBUG
	writeDebugStreamLine("Attempting to load stream:");
	writeDebugStreamLine(name);
#endif
	displayLCDCenteredString(0, "Finding file...");

	flash_file fHandle;
	findFile(name, &fHandle);
	if(fHandle.addr != NULL) {
#ifdef DEBUG
		writeDebugStreamLine("Found file!");
#endif

		displayLCDCenteredString(0, "Loading replay...");

		unsigned int streamSz = (fHandle.data[0] | (((unsigned int)(fHandle.data[1])) << 8));
		for(unsigned int i=0;i<streamSz;i++) {
			repSt->streamData[i] = fHandle.data[i];
		}
		repSt->streamSize = streamSz;
#ifdef DEBUG
		writeDebugStreamLine("Loaded %i bytes.", streamSz);
#endif
	repSt->loaded = true;
	} else {
		clearLCDLine(0);
		displayLCDCenteredString(0, "File not found!");
#ifdef DEBUG
		writeDebugStreamLine("Could not find file.");
#endif
	}
}

Recorder.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_7,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_8,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           LBdrive,       tmotorVex393HighSpeed_HBridge, openLoop, driveLeft, encoderPort, I2C_1)
#pragma config(Motor,  port2,           LFdrive,       tmotorVex393HighSpeed_MC29, openLoop, driveLeft, encoderPort, I2C_2)
#pragma config(Motor,  port3,           LBarm,         tmotorVex393_MC29, openLoop, encoderPort, I2C_3)
#pragma config(Motor,  port4,           LTarm,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           claw,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           RTarm,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           RBarm,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           slide,         tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_6)
#pragma config(Motor,  port9,           RFdrive,       tmotorVex393HighSpeed_MC29, openLoop, reversed, driveRight, encoderPort, I2C_7)
#pragma config(Motor,  port10,          RBdrive,       tmotorVex393HighSpeed_HBridge, openLoop, reversed, driveRight, encoderPort, I2C_8)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

#define DEBUG

#include "./Enterprise.c"
#include "./ArkRoyal.c"
/* Recorder control stub. */

void loadAutonomous(replayData* replay) {
	/*
	int pos = sensorValue[autoSelector];

	if(pos < 727) {		// Illuminati Skills
		writeDebugStreamLine("Loading: ilmskills");
		loadReplayFromFile("ilmskills", replay);
	} else if(pos < 1920) {	// Illuminati routine
		writeDebugStreamLine("Loading: ilmroutine");
		loadReplayFromFile("ilmroutine", replay);
	} else if(pos < 2678) {	// Off
		return;
	} else if(pos < 3200) {	// A1
		writeDebugStreamLine("Loading: slot1");
		loadReplayFromFile("slot1", replay);
	} else if(pos < 3768) { // A2
		writeDebugStreamLine("Loading: slot2");
		loadReplayFromFile("slot2", replay);
	} else if(pos > 4080) {	// A3
		writeDebugStreamLine("Loading: slot3");
		loadReplayFromFile("slot3", replay);
	}
	*/
	writeDebugStreamLine("Loading: slot1");
	loadReplayFromFile("slot1", replay);

	clearLCDLine(0);
	displayLCDCenteredString(0, "Load done.");
	writeDebugStreamLine("Loading done.");
}

void saveAutonomous(replayData* replay) {
	writeDebugStreamLine("Saving: slot1");
	saveReplayToFile("slot1", replay);
	/*
	int pos = sensorValue[autoSelector];

	if(pos < 727) {		// Illuminati Skills
		writeDebugStreamLine("Saving: ilmskills");
		saveReplayToFile("ilmskills", replay);
	} else if(pos < 1920) {	// Illuminati routine
		writeDebugStreamLine("Saving: ilmroutine");
		saveReplayToFile("ilmroutine", replay);
	} else if(pos < 2678) {	// Off
		return;
	} else if(pos < 3200) {	// A1
		writeDebugStreamLine("Saving: slot1");
		saveReplayToFile("slot1", replay);
	} else if(pos < 3768) { // A2
		writeDebugStreamLine("Saving: slot2");
		saveReplayToFile("slot2", replay);
	} else if(pos > 4080) {	// A3
		writeDebugStreamLine("Saving: slot3");
		saveReplayToFile("slot3", replay);
	}
	*/

	clearLCDLine(0);
	displayLCDCenteredString(0, "Save done.");
	writeDebugStreamLine("Saving done.");
}

void pre_auton() {
}

task autonomous() {
	clearReplay();

	loadAutonomous(&replay);

	replay.streamIndex = 0;
	initState();

	while(replay.streamIndex < replay.streamSize) {
		replayToControlState();
		controlLoopIteration();
		sleep((int)deltaT);
	}

	stopAllMotorsCustom();
}

unsigned int currentTime = 0;

/* Max recording time in milliseconds.
 *
 *  For a regular match autonomous recording, this should be 15000 milliseconds (= 15 seconds).
 *  For an Auto Skills recording, this should be 60000 milliseconds (= 60 seconds = 1 minute).
 */
unsigned int timelimit = 61000;
unsigned int deadband = 25;

task usercontrol()
{
	initState();
	clearReplay();

	clearLCDLine(0);
	clearLCDLine(1);
	displayLCDCenteredString(0, "Ready to record.");
	displayLCDCenteredString(1, "Do stuff.");

	while (true)
	{
		if(abs(vexRT[Ch2]) > deadband || abs(vexRT[Ch3]) > deadband) {
			break;
		}

		if(vexRT[Btn7U] || vexRT[Btn7D] || vexRT[Btn6U] ||
		   vexRT[Btn6D] || vexRT[Btn5U] || vexRT[Btn5D])
		{
			break;
		}

		sleep(5);
	}

	clearLCDLine(0);
	clearLCDLine(1);
	displayLCDCenteredString(0, "Recording...");

	while (true)
	{
		controllerToControlState();
		controlLoopIteration();

		if(timelimit > 0) {
			controlStateToReplay();
		}

		currentTime += (int)deltaT;

		if(vexRT[Btn7L]) {
			break;
		}

		if((currentTime > timelimit) && (timelimit > 0)) {
			break;
		}

		sleep((int)deltaT);
	}

	replay.streamSize = replay.streamIndex+1;

	stopAllMotorsCustom();

	bool doSave = false;
	while(true) {
		displayLCDCenteredString(0, "Save replay?");
		if(!doSave) {
			displayLCDCenteredString(1, "[No] Yes ");
		} else {
			displayLCDCenteredString(1, " No [Yes]");
		}

		if(nLCDButtons & 0x01) {
			doSave = false;
		} else if(nLCDButtons & 0x02) {
			break;
		} else if(nLCDButtons & 0x04) {
			doSave = true;
		}
	}

	if(doSave) {
		saveAutonomous(&replay);
	}
}