Replay motions -- how do you really want this to work?

The idea with this code is that an advanced programmer that had very little time to program the robot (I’m sure none of us have been there) could program this at home, tweak it when he gets access to the robot, then have the driver drive each autonomous so it will probably save time (depending on how many autonomous you want to create) and it will definitely save time if the hardware people want to change any of the robot and you have to change your code.

@ShadowIQ you are not entirely wrong… trying to capture at 50ms intervals lead to lost data. I suspect there is a frequency that may be optimal given the hardware, anyone know it? I set the interval to 100ms and am not losing data, but the fastest safe speed would be nice to know.

@OverlyOptimisticProgramer Adding sensors would be great! How are you coming along with that project?

I’m not sure of the protocol of posting longish code chunks, so hope this isn’t triggering anyone… The iteration below works with a test sample of one simple bot. It has the frequency issue and the typical failings of time-based code. I did add an adjustment for battery voltage at playback, but its a total guess and might be worse than no adjustment.

Something that would be great is a parser to turn it into more typical timed auton code so it could be tweaked after the recording (and take up less lines of code.)

If people could test this on more builds and report back, that would be great.


// ---->>> replace this comment with motor setup code <<<----

#pragma DebuggerWindows("debugStream")

// HOW TO USE THIS RECORDER:
// 1. Look for the THREE ---->>> paste X here <<<---- comments and paste the described code from your competition template.
// 2. Have a game field set up and a driver ready to drive the bot for an autonomous run.
// 3. While using the programming cable with the controller connected to the robot via vexNet, download this code.
// 4. With the debug stream window open, press start to run the code. There will be a 3 second count down, then 15 seconds to drive.
// 5. Copy the text from the debug stream window into your autonomous task. (There will be 305 lines of code to copy.)
// 6. Copy the following variables and functions into your competition template above the pre-autonomous task.

/*---- START COPY --------------------------------------------------------------------------------------------*/

int R_FREQ = 100; // 100 the frequency of the recording in milisecons. A lower number will attempt to capture more data, but may result in errors in the recording
int R_Batt; // the battery voltage at the time of the recording.
int P_Freq; // the replay frequency in miliseconds
int lineCounter = 0; // the number of each line of recorded code
int timeCounter = 0 * R_FREQ; // the time in mS of each line
int C1, C2, C3, C4, B5D, B5U, B6D, B6U, B7D, B7L, B7R, B7U, B8D, B8L, B8R, B8U; // signal values from main controller 
int xC1, xC2, xC3, xC4, xB5D, xB5U, xB6D, xB6U, xB7D, xB7L, xB7R, xB7U, xB8D, xB8L, xB8R, xB8U; // signal values from partner controller 


// function to assess battery voltage and adjust replay frequency
void replayFreq()
{
	P_Freq = R_FREQ * (R_Batt / nImmediateBatteryLevel);	//  with R_FREQ at 100, there would be ~1mS difference in playback per 80mV difference in voltage
	// recording at 8200mV and playback at 7200mV would make P_Freq = 113mS, meaning the last ~2 secs would be stopped by competition control
	// what is the real relationship of voltage and speed?  would vary greatly by robot weight and geomoetry.  Add a constant for teams to tune?
}

// function to record main controller channel signals to globals
void main_C(int C1_now, int C2_now, int C3_now, int C4_now)
{
	C1 = C1_now;
	C2 = C2_now;
	C3 = C3_now;
	C4 = C4_now;
}

// function to record main controller button signals to globals
void main_B(int B5D_now, int B5U_now, int B6D_now, int B6U_now, int B7D_now, int B7L_now, int B7R_now, int B7U_now, int B8D_now, int B8L_now, int B8R_now, int B8U_now)
{
	B5D = B5D_now;
	B5U = B5U_now;
	B6D = B6D_now;
	B6U = B6U_now;
	B7D = B7D_now;
	B7L = B7L_now;
	B7R = B7R_now;
	B7U = B7U_now;
	B8D = B8D_now;
	B8L = B8L_now;
	B8R = B8R_now;
	B8U = B8U_now;
}

// function to record partner controller channel signals to globals
void partner_C(int xC1_now, int xC2_now, int xC3_now, int xC4_now)
{
	xC1 = xC1_now;
	xC2 = xC2_now;
	xC3 = xC3_now;
	xC4 = xC4_now;
}

// function to record partner controller button signals to globals
void partner_B(int xB5D_now, int xB5U_now, int xB6D_now, int xB6U_now, int xB7D_now, int xB7L_now, int xB7R_now, int xB7U_now, int xB8D_now, int xB8L_now, int xB8R_now, int xB8U_now)
{
	xB5D = xB5D_now;
	xB5U = xB5U_now;
	xB6D = xB6D_now;
	xB6U = xB6U_now;
	xB7D = xB7D_now;
	xB7L = xB7L_now;
	xB7R = xB7R_now;
	xB7U = xB7U_now;
	xB8D = xB8D_now;
	xB8L = xB8L_now;
	xB8R = xB8R_now;
	xB8U = xB8U_now;
}

// function to set motor values to recorded signals
void replayVexRT()
{
	// ---->>> replace this comment with your user control code, everything within the while(true) brackets <<<----
	// replace all the vexRT] statements with the corresponding variable (note: buttons 7U and 8U cannot be used in this code)
	// i.e. change "motor[port1] = vexRT[Ch3];" to "motor[port1] = C3;"
	// if your button code is written as a boolean test for true or false, change true to 1 and false to 0
	wait1Msec(P_Freq);
}
/*-----------------------------------------------------------------------------------END COPY-------*/


// task to record vexRT signals to the debug stream once every R_FREQ ms
task recordVexRT()
{
	while(true)
	{
		// main controller values
		C1 = vexRT[Ch1];
		C2 = vexRT[Ch2];
		C3 = vexRT[Ch3];
		C4 = vexRT[Ch4];
		B5D = vexRT[Btn5D];
		B5U = vexRT[Btn5U];
		B6D = vexRT[Btn6D];
		B6U = vexRT[Btn6U];
		B7D = vexRT[Btn7D];
		B7L = vexRT[Btn7L];
		B7R = vexRT[Btn7R];
		B7U = vexRT[Btn7U];
		B8D = vexRT[Btn8D];
		B8L = vexRT[Btn8L];
		B8R = vexRT[Btn8R];
		B8U = vexRT[Btn8U];

		// partner controller values
		xC1 = vexRT[Ch1Xmtr2];
		xC2 = vexRT[Ch2Xmtr2];
		xC3 = vexRT[Ch3Xmtr2];
		xC4 = vexRT[Ch4Xmtr2];
		xB5D = vexRT[Btn5DXmtr2];
		xB5U = vexRT[Btn5UXmtr2];
		xB6D = vexRT[Btn6DXmtr2];
		xB6U = vexRT[Btn6UXmtr2];
		xB7D = vexRT[Btn7DXmtr2];
		xB7L = vexRT[Btn7LXmtr2];
		xB7R = vexRT[Btn7RXmtr2];
		xB7U = vexRT[Btn7UXmtr2];
		xB8D = vexRT[Btn8DXmtr2];
		xB8L = vexRT[Btn8LXmtr2];
		xB8R = vexRT[Btn8RXmtr2];
		xB8U = vexRT[Btn8UXmtr2];

		// increment lineCounter and timeCounter
		lineCounter++;
		timeCounter = lineCounter * R_FREQ;

		// write the values from the  main controller to the debug stream window
		writeDebugStream("main_C(%d", C1);
		writeDebugStream(",%d", C2);
		writeDebugStream(",%d", C3);
		writeDebugStream(",%d", C4); // break signals into channel and button groups
		writeDebugStream("); main_B(%d", B5D);
		writeDebugStream(",%d", B5U);
		writeDebugStream(",%d", B6D);
		writeDebugStream(",%d", B6U);
		writeDebugStream(",%d", B7D);
		writeDebugStream(",%d", B7L);
		writeDebugStream(",%d", B7R);
		writeDebugStream(",%d", B7U);
		writeDebugStream(",%d", B8D);
		writeDebugStream(",%d", B8L);
		writeDebugStream(",%d", B8R);
		writeDebugStream(",%d", B8U);
		writeDebugStream(");  ");

		// write the values from the partner controller to the debug stream window
		writeDebugStream("partner_C(%d", xC1);
		writeDebugStream(",%d", xC2);
		writeDebugStream(",%d", xC3);
		writeDebugStream(",%d", xC4);
		writeDebugStream("); partner_B(%d", xB5D);
		writeDebugStream(",%d", xB5U);
		writeDebugStream(",%d", xB6D);
		writeDebugStream(",%d", xB6U);
		writeDebugStream(",%d", xB7D);
		writeDebugStream(",%d", xB7L);
		writeDebugStream(",%d", xB7R);
		writeDebugStream(",%d", xB7U);
		writeDebugStream(",%d", xB8D);
		writeDebugStream(",%d", xB8L);
		writeDebugStream(",%d", xB8R);
		writeDebugStream(",%d", xB8U);
		writeDebugStream(");  ");

		writeDebugStreamLine("replayVexRT(); // %d", timeCounter);

		wait1Msec(R_FREQ);

	}
}

task main()
{
	// clear previous recordings
	clearDebugStream();
	wait1Msec(100);
	// save current battery voltage to R_Batt
	R_Batt = nImmediateBatteryLevel;

	// countdown in debug stream
	writeDebugStreamLine("// Begin recording in: 3");
	wait1Msec(1000);
	writeDebugStreamLine("// Begin recording in: 2");
	wait1Msec(1000);
	writeDebugStreamLine("// Begin recording in: 1");
	wait1Msec(1000);

	// write header for autonomous code
	writeDebugStreamLine("// BEGIN AUTONOMOUS RECORDING");
	writeDebugStreamLine("// Copy the following lines into your autonomous task:");
	writeDebugStreamLine("// R_FREQ = %d", R_FREQ);
	writeDebugStream("R_Batt = %d", R_Batt);
	writeDebugStreamLine(";");
	writeDebugStreamLine("replayFreq();");

	// start recording the vexRT signals
	startTask (recordVexRT);

	// allow driver control for 15 seconds
	clearTimer(T1);
	while (time1[T1] < 15000)
	{
	// ---->>> replace this comment with your user control code, everything within the while(true) brackets <<<----
	}

	// stop recording the vexRT signals
	stopTask (recordVexRT);
	writeDebugStreamLine("motor[port1] = motor[port2] = motor[port3] = motor[port4] = motor[port5] = motor[port6] = motor[port7] = motor[port8] = motor[port9] = motor[port10] = 0;");
	writeDebugStreamLine("// END AUTON RECORDING");

	// stop all motors
	motor[port1] = motor[port2] = motor[port3] = motor[port4] = motor[port5] = motor[port6] = motor[port7] = motor[port8] = motor[port9] = motor[port10] = 0;
}

I’ll try to test out the timer based on Thursday and the sensor based code on Friday… the timer code first because we’ll be needing a 100% working transcriber on Monday…

Some info in this thread, it should be valid but it has changed occasionally with firmware version.
https://vexforum.com/t/vexnet-2-0-data-rate-study/28419/1

@Doug Moyers this is the side of programming I try to avoid because of my lack of knowledge of how electronics work but here is my swing at it: ~4k bytes per second from the cortex to the PC at 2 bytes a character leads to 2000 characters/second… (assuming maximum efficiency which I doubt is the case) to put that in perspective, @Doug Moyers’s latest code spewed out ~140-150 characters… which means update rate, at best, 70-75 ms… I’m doing 100ms to be safe…

In 2014, I was working with Steve on a “re-run” for 2915a’s toss up robot. He used this program at the world championships, where it worked surprisingly well until he changed his drive motors (without recording his autonomous programs again) and everything turned to a disaster.

The first iteration of the program was based off of time and joystick values like some of you have mentioned. This proved to be inaccurate because of inconsistencies with battery voltages and motors wearing out/getting weaker as they are worn in.

The second iteration was based off of a PID controller which took snapshots of the position of the wheels in encoder ticks every x ms. This again proved to be below average because of accumulative error, and there were problems getting control loop to work to a standard we were happy with because of the ever changing target.

What we had work the best was a hybrid of the two with velocity targets instead of positional targets, and essentially stripping the PID down to just the proportional component. This mean’t that we could essentially run the robot off the joystick values to try and hit a target RPM, and then offset the values with the error. In regards to the rate of sampling, we found that there was a sweet spot of around 50ms. I’m not sure why, probably a mixture between what you guys have discussed and the reactivity of the control loop we used.

Here are a couple of auton programs using it:


If you look up the games from 2014 worlds you will probably see some others if you are interested.

I don’t think there will be an out of the box solution that will work for every robot. Every robot is different.

I have in the past held back in posting the code for this specific rerun because of a number of reasons (both competitive advantage and not wanting to give people the answers), but at this point if you want to get a similar algorithm to run on your robot you will have to understand it pretty well anyway so I don’t think there is much harm in making it public. You can find the 2014 code here: https://github.com/Jackbk/vex

It certainly won’t work out of the box, but I hope some people will be able to take inspiration from it and improve on the theory behind it. If I were to continue with it I would probably start by improving how the data is stored, as well as having some sort of memory of how accurate the last autonomous run was, and to try to correct for the error it recorded the previous time.

I should probably mention that it wasn’t as accurate as conventional programming, but it was much faster and we could achieve a lot more in the autonomous period. Lining up on walls/bump/goals helped a lot with accuracy during the run.

EDIT: fixed description of iteration 3 because I had it confused with something else I wrote and it was wrong

@jacko it sounds like your autonomous in toss up needed to be exact so do you think that we need the same level of accuracy for this tournament? If we need less accuracy this tournament, would the ideas behind your first or second iteration work?

That kind of depends on your intake. If you have a big claw that is more than big enough to pick stuff up then you don’t really need to be really accurate but from personal experience you need to be pretty accurate when picking things up. It was mentioned that the first iteration used time and so was affected by battery voltage but In my opinion if you have something that doesn’t require extreme accuracy then i would go for it :slight_smile:
And yes i know i’m not jacko :stuck_out_tongue:

Well that depends on how accurate you wan’t your autonomous to be. If you are happy with the accuracy of an equivalent time based auton then it would probably work for you (ie drive forward for 2 seconds, lift arm, drive forwards for 1 second). It would work, just maybe not all of the time. If it was me, I would certainly want more accuracy than a time based program would give.

So I just tested timer based rerun… at first it seemed like it worked great… then we drove it around and programmed other autonomouses for a bit then tested it again… it fell apart completely. We tried a fresh battery, but the autonomous fell apart again. Morale of the story: timer based does not work unless you are doing something super simple… in which case you should just program it by hand…

What does fall apart mean? Steps not replayed, or the normal issue of timed code being further and further off the longer it is?

While I was thinking about how to parse the signal code to motor code, I figured out a better way to do all this. The following generates motor code with waits only when there are changes… meaning it looks more like normal time based code. It also is efficient enough that I did not have issues with 50ms sampling (testing on a 5 motor bot.)


// -------->>>   replace this comment with motor and sensor setup code   <<<--------- //

#pragma DebuggerWindows("debugStream")

// HOW TO USE THIS RECORDER:
// 1. Paste the motor control code from your competition template at line 1 above.
// 2. Paste the user control code from your competition template in the userControlCode function
// 3. Adjust R_mSec and R_Freq values if you want a different length or frequency of recording
// 4. Have a game field set up and a driver ready to drive the bot for an autonomous run.
// 5. While using the programming cable with the controller connected to the robot via vexNet, download this code.
// 6. With the debug stream window open, press start to run the code. There will be a 3 second count down before user control starts.
// 7. Copy the text from the debug stream window into your autonomous task.
// 8. Copy the variables and function between /*---- START COPY -- and --END COPY-------*/ into your competition template above the pre-autonomous task.

int R_mSec = 15000; // 15000, length of autonomous redording in miliseconds

int loopCounter; // the number of each loop of code
int timeCounter; // the time in mS of each loop
int motorValue[10]; // array of motor values updated in userControlCode
int motorValue_Last[10]; // motor values of previous loop
bool motorChange = false; // any motor change within the last loop sets to true
int	loopOfLastChange; // last loop any motor value changed
int	timeSinceLastChange; // calculate each loop motorChange = false

/*---- START COPY -----------------------------------------------------------------------------------------------------------------------*/

int R_Batt; // the battery voltage at the time of the recording.
int R_Freq = 50; // 50 the frequency of the recording in milisecons. A lower number will attempt to capture more data, but may result in errors in the recording
int P_Freq = 50; // the replay frequency in miliseconds, calculated in replayFreq()

// function to assess battery voltage and adjust replay frequency
void replayFreq()
{
	// with R_Freq at 100, there would be ~1mS difference in playback per 80mV difference in voltage
	// recording at 8200mV and playback at 7200mV would make P_Freq = 113mS, meaning the last ~2 secs would be stopped by competition control
	P_Freq = R_Freq * (R_Batt / nImmediateBatteryLevel);
}

/*-------------------------------------------------------------------------------------------------------------------------END COPY-------*/

// function to hold user control code
void userControlCode()
{

	// ---->>>       replace this comment with your user control code, everything within the while(true) brackets        <<<---- //
	// ---->>>  if you are using global variables or fucntions, they must be coppied into this code above this function  <<<---- //

	for (int m = 0; m < 10; m++)
	{
		motorValue[m] = motor[m];
	}
}

// task to record motor value changes to the debug stream
task recordMotorValues()
{
	// initialize arrays to 0
	for (int m = 0; m < 10; m++)
	{
		motorValue[m] = 0;
		motorValue_Last[m] = 0;
	}

	while(true)
	{
		// increment loopCounter
		loopCounter++;

		for (int m = 0; m < 10; m++)
		{
			if(motorValue[m] != motorValue_Last[m]) // if motorValue changes from last loop
			{
				writeDebugStream("	motor port%d", m+1); // write changed values to the debugStream as motor] commands
				writeDebugStream(" ] = %d", motorValue[m]);
				writeDebugStream("; ");
				motorChange = true;
			}
		}

		if ( motorChange == true ) // if any motor value changed this loop
		{
			timeSinceLastChange = (loopCounter - loopOfLastChange) * R_Freq ; // calculate elapsed time since last change
			timeCounter = loopCounter * R_Freq; // calculate current run time of autonomous
			writeDebugStream("	wait1Msec( P_Freq * %d", (loopCounter - loopOfLastChange));	// end the line of code with a wait equal to the time since the last motor change
			writeDebugStream(" ); // %d", timeSinceLastChange);	// end the line of code with a wait equal to the time since the last motor change
			writeDebugStreamLine(" wait");
			writeDebugStreamLine(" ");	// line break after wait
			writeDebugStreamLine("// %d", timeCounter);	//print the current time of the autonomous add line break between actions
			loopOfLastChange = loopCounter; // restart loopsSince count
		}

		// update motorVaule_Last
		for (int m = 0; m < 10; m++)
		{
			motorValue_Last[m] = 	motorValue[m];
		}
		motorChange = false;
		wait1Msec(R_Freq);
	}
}

task main()
{
	// clear previous recordings
	clearDebugStream();
	wait1Msec(100);
	// save current battery voltage to R_Batt
	R_Batt = nImmediateBatteryLevel;

	// countdown in debug stream
	writeDebugStreamLine("// Begin recording in: 3");
	wait1Msec(1000);
	writeDebugStreamLine("// Begin recording in: 2");
	wait1Msec(1000);
	writeDebugStreamLine("// Begin recording in: 1");
	wait1Msec(1000);

	// write header for autonomous code
	writeDebugStreamLine("// BEGIN AUTONOMOUS RECORDING");
	writeDebugStreamLine("/*---- START COPY -------------------------------------------------------------------------------------------------------------*/");
	writeDebugStreamLine("R_Freq = %d", R_Freq);
	writeDebugStreamLine(";");
	writeDebugStream("R_Batt = %d", R_Batt);
	writeDebugStreamLine(";");
	writeDebugStreamLine("replayFreq();");
	writeDebugStreamLine("");
	writeDebugStreamLine("//000");

	// start recording the vexRT signals
	startTask (recordMotorValues);

	// allow driver control for R_mSec miliseconds
	clearTimer(T1);
	while (time1[T1] < R_mSec)
	{
		userControlCode();
	}

	// stop recording the vexRT signals
	stopTask (recordMotorValues);

	writeDebugStreamLine("for (int i; i < 10; i++){ motor* = 0; }");
	writeDebugStreamLine("/*---------------------------------------------------------------------------------------------------------------END COPY-------*/");
	writeDebugStreamLine("// END AUTON RECORDING");

	// stop all motors
	for (int i; i < 10; i++)
	{
		motor* = 0;
	}
}

The autonomous lines it generates look like:


// 1700
	motor port3 ] = -60; 	motor port4 ] = -60; 	wait1Msec( P_Freq * 20 ); // 1000 wait
 

Where the first comment is the mSec time the line is into the auton (and gives a place to add a discription.) P_Freq is the calculated replay speed based on battery power, and it is multiplied by the number of loops this motor state was active, and that math is added as a comment. So, relatively easy to tweak.**

@Doug Moyers we were trying to score the cube, the lower battery grabbed the cube but only made it half way to dumping before the code moved on. I also think it did not turn enough… the code for a fresh battery turned way too much. I highly doubt there was a bug that created this inaccuracy. Tomorrow I’m going to try to brute force my way through… make an autonomous for every battery level… still probably faster than just regular coding it.

That’s an interesting concept. Was it any more accurate or is it just a way to save data? You could also have a deadband around the last value, for example only update motor values if it has changed more than 3 either way if you wanted to cut more data out.

Have you measured the time difference between executing your drive loop and your replay function? Having a wait on the bottom like that ignores any difference and could cause inconsistencies if either takes longer to process than the other (although very minor) - I would suggest using a timer based approach, so that the computation time is included in your wait.

@Doug Moyers I also like how your code accounts for the battery, are you just guessing speed of motors goes down linearly as voltage decreases or do you actually know?

That’s farm fresh code with pretty much no real world assessment. Interesting idea running a timer instead of waits. The waits make the voltage adjustment pretty easy to enact.

Yes, total guess and unlikely correct. It might be why your tests started to come apart in the last version, since it was in place there too. The function that sets P_Freq could use some serious study - but it does give you a single place to fiddle with.

That’s one of the biggest keys to making rerun code work I think… I’ll try to figure it out tomorrow…

I for one would find this very useful if done accurately. No, not for normal auton, but for skills. With perfect scores being possible this year, a team who gets perfect in driver skills could have guaranteed themselves top.

Actually, yeah!! How long is skills again?

So I figured out that battery is almost definitely proportional to the motor power… which means @Doug Moyers’s replay frequency style is probably great with adapting from one battery level to the next… the only problem is that if the battery is proportional, and you record a timer based autonomous on a 8.5V battery, and then test the recording with a 7.5V battery, you still would get much closer than what I got. We were further off than 12% error that would have given us, but that 12% may have been amplified… also I am starting to suspect variances in slipping…
@AcidEpicice one minute
@Mr_L_on_Yoshi I agree totally that it has potential for a great autonomous skills… but between slipping errors and driver error, even if the power given to each motor is perfectly consistent, your minute autonomous with sensors and near perfect efficiency is going to be higher than your minute autonomous with a motor transcriber code, unless you score perfectly either way.

Yes, pure time-based autonomous will not do well for 60 seconds.

My guess for how to best use something like this recorder would be to create it and run it only on batteries charged to a set level, like 8000. Fresh of the charger, they drop rapidly from something like 8400 to 8000, then more slowly drop voltage. Also, drive very deliberately while recording - drive like an autonomous robot, not a person. Go slower than normal, stop before interacting with objects, etc.

See how complicated this is getting? IMO, the real use of this recorder is for a 5 second autonomous for teams that otherwise would have nothing.