[Tom Saxton]

Originally Posted by SlimBoJones

Tom, post your code for when you are copying the OI variables to your static variables, and the code you are using to select your autonomous code from your static variables.

It might offer some more insight as to why you're seeing this behaviour.

-SlimBoJones...

Here's all of the sensor reading code. The autonomous code uses the global variable "g_stratAutonomous" to determine the autonomous mode via a switch statement.

The variables with a g_ are public globals, available to the autonomous mode code in a different file. The s_ variables are statics available only to user_routines.c code, which is the tele-operation code and some utilities used in both modes.

After an optional prefix, each variable name starts with a tag which describes the type of information stored in the variable. "ana" is an analog value from the RC (which needs to go through Get_Analog_Value to get a 10-bit analog value), "oana" is a direct 8-bit analog value from the OI. "digi" is a digital-in value, "digo" is digital-out. "f" is a boolean flag. "dist" is an accumulated optical encoder value.

We'll get all of our code posted on our web site after cleaning up a bit of the patches and hacks thrown in while debugging and experimenting in the pit between matches.

My favorite time at the PNW regional was when we were replacing a key on an arm axle that got sheered and also adding a limit switch to keep that from happening again. The mechanical stuff got finsihed amazingly quickly, but they were calling us to queue up by the time I was able to download and try the fresh new code, which of course was backwards the first time allowing us to drive against the stop, but not away from it!

///////////////////////////////////
// excerpt from user_routines.h
///////////////////////////////////

enum {
	stratLoaderLeft,
	stratLoaderRight,
	stratStartingTetra,
	stratHangingTetra,
	stratInterference,
	stratVisionTetra,
	stratMax
};

// strategy pot values
#define oanaManualLoader    1
#define oanaAutoLoader        26
#define oanaStartingTetra     55
#define oanaHangingTetra     88
#define oanaInterference      128
#define oanaVisionTetra        184

// slow loop count, our crappy timer
extern int g_cLoop;
extern uchar g_fPrint;

// measured arm positions
extern long g_mdegShoulderCur;
extern long g_mdegElbowCur;
extern long g_mdegWristCur;

// override flags
extern uchar s_fOverrideEncoders;
extern uchar s_fOverrideArmSensors;


enum {
	tposLeft = -1,
	tposCenter = 0,
	tposRight = 1
};
extern char g_tposStartingPos;
extern uchar g_stratAutonomous;
extern uchar g_fFarLoading;

///////////////////////////////////////////
// our sensor code from user_routines.c
///////////////////////////////////////////


// call this routine every time we get new OI data during autonomous
// or tele-operation modes
//
// the function reads new values from the OI and RC and computes averages
// from values accumulated from calls to Process_Data_From_Local_IO
void UpdateSensors()
{
	static ulong s_distLeftPrev = 0;
	static ulong s_distRightPrev = 0;
	
	int anaCurrentSensorAverageR;
	int anaCurrentSensorAverageL;
	int fOverride;
	ulong distT;

	long speedLeft, speedRight;

	// Our dopey timer.
	++g_cLoop;
	g_fPrint = (g_cLoop % 10) == 0;

	speedLeft = SpeedFromSample(Get_Left_Encoder_Count(), s_adistSampleLeft);
	speedRight = SpeedFromSample(Get_Right_Encoder_Count(), s_adistSampleRight);

	//	Arm Code
	s_fElbowLowLimit = digiElbowClosed == 0;

	g_mdegShoulderCur = MdegFromEncoder((s_anaPotShoulderTotal / s_cSampleFast) - encoderBiasShoulder);
	g_mdegElbowCur = MdegFromEncoder(encoderBiasElbow - (s_anaPotElbowTotal / s_cSampleFast));
	g_mdegWristCur = MdegFromEncoder(encoderBiasWrist - (s_anaPotWristTotal / s_cSampleFast));
	s_anaPotShoulderTotal = s_anaPotElbowTotal = s_anaPotWristTotal = 0;

	// monitor the left encoder
	distT = Get_Left_Encoder_Count();
	if (s_distLeftPrev != distT || pwmDriveL1 == 127)
	{
		s_distLeftPrev = distT;
		s_cLoopNoLeftEncoder = 0;
	}
	else if (s_cLoopNoLeftEncoder < 255)
	{
		++s_cLoopNoLeftEncoder;
	}

	// monitor the right encoder
	distT = Get_Right_Encoder_Count();
	if (s_distRightPrev != distT || pwmDriveR1 == 127)
	{
		s_distRightPrev = distT;
		s_cLoopNoRightEncoder = 0;
	}
	else if (s_cLoopNoRightEncoder < 255)
	{
		++s_cLoopNoRightEncoder;
	}

	// turn on the warning LED if either encoder has stopped responding
	digoEncoderLED = s_cLoopNoLeftEncoder >= 20 || s_cLoopNoRightEncoder >= 20;

//  Override Switch & LED Code
//
// WARNING: the OI bits get blasted when we're in autonomous mode
//
	if (!autonomous_mode)
	{
		s_fTurboDrive = digiTurboDrive;

		fOverride = digiEncoderOverride == 0;
		if(fOverride != s_fOverrideEncoders)
		{
			s_fOverrideEncoders = fOverride;
			printf("Encoders %s\n", s_apszOverride[s_fOverrideEncoders]);
		}

		fOverride = digiArmSensorOverride != 0;
		if(fOverride != s_fOverrideArmSensors)
		{
			s_fOverrideArmSensors = fOverride;
			printf("digiArmSensorOverride. %d\n", (int)digiArmSensorOverride);
			printf("Arm Sens. %s\n", s_apszOverride[s_fOverrideArmSensors]);
		}
		digoArmLimitLED = s_fOverrideArmSensors;

		fOverride = digiCurrentOverride == 0;
		if(fOverride != s_fDontAutoAdjustElbow)
		{
			s_fDontAutoAdjustElbow = fOverride;
			printf("Elbow Auto Adj. %s\n", s_apszOverride[1-s_fDontAutoAdjustElbow]);
		}
		digoCurrentLED = s_fDontAutoAdjustElbow;

		fOverride = digiFarLoading != 0;
		if(fOverride != g_fFarLoading)
		{
			g_fFarLoading = fOverride;
			printf("far loading %s\n", s_apszOverride[g_fFarLoading]);
		}
		digoCamLED = g_fFarLoading;

		fOverride = digiCompressEnable == 0;
		if(fOverride != s_fDisableCompressor)
		{
			s_fDisableCompressor = fOverride;
			printf("Comp. %s\n", s_apszOverride[s_fDisableCompressor]);
		}
		
		// autonomous mode switches
		g_tposStartingPos = 0;
		if (digiStartPosLeft)
			g_tposStartingPos = -1;
		if (digiStartPosRight)
			g_tposStartingPos = 1;

		// get the strategy mode from the potentiometer on the OI
		g_stratAutonomous = StratFromOana(oanaStrategy);

		// check for kyle mode
		s_fKyleDriveMode = g_tposStartingPos == tposLeft;
	}
	else
	{
		s_fDisableCompressor = fFalse;
		s_fDontAutoAdjustElbow = fFalse;
		s_fOverrideEncoders = fFalse;
		s_fOverrideArmSensors = fFalse;
	}

	// turn on the light if the pressure has dropped low enough to want the compressor
	digoCompressLED = !digiCompPressureHigh;

	if (g_fRobotDisabled)
	{
		s_mdegShoulderPrev = g_mdegShoulderCur;
		s_mdegElbowPrev = g_mdegElbowCur;
		set_pid_stop(pidDriveL);
	}
	
	s_cSampleFast = 0;
}

// given the value from the autonomous strategy potentiometer,
// return the corresponding "strat" value
int StratFromOana(uchar oana)
{
	return ValFromTable(oana, s_mp_strat_oana, stratMax);
}

// this array is used to determine the setting of
// the autonomous mode potentiometer
static const uchar s_mp_strat_oana[stratMax] =
{
	oanaManualLoader,
	oanaAutoLoader,
	oanaStartingTetra,
	oanaHangingTetra,
	oanaInterference,
	oanaVisionTetra
};

// given a reading from a detent potentiometer and a table
// of setting values, find the closest preset to the given
// value
int ValFromTable(int oana, const uchar pmp_val_oana[], int valMax)
{
	int doanaBest = 255;
	int valBest = 0;
	int val;

	for (val = 0; val < valMax; ++val)
	{
		int doana = oana - pmp_val_oana[val];
		if (doana < 0)
			doana = -doana;
		if (doana < doanaBest)
		{
			valBest = val;
			doanaBest = doana;
		}
	}

	return valBest;
}

// call this every time through the main or autonomous loops
// to collect data from sensors attached to the RC
void Process_Data_From_Local_IO(void)
{
	static uchar s_fShoulderButtonUp = fTrue;
	
    /* Add code here that you want to be executed every program loop. */

	s_anaPotShoulderTotal += Get_Analog_Value(anaPotShoulder);
	s_anaPotElbowTotal += Get_Analog_Value(anaPotElbow);
	s_anaPotWristTotal += Get_Analog_Value(anaPotWrist);

	if (digiShoulderFwd && digiShoulderBack)
	{
		if (s_fShoulderButtonUp)
		{
			s_fToggleShoulderPos = fTrue;
			s_fShoulderButtonUp = fFalse;
		}
	}
	else
	{
		s_fShoulderButtonUp = fTrue;
	}
	
	++s_cSampleFast;
}