mecanum programming

[Alan Anderson]

This is what we use for a simple open-loop mecanum drivebase. Default_Routine() computes internal speed, turn rate, and slide (sideways travel) values from the joystick inputs, then computes motor speeds from the internal values.

The p1 joystick is on the left, and p2 is on the right.
pwm01 is the right front motor, pwm02 is the left front, pwm03 is the right rear, and pwm04 is the left rear.
rc_dig_in01 is a "deadman" switch which sets the motors to neutral unless it is closed. This was intended to be used by people "riding" the robot.

/*******************
function:	deadband()
inputs:	inp, input value
		center, value of input that represents neutral
		band, size of deadband to consider neutral
output:	scaled and deadband-compensated output value
		inputs below center will yield negative output
		inputs above center will yield positive output
		inputs within deadband either side of center will yield 0
usage:	typically call with joystick value (0-254) as input,
		joystick neutral (127) as center, and a small number (5)
		as the size of the deadband.  The white POS joysticks
		should probably have a much larger band (10) instead.
*******************/
int deadband(int inp, int center, int band)
{
int oup;
	if (inp < center-band)
		oup = inp - center + band;
	if (center-band <= inp && inp <= center+band)
		oup = 0;
	if (center+band < inp)
		oup = inp - center - band;
	return oup;
}

/*********************
function:	pwm_limit()
inputs:	inp, input value
output:	scaled and range-limited output value
usage:	call with signed value (+/- 127) as input,
		will return a valid pwm value
*********************/
unsigned char pwm_limit (signed int inp)
{
	if(inp<-127)
		inp = -127;
	if (inp>127)
		inp = 127;
	inp = inp+127;

	return (char)inp;
}

/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE:       Performs the default mappings of inputs to outputs for the
*                Robot Controller.
* CALLED FROM:   this file, Process_Data_From_Master_uP routine
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void Default_Routine(void)
{
signed int speed, turn, slide;
signed int front_r, front_l, back_r, back_l;
long int l_y, r_y, l_x, r_x;

	l_y = deadband(p1_y,127,10);
	r_y = deadband(p2_y,127,10);
	l_x = deadband(p1_x,127,10);
	r_x = deadband(p2_x,127,10);

	l_y = l_y/2;
	r_y = r_y/2;
	l_x = l_x/2;
	r_x = r_x/2;

	speed = l_y+r_y;
	turn = l_y-r_y;
	slide = -l_x-r_x;
  
  	front_r = speed-turn+slide;
	front_l = speed+turn-slide;
	back_r = speed-turn-slide;
	back_l = speed+turn+slide;

if (rc_dig_in01 == 0)
{
	pwm01 = pwm_limit(front_r);
	pwm02 = pwm_limit(front_l);
	pwm03 = pwm_limit(back_r);
	pwm04 = pwm_limit(back_l);
}
else
{
	pwm01 = 127;
	pwm02 = 127;
	pwm03 = 127;
	pwm04 = 127;
}