Sample Auto_lift RobotC program

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:

auto_lift.BDS
auto_lift.ECP

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

//function prototypes
void Joystick();
int auto_lift(int goal, int rate, int tol); //sets lift pwm and moves until withing tol of goal
void lift_function();

//Global Variables
//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;

//lift variables
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

task main()
{
while(true)
{
//Read Joystick
Joystick();
//Read Sensors
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
pwm_lift_cmd =0;
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;

}
else
{
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

//Core functions
auto_lift(lift_goal, LIFT_RATE,LIFT_TOL) ; // modifies pwm_lift_cmd if lift_goal != NO_GOAL
lift_function();

}//end while
}//end task main
void lift_function()
{

//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;
else
if(lift_pos – LIFT_PICKUP < LIFT_TOL && lift_pos-LIFT_PICKUP >-LIFT_TOL) lift_state = LIFT_PICKUP;
else
if(lift_pos – LIFT_DROP < LIFT_TOL && lift_pos-LIFT_DROP >-LIFT_TOL) lift_state = LIFT_DROP;
else
if(lift_pos – LIFT_HANG < LIFT_TOL && lift_pos-LIFT_HANG >-LIFT_TOL) lift_state = LIFT_HANG;
else
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;
}
else
{
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;
//}
// else
//{
//  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;
}
else

if(error < -tol)
{
pwm_lift_cmd = rate;
return 0;
}
else //we are within the tol so we have met the goal
{
pwm_lift_cmd = 0; //stop moving
lift_goal = NO_GOAL;
return 1;
}

}
void Joystick()
{
//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
}

End Code******************************************************

Advertisements

One Response to Sample Auto_lift RobotC program

  1. Isidra says:

    That is a great tip especially to thjose new to the
    blogosphere. Short but very preciise information… Thank you for sharing this one.
    A must read article!

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: