Since I have been working with the kids on auto_lift control I thought I would put in writing a sample program that uses potentiometer feedback to stop the lift at discrete goals. The friction of the lift is relied upon to hold the lift in place since closed loop feedback is removed once the lift has reached the goal. This program doesn’t require scripting logic , however, the int =auto_lift(int goal,int rate,int TOL) function is tailored to fit easily within a script if required.
Here is the RobotC Sample_Auto_lift.c Program :
Edit: 1/6/2011 Simplified EasyC version here:
Tuning for minimum overshoot.
A note on how to tune the program. The lift is commanded at a fixed rate. This rate will cause an overshoot after the lift_goal is reached. The size of the overshoot depends on the lift inertia, the magnitude of the fixed rate and the motor drag torque. The LIFT_TOL is used to bias the target goal in a direction that anticipates the overshoot and shuts the lift rate off early.
You can get an idea of the overshoot by setting LIFT_TOL = 0 , pushing the select goal button and then looking at the difference between where the lift stopped relative to the lift goal position. Repeat this for both above and below goal starting conditions. You will find that the overshoot is a lot more for the down position due to the assistance of gravity.
Now you can set the LIFT_TOL equal to the overshoot in the up direction. To compensate for the extra overshoot in the down direction, you can increase LIFT_TOL to LIFT_TOL_DOWN =LIFT_TOL*factor. Use LIFT_TOL_DOWN in place of LIFT_TOL in the down direction error test. I.e. if(error <-LIFT_TOL_DOWN)) lift_cmd = -LIFT_RATE.
You can also keep the LIFT_TOL small and just adjust the goal heights. Probably would need separate heights for up and down directions.
If your overshoots are just too large , then you can lower your average rate or make the lift_rate = K*error and limit the rate to [ -127, 127]. This is essentially a PID loop without the I and D. K is just a gain constant. Sometimes, I also use a little timed speed reversal to assist in the braking of the arm.
RobotC Code Starts Here (copy and paste in RobotC non competition program file template)****************************************************
#pragma config(Sensor, in5, lift_pot, sensorNone)
#pragma config(Motor, port10, lift_motor, tmotorNormal, openLoop)
//*!!Code automatically generated by ‘ROBOTC’ configuration wizard !!*//
//599 Robodox Vex Team
//Author:Chris S. Engineering Mentor
//Sample auto lift control program that uses a potentiometer to sense lift position.
//Simply hook up a motor to a potentiometer , set the config to match your ports and run.
//Lift states are commanded by either the joystick Ch3 or joystick buttons 5Down,5Up,6Down,6Up.
//At anytime the auto_lift is operating, a Ch3 input will automatically disconnect the auto_lift and allow manual takeover.
//Potentiometer – Returns an analog value between 0 and 4095
//(although mechanical stops my limit the values to between 5 and 4092)
//When Ch3 input is less than DEADZONE then auto_lift commands are enabled.
//If a lift_goal is chosen by a button, auto_lift causes the lift to move toward the goal at LIFT_RATE.
//When the (goal-lift_pos) is within LIFT_TOL, lift_goal is set to NO_GOAL and auto_lift is exited.
//lift_state variable is set equal to a lift goal when the lift_pos is within LIFT_TOL of the goal. This
//allows other functions to poll the discrete lift_states. I.E. if(lift_state == LIFT_DOWN) do something.
//lift_state = LIFT_OTHER when not within +_LIFT_TOL of a goal.
//Routine also contains max/min lift limiting.
#define LIFT_RATE 20 //speed of auto lift
//lift goals and/or states
#define NO_GOAL -99 //if lift_goal = NO_GOAL, auto_lift is bypassed.
#define LIFT_OTHER -88 //use when lift is in between discrete states
#define LIFT_MIN 50 //lift goals
#define LIFT_DOWN 200
#define LIFT_PICKUP 1000
#define LIFT_DROP 2000
#define LIFT_HANG 3000
#define LIFT_MAX 4000
#define LIFT_TOL 50 // lift position control tolerance
#define DEADZONE 20 // joystick deadzone
int auto_lift(int goal, int rate, int tol); //sets lift pwm and moves until withing tol of goal
//Define variables for complete joystick although we only need 5 inputs
// Joystick Axes – Joystick 1
int RT_Ch1 = 0;
int RT_Ch2 = 0;
int RT_Ch3 = 0;//only need this chan really
int RT_Ch4 = 0;
// Shoulder Buttons – Joystick 1
int RT_5Down = 0;//need these four
int RT_5Up = 0;
int RT_6Down = 0;
int RT_6Up = 0;
// Front Buttons – Joystick 1
int RT_7Up = 0;
int RT_7Right = 0;
int RT_7Down = 0;
int RT_7Left = 0;
int RT_8Up = 0;
int RT_8Right = 0;
int RT_8Down = 0;
int RT_8Left = 0;
int pwm_lift_cmd = 0 ;
int lift_goal = NO_GOAL ;
int lift_pos= 0; //lift pot
int lift_state = LIFT_OTHER;//this is the discrete state of the lift
lift_pos = SensorValue(lift_pot);//get the potentiometer postion
// Operator function code
if(RT_Ch3 < DEADZONE && RT_Ch3 >- DEADZONE) //adds simple deadzone
//inside deadzone so set command to zero and allow auto commands
if(RT_5Down) lift_goal = LIFT_DOWN;
if(RT_5Up) lift_goal = LIFT_PICKUP;
if(RT_6Down) lift_goal = LIFT_DROP;
if(RT_6Up) lift_goal = LIFT_HANG;
pwm_lift_cmd = RT_Ch3/2 ;//half gain for demo
lift_goal = NO_GOAL;//joystick input so reset lift_goal = NO_GOAL
//End Operator Function code
auto_lift(lift_goal, LIFT_RATE,LIFT_TOL) ; // modifies pwm_lift_cmd if lift_goal != NO_GOAL
}//end task main
//Compute discrete lift states for other functions to test on.
if(lift_pos – LIFT_DOWN < LIFT_TOL && lift_pos-LIFT_DOWN >-LIFT_TOL) lift_state = LIFT_DOWN;
if(lift_pos – LIFT_PICKUP < LIFT_TOL && lift_pos-LIFT_PICKUP >-LIFT_TOL) lift_state = LIFT_PICKUP;
if(lift_pos – LIFT_DROP < LIFT_TOL && lift_pos-LIFT_DROP >-LIFT_TOL) lift_state = LIFT_DROP;
if(lift_pos – LIFT_HANG < LIFT_TOL && lift_pos-LIFT_HANG >-LIFT_TOL) lift_state = LIFT_HANG;
if(lift_pos – LIFT_OTHER < LIFT_TOL && lift_pos-LIFT_OTHER >-LIFT_TOL) lift_state = LIFT_OTHER;
//limit lift travel with potentiometer
if(pwm_lift_cmd > 0 && lift_pos >= LIFT_MAX || pwm_lift_cmd <0 && lift_pos <=LIFT_MIN)
motor[lift_motor] = 0;
motor[lift_motor] = pwm_lift_cmd;
//sometimes it is good to also have hard limit switches in case the potentiometer slips.
//if(pwm_lift_cmd > 0 && upper_limit_sw || pwm_lift_cmd <0 && lower_limit_sw)
// motor[lift_motor] = 0;
// motor[lift_motor] = pwm_lift_cmd;
int auto_lift(int goal, int rate, int tol)
//goal is the position you want to move to… in pot units
//rate is the pwm that you want the lift motor to move at
//tol is how close you want to be within the goal when you are done moving
//tol should be set large than the noise band of the position sensor
if(lift_goal == NO_GOAL)
return 0 ; // no goal so get out
int error = lift_pos – goal;
if( error > tol )
pwm_lift_cmd = -rate;
if(error < -tol)
pwm_lift_cmd = rate;
else //we are within the tol so we have met the goal
pwm_lift_cmd = 0; //stop moving
lift_goal = NO_GOAL;
//Read Joystick inputs ****************************************************************************
// Joystick Axes – Joystick 1
RT_Ch1 = vexRT[Ch1]; // Returns -127 to +127
RT_Ch2 = vexRT[Ch2] ; // Returns -127 to +127
RT_Ch3 = vexRT[Ch3]; // Returns -127 to +127
RT_Ch4 = vexRT[Ch4] ; // Returns -127 to +127
// Top Buttons – Joystick 1
RT_5Down = vexRT[Btn5D]; // Returns 1 or 0
RT_5Up = vexRT[Btn5U]; // Returns 1 or 0
RT_6Down = vexRT[Btn6D]; // Returns 1 or 0
RT_6Up = vexRT[Btn6U]; // Returns 1 or 0
// Front Buttons – Joystick 1
RT_7Up = vexRT[Btn7U]; // Returns 1 or 0
RT_7Right = vexRT[Btn7R]; // Returns 1 or 0
RT_7Down = vexRT[Btn7D]; // Returns 1 or 0
RT_7Left = vexRT[Btn7L]; // Returns 1 or 0
RT_8Up = vexRT[Btn8U]; // Returns 1 or 0
RT_8Right = vexRT[Btn8R]; // Returns 1 or 0
RT_8Down = vexRT[Btn8D]; // Returns 1 or 0
RT_8Left = vexRT[Btn8L]; // Returns 1 or 0