I am going crazy

[billbo911]

We are doing exactly as you describe. It takes to simple functions, call the first one then call the second. The first one looks at the button inputs on the console, the second one sends the command to the lift function.

int Get_Oi_Switch (void)
{
	if (p1_sw_top == 1)
	{
		liftcmd = LOAD;
	}
	if (p1_sw_trig == 1)
	{
		liftcmd = HURDLE;
	}
	if (p1_sw_aux1 == 1)
	{
		liftcmd = PICK;
	}
	if (p1_sw_aux2 == 1)
	{
		liftcmd = PLACE;
	}
	return liftcmd;	
}

int Set_Lift_Height (int pos_demand)
{
  int liftdrv;
  int liftvel;
	liftpos = ((int)Get_Encoder_5_Count());
	liftposerr = (pos_demand - liftpos);
	liftvel = (liftpos - lastliftpos);
	lastliftpos = liftpos;  // Save position for next loop
	liftdrv = ((liftposerr * LIFT_KP) + (liftvel * LIFT_KD));  // Actual PD equation
	if (liftdrv > LIFT_MAX_V)
	{
		liftdrv = LIFT_MAX_V; // Limit lift speed to defined range
	}
	if (liftdrv < -LIFT_MAX_V)
	{
		liftdrv = -LIFT_MAX_V;
	}
	
	return (liftdrv + 127);
}

As you can see, there are some definitions that are set up outside of this file that define the individual heights and velocities. Also included in the Set_Lift_Height function is a PD loop. We are not using the integral function of a PID loop.


EDIT: If it helps, feel free to use this code. Just make sure you understand it and modify it to match your system. All I ask is that you let us know how it works out for you.