[Lord_Jeremy]

Originally Posted by johncap100

Hey Lord jeremy I an using the old robots for demos at my school,
would you be willing to share some of your code, partcularly any autononmous code?
thanks John

Hey! Sure, though I think it would be better for your team if I help you develop a functional understanding of how to write autonomous code. Mind you, I haven't had a chance to test my codebase out yet, so I'm not sure if it works just yet.

First of all, I based my code on Kevin Watson's FRC code, available at http://kevin.org/frc/. Specifically, I'm pretty sure I started with the ifi_frc_sensor.zip code. Looking at the page again, I see that he has ifi_clock.zip, which probably would help with an autonomous routine. I did without the clock and instead wrote my own code to keep track of time for the purpose of autonomous timing.

The first step to writing autonomous is looking at how it can interface with your robot control routines. For instance, all my drive code is contained within a function called TankDrive, which takes parameters of two joysticks and two output PWM pointers. To facilitate autonomous control of drive, I chose to write a separate function in a separate C file called AutoTankDrive, which just took the parameters of two output PWM pointers. That way, I could call AutoTankDrive in the Autonomous function of Kevin's autonomous.c with two PWM values - the values of the motors. Inside the AutoTankDrive function, I would just have calls to TankDrive, but instead of passing joystick values as parameters I would pass defined integers. To see what I mean, check out the following simplified version of my code:

void AutoTankDrive( unsigned char *pOutput1 , unsigned char *pOutput2 )
{
	switch( cCurrentAutonomousRoutine )
	{
		case 1:
			if( iCurrentAutonomousMilliseconds < 3000 )
				TankDrive( 254 , 254 , pOutput1 , pOutput2 );
			else if( iCurrentAutonomousMilliseconds < 4000 )
				TankDrive( 200 , 200 , pOutput1 , pOutput2 );
			else if( iCurrentAutonomousMilliseconds < 5000 )
				TankDrive( 150 , 150 , pOutput1 , pOutput2 );
			else
				TankDrive( 127 , 127 , pOutput1 , pOutput2 );			
			break;
		case 2:
			//todo: write more routines
		default:
			break;
	}
}

In that block, you see that when the autonomous selection variable cCurrentAutonomousRoutine is 1, the first case is executed. In that case, the Joysticks are first set to 254/254 (full forward) for 3 seconds, 200/200 (somewhat slower forward) for 1 second (time 3 to time 4), 150/150 (even slower forward) for another second, and 127/127 (full stop) for the remainder of the time. I simply called my TankDrive function with static values instead of the dynamic joystick variables. I did the same thing for AutoJoint (arms) and AutoRelay (pneumatics) functions.

Now onto the next part of autonomous, selecting a mode. I wrote four functions for selecting the mode, SelectAutonomous1Input through SelectAutonomous4Input. Each takes either 1, 2, 3, or 4 inputs from digital ins - the switches on the robot. I wrote four of them so that it's very easy to increase or decrease the number of switches being used. They all modify the same variable, cCurrentAutonomousRoutine. All I did was bitshift the inputs and bitwise OR them. What that means is that say input 1 is 1, input 2 is 0 and input 3 is 1. Input 3 * 100 + Input 2 * 10 + Input 1 * 1 = 101, which in binary is routine number 5. This is what it looks like:

void SelectAutonomous4Input( char cInput1 , char cInput2 , char cInput3 , char cInput4 )
{
	cCurrentAutonomousRoutine = ( cInput1 << 3 ) | ( cInput2 << 2 ) | ( cInput3 << 1 ) | cInput4;
}

Now onto the annoying part of this - keeping time. To do this, I took advantage of the IC timer handlers that Kevin wrote in his code, in the file timers.c. First, you'll need to go to timers.h and uncomment one of the defines that enables the timers. I used TIMER_0. Then I went to the Initialize_Timer_0 function in timers.c and set te prescaler to 1:256. That means I set the following variables:

T0CONbits.T0PS0 = 1;
T0CONbits.T0PS1 = 1;
T0CONbits.T0PS2 = 1;

This sets the timer up so that each tick will equal 25.6 microseconds. I figure that this is sufficient resolution to ensure fairly accurate timekeeping without calling interrupts too often. I'd appreciate it if someone more knowledgable pointed out if I'm calling it too often or something. The next part of this is simple, just go down to the Timer_0_ISR and write in it a call to AutonomousTick with the parameter 26. Back in my autonomous C file I wrote the function AutonomousTick to take the parameter of the number of microseconds that have passed since the last call. I'm sure I could do all sorts of things to make this more accurate, as it's really been 25.6 microseconds, but I didn't feel like doing any of that. Anyway, my AutonomousTick function just consists of the following:

void AutonomousTick( unsigned int iPeriod )
{
	iCurrentAutonomousMicroseconds += iPeriod;
	iCurrentAutonomousMilliseconds += ( int )( iCurrentAutonomousMicroseconds / 1000 );
}

There you can see that I keep a running total of the number of microseconds that have elapsed since autonomous started, as well as the number of milliseconds I'm currently up to, because who wants to count in microseconds. Of course, you'll notice I cast the number as an int, because our RC doesn't like floating point numbers.

Anyway, I think that's about it. Post back or PM me if you need any additional help. Sorry, but I'm loathe to post the codebase I've put together because I did a lot of weird stuff so that we could use the same code for four different robots. It's very confusing to look at >.>