My Sensor Based Auton Playback

Hi, I’m Adam from 99371A Dreadnought and I thought I’d share some ideas I’ve been experimenting with this season. Between December and February, our team worked on our skills bot (the double mobile goal one you can check out on our channel) and in addition to traditional PID I started drafting what a sensor-based playback might look like.
I know that other people have worked on this same idea and I’m sure much of what I coded overlaps with existing ideas, but I still wanted to share what I worked on in the hopes that someone could take it further.
I got the playback to the point where if is fairly consistent for the first 15 seconds, but afterward it’s no more consistent that classic power and timing playback. Perhaps someone can tweak it to make it valid for competition autonomous programs or even skills programs.

The general idea is that every subsystem movement, especially the drive, is represented as
sensor targets, even in usercontrol. The subsystem power outputs are mathmatically equivalent to whatever you’d normally set them to (127, 0, vexRT[Ch3], etc.), but this sensor target is stored in a giant array every x seconds as you drive the robot. When playing back you use proportional to speed up or slow down at that given moment to get closer to the desired target.

Example: Setting your drive to 100 when the encoder reads 100

motor=100; //sets motor power
L[t/dTplayback]= getMotorEncoder(BL) + ((1/kL)*motor[BL]); //sets current position in array “L” to an imaginary target based on current position and a constant “kL”

Playback:
motor[BL]=motor[FL]=(int)(kLTError(L[t/dTplayback]-getMotorEncoder(BL))+turnError);
Assuming the ecoder values are the same, this is the same power output 100, but if the robot is behing its target it will increase the power using constant kL to reach its target.

Gyro correction:
Additionally I tracked the gyro value over time and would alter the values of the left and right drive to arc if the desired gyro angle is off.

Notes:
-In order for this to work, the robot must be recorded with power outputs slower than its full potential so it can compensate in playback if it falls behind
-I know one important change will be implementing trueSpeed to make power proportional to speed

I included an example of what output arrays for 60 seconds of playback might look like, “94(comp).h”

I’m aware there are many faults in the code, the purpose of sharing this is in the hope someone can take the idea further. I look forward to your feedback!