Recording An Autonomus

So I’ve made a code that is able to record and roughly replay an autonomous. The problem the replaying is not accurate enough to be useful. I was curious aif anyone can look over my code to see how I can do it better, or had an extremely curate autonomous themselves and would suggest and idea to make the code more accurate.

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  sBS,            sensorQuadEncoder)
#pragma config(Sensor, I2C_1,  FL,             sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  FR,             sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  BR,             sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_4,  BL,             sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           BS,            tmotorVex393HighSpeed_HBridge, openLoop, encoderPort, dgtl1)
#pragma config(Motor,  port2,           FL,            tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port3,           FR,            tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port4,           BL,            tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_3)
#pragma config(Motor,  port5,           BR,            tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_4)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VEX EDR                      */
/*                                                                           */
/*---------------------------------------------------------------------------*/

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

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

void pre_auton()
{

}



//Prototype Functions
bool compare(int base, int comp){

	bool same = false;

	//writeDebugStreamLine(" Base: %i", base);
	//writeDebugStreamLine("Comp: %i", comp);

	if(comp > (base - 3) && comp < (base + 3)){

		same = true;
	}//if


	return same;
}//compare


task autonomous()
{

  //Sets motor encoder value equal to zero
	nMotorEncoder(FL) = 0;
	nMotorEncoder(FR) = 0;
	nMotorEncoder(BL) = 0;
	nMotorEncoder(BR) = 0;
	SensorValue[sBS] = 0;

	bool BSdone = false;
	bool FLdone = false;
	bool FRdone = false;
	bool BLdone = false;
	bool BRdone = false;

	int totalSteps = 12;
	int ports = 5;


int auto[12][5] = { {0,0,0,0,0},
{0,9,0,8,0},
{0,1416,-1189,1396,-1472},
{0,1256,-2165,1236,-3426},
{0,1649,-2613,1615,-4100},
{0,1656,-2621,1623,-4108},
{0,1668,-2679,1632,-4182},
{0,1658,-2702,1623,-4210},
{0,2467,-3434,2415,-5206},
{0,2544,-3492,2491,-5275},
{0,3133,-3272,3062,-4772},
{0,4342,-4264,4246,-6015},
};


  int motorsFinished = 0;

  for(int step = 0; step < totalSteps; step++){

    BSdone = false;
	  FLdone = false;
	  FRdone = false;
	  BLdone = false;
	  BRdone = false;

  writeDebugStreamLine("");
  writeDebugStreamLine("Step: %i", step);
  writeDebugStreamLine("");


  	while(motorsFinished != ports){

      //BS if
      if(!BSdone){

        //Checks to see if the motor value matches the target value and if it does
        if(compare(auto[step][0], SensorValue[sBS])){
          //Stops motor
          motor[BS] = 0;
          //Says this motor is done to prevent entering this loop and creating a false positive
          BSdone = true;
          //Increases total of motors done
          motorsFinished++;
          //Breaks out of this if so the motor doesn't restart
          goto BSend;

          //If the target value is below the actual value the motor will spin backwards
        }else if(SensorValue[sBS] > auto[step][0]){
          motor[BS] = -30;
          //If the target value os above the motor value it will spin foreward
        }else if(SensorValue[sBS] < auto[step][0]){
          motor[BS] = 30;
        }

        //Where the goto comes to
        BSend:;

      }//BS if

  		//FL if
      if(!FLdone){

        //Checks to see if the motor value matches the target value and if it does
        if(compare(auto[step][1], nMotorEncoder(FL))){
          //Stops motor
          motor[FL] = 0;
          //Says this motor is done to prevent entering this loop and creating a false positive
          FLdone = true;
          //Increases total of motors done
          motorsFinished++;
          //Breaks out of this if so the motor doesn't restart
          goto FLend;

          //If the target value is below the actual value the motor will spin backwards
        }else if(nMotorEncoder(FL) > auto[step][1]){
          motor[FL] = -60;
          //If the target value os above the motor value it will spin foreward
        }else if(nMotorEncoder(FL) < auto[step][1]){
          motor[FL] = 60;
        }

        //Where the goto comes to
        FLend:;

      }//FL if


      //FR if
      if(!FRdone){

        //Checks to see if the motor value matches the target value and if it does
        if(compare(auto[step][2], nMotorEncoder(FR))){
          //Stops motor
          motor[FR] = 0;
          //Says this motor is done to prevent entering this loop and creating a false positive
          FRdone = true;
          //Increases total of motors done
          motorsFinished++;
          //Breaks out of this if so the motor doesn't restart
          goto FRend;

          //If the target value is below the actual value the motor will spin backwards
        }else if(nMotorEncoder(FR) > auto[step][2]){
          motor[FR] = -60;
          //If the target value os above the motor value it will spin foreward
        }else if(nMotorEncoder(FR) < auto[step][2]){
          motor[FR] = 60;
        }

        //Where the goto comes to
        FRend:;

      }//FR if




      //BL if
      if(!BLdone){

        //Checks to see if the motor value matches the target value and if it does
        if(compare(auto[step][3], nMotorEncoder(BL))){
          //Stops motor
          motor[BL] = 0;
          //Says this motor is done to prevent entering this loop and creating a false positive
          BLdone = true;
          //Increases total of motors done
          motorsFinished++;
          //Breaks out of this if so the motor doesn't restart
          goto BLend;

          //If the target value is below the actual value the motor will spin backwards
        }else if(nMotorEncoder(BL) > auto[step][3]){
          motor[BL] = -60;
          //If the target value os above the motor value it will spin foreward
        }else if(nMotorEncoder(BL) < auto[step][3]){
          motor[BL] = 60;
        }

        //Where the goto comes to
        BLend:;

      }//bL if



      //BR if
      if(!BRdone){

        //Checks to see if the motor value matches the target value and if it does
        if(compare(auto[step][4], nMotorEncoder(BR))){
          //Stops motor
          motor[BR] = 0;
          //Says this motor is done to prevent entering this loop and creating a false positive
          BRdone = true;
          //Increases total of motors done
          motorsFinished++;
          //Breaks out of this if so the motor doesn't restart
          goto BRend;

          //If the target value is below the actual value the motor will spin backwards
        }else if(nMotorEncoder(BR) > auto[step][4]){
          motor[BR] = -60;
          //If the target value os above the motor value it will spin foreward
        }else if(nMotorEncoder(BR) < auto[step][4]){
          motor[BR] = 60;
        }

        //Where the goto comes to
        BRend:;

      }//BR if


    }//While

    motorsFinished = 0;
    //writeDebugStreamLine("New Step");
  }//for

  //writeDebugStreamLine("OUT");

}//autonomous

task usercontrol()
{

	//boolean values representing if the the button is pressed or not
	int up8U = 0;
	int up8D = 0;
	bool upCh3 = false;
	bool downCh3 = false;
	bool upCh2 = false;
	bool downCh2 = false;

	//Varible that will hold if any button presses have changed
	bool change = false;

  //Sets motor encoder value equal to zero
	nMotorEncoder(FL) = 0;
	nMotorEncoder(FR) = 0;
	nMotorEncoder(BL) = 0;
	nMotorEncoder(BR) = 0;
	SensorValue[sBS] = 0;
	//Counts iterations through while loop so there isn't a constant stream of output data
	int iterations = 0;

  while (true)
  {


	 //These check if the state of a button has changed since the last iteration
	 if(up8U != vexRT[Btn8U]){
	 	 change = true;
   }

	 if(up8D != vexRT[Btn8D]){
	 	 change = true;
   }

   //These check for the axis of they have changed state
   if(upCh3 != (vexRT[Ch3] > 100)){
	   change = true;
   }

   if(downCh3 != (vexRT[Ch3] < -100)){
	   change = true;
   }

	 if(upCh2 != (vexRT[Ch2] > 100)){
	   change = true;
   }

   if(downCh2 != (vexRT[Ch2] < -100)){
	   change = true;
   }

   //If change is true
   if(change){
    writeDebugStream("%d,", SensorValue[sBS]);
    writeDebugStream("%d,", nMotorEncoder(FL));
		writeDebugStream("%d,", nMotorEncoder(FR));
		writeDebugStream("%d,", nMotorEncoder(BL));
		writeDebugStreamLine("%d", nMotorEncoder(BR));

    change = false;
   }

   //Changes what the state varibles are
   up8U = vexRT[Btn8U];
   up8D = vexRT[Btn8D];

   //So that if the axis do change form up to down or down to up, You don't need more if statements to track the change, just to see what it currently is
   upCh3 = false;
	 downCh3 = false;
	 upCh2 = false;
	 downCh2 = false;

   if(vexRT[Ch3] > 100){
   	upCh3 = true;
   }
   if(vexRT[Ch3] < -100){
   	downCh3 = true;
   }

   if(vexRT[Ch2] > 100){
   	upCh2 = true;
   }
   if(vexRT[Ch2] < -100){
   	downCh2 = true;
   }

  }//While


}//User control

Here’s the rest of my post (Because there is a max of characters):

I also use this little bit of java code to convert the raw output data into a two dimensional that Robot C can use.

import java.io.BufferedReader;
import java.io.FileReader;
import java.io.IOException;

public class DataEditor {

	public static void main(String] args) {
		
		int counts = 0;
		
		BufferedReader br = null;
		FileReader fr = null;
		
		String data = "";
		
		try {

			fr = new FileReader("data");
			br = new BufferedReader(fr);

			String sCurrentLine;

			while ((sCurrentLine = br.readLine()) != null) {
				data += sCurrentLine + "\n";
				counts++;
			}

		} catch (IOException e) {

			e.printStackTrace();

		} finally {

			try {

				if (br != null)
					br.close();

				if (fr != null)
					fr.close();

			} catch (IOException ex) {

				ex.printStackTrace();

			}

		}
		
		
		
		String] splitData = data.split("\n");
		
		System.out.print("int auto" + counts + "][5] = { ");
		
		for(int i = 0; i < splitData.length; i++){
			
			System.out.println("{" + splitData* + "},");
			
			
		}
		
		System.out.println("};");
		

	}

}

My end goal is to create a program the you enter information about your robot into and then it generates the recording and running code for you, but I need to get it more accurate first.

Sorry that it’s a lot of code but here’s also a link to the git hub that I’m using for the project.
https://github.com/extension5525A/Project-Uhuru*

TL;DR:
Search this forum for examples of rerun and replay.

Detail:
When embarking on a large project, it is usually helpful to read the work of those who have gone before. (Thus the phrase “shoulders of giants…”) It is almost always better to extend, improve, and expand existing research rather than repeat the basic steps others have already covered. I would expect any employee I hired to become well versed in the current state of the art before they attempt to extend it. That’s something you should learn to do as your first instinct.

I don’t say this to criticize the hard work you’ve done. And, if you choose to start your own project from scratch, either for the learning experience, or because you believe you’ve found a better way, then by all means do so. However, I gather from the fact you’re asking for basic help that you haven’t done the background work. It would likely benefit you.

Yes, that is one of the standard problems people who have done this before have encountered. They’ve dealt with it in a variety of ways, researched different solutions, and provided different sets of sample code.

My suggestion is to read up on what others have done before.

That is the typical goal. There are several examples of this working. We’ve used three different sets of sample code found on this forum (or in github and linked to from this forum) to do what you describe.

Here’s one nice thread. There are others.

https://vexforum.com/t/answered-what-rules-are-in-place-to-protect-robots-that-are-hanging/18114/1

If you don’t like using the search function on this forum, you can use google and limit the results to the vexforum site.

[Edited for typos. Others may remain; I’ll fix them if I ever see them.]