GHCHS Robodox 599D Sets World Record at Golden State VEX Robotics Championship

Programming skills world record = 35 points.

Video of world record.‎

Quoted from the Robodox Facebook page:

559A, B, and D competed at CAMS’ VEX competition today. 599B made it to the quarterfinals, 599A made it to the semifinals, and 559D won in their division and were comp finalists, losing in the first match they didn’t play in, making a great comeback by winning the next match (and beating the tactics of this robot-entangling-and-what-I-thought-was-pinning-wall-’bot with their alliance partner) and finally, losing the last match of the round by two points. And 599D won the Excellence Award!! They also beat the world record for the Programming Skills Challenge and won the Drive Skills Challenge at the comp 599B was the second place for the Drive Skills Challenge.

A GHCHS writeup

 

As a mentor, I’m proud to say I didn’t have much to do with this:)

Note: Vex Gyro (revised 1/4/2012)

Revised based upon more accurate information for Vex people.

I bought a Vex Gyro at Worlds and decided to try it out with RobotC.

The gyro chip is a MEMS ST  LY3100ALH  1000 deg/sec max rate gyro.   The Vex people amplified the output by a factor of 1.511 so that the nominal 3 V max is 4.533 v.   Don’t know why they didn’t amplify it to 5v … but it results in a gyro scale factor of 600 deg/s/volt.   The cortex equation that converts A2D to dps can be derived as follows:

Vgyro = (dps*.0011 + 1.5) * 1.511 = dps* .00166 + 2.267 volts

A2D = Vgyro*2/3*4095/3.3 = Vgyro*827.27 counts

or  A2D = (dps*.00166 + 2.267)*827.27

or A2D = 1.374 * dps + 1874

The inverse gives

dps = (A2D-1874)*.7277

Setting the Gyro Deadzone:

The noise density is .016 deg/s/√(HZ) and the bandwidth is 140 hz so the noise output   = .016*√140 = .19 dps rms.    The A/D sampling resolution is  1 bit  from above is  .728 dps.  The standard deviation of the sampling noise is typically about .289 LSB  or .21 dps.   RSS of these two sources is  .283 dps.   Typically we set the deadzone at the  2 sigma level of the total noise sources or .57 deg/s.  This  is less than 1 count on the A/D output so the dz should be placed at 1 counts  to keep the gyro integration from drifting.   I use 2 counts to add some margin.   Without this deadzone you would expect to see a random walk drift on the integration of the gyro angle when sitting still.

Corrected 1/4/2012

When constantly turning with rates that exceed the deadzone,  the rms rate noise contributes about

 sqrt(Time*dt) *.283 degs rms

angle drift where dt is the gyro integration step period.     If one updates at 100 hz then the rms drift is sqrt(Time)*.0283  So with a 1 minute programming challenge time we would expect around  .22 deg rms of drift.   Which is not too bad.

Bias Calibration Time

How long should we average the bias before starting the integration to get angle.   Using an average to estimate of the bias sees the standard error drop by 1/√N where N is the number of samples.   We want enough samples to drive the standard error down to where it doesn’t contribute significantly to the total drift rate after 60 second programming challenge.   If we pick a 10% increase in the total drift error then we can tolerate 1 deg error/60 sec.  From this we can calculate N

1/60 = 1/√N *.283

so N = 288

So we can generally sample at 1 per ms so a 1 second averaging time should be sufficient.

Vex Pendulum Robot:

I wanted to use this with the accelerometer to reconstruct the Vex bbot inverse pendulum robot… however it will be still noisy in tilt angle due to the low resolution of the gyro.   My last bbot used a 400 dps max  with a 10 bit A/D so with the Cortex 12 bit A/D and 1000 deg/sec max we would not see any improvement.  I was hoping for a variable scaling gyro that would allow me to use 100 to 200 deg/s maximum for this application.

Test Results:

The RobotC version 2.32 gyro type did not work with this gyro yet.   I wrote a  RobotC gyro test software that used the A/D to measure the voltage and then process it with my own task process_gyro(). Running this process calculates an unbiased rate gyro output and gyro angle from integrated rate.     The gyro worked pretty well with a 500 hz integration rate and a 1 second initialization time.   Typically a rotation to 180 deg and back was within 5 degs.  There is noticible cross axis sensitivity and also sensitivity to EMI noise so you don’t want to mount this on your processor.

For those interested here is the program:

#pragma config(Sensor, in1,    gyro,                sensorPotentiometer)
//*!!Code automatically generated by ‘ROBOTC’ configuration wizard               !!*//
//Program author:  Chris Siegert
//Vamfun@yahoo.com
//Mentor Vex Team 599 and 1508a
//Global Variables
//The gyro board scales the 3 volt chip to 4.533v with gain factor 1.511
//this gives 600 dps/volt scale at board
const int gyro_init_time_ms = 1000; //ms
const float sf_gyro = 600.*5./4095.;  //units dps_per_cnt = 600 dps/volt*5volt/4095cnts
const int gyro_dz = 2 ;// dead zone in a/d counts (approx .67 deg_per_sec per count)
const int gyro_int_step_size = 2; // integer ms step size 500 hz nominal
//Chip Vdd = 3v  and Nominal gyro voltage output = 1.5 v -> 4095*/3.3*2/3*1.5*1.511. = 1874 cnts
int gyro_bias = 1874;  //nominally 1874 cnts
int gyro_angle = 0; //gyro angle degs  1 deg resolution
int rate_cnts=0; //unbiased gyro rate a/d counts
int gyro_rate_x10 = 0;//unbiased gyro rate deg/sec*10

//Tasks
task Process_gyro()
{
float cum = 0;
//………………. initialize gyro …………………………….
time1[T1]= 0;
int count = 0;
while(time1[T1]< gyro_init_time_ms)
{
count++ ;
wait1Msec(1);// this gives about 1 count per ms to average
cum = cum + (float)(SensorValue[gyro]);

}
gyro_bias =  cum/count;
cum = 0;

//………………. integrate gyro……………………………….
while(true)
{
int dt = time1[T1];
rate_cnts = SensorValue[gyro]-gyro_bias;
gyro_rate_x10 =10*sf_gyro*(float)rate_cnts;

if(dt >=gyro_int_step_size)
{
float error = SensorValue[gyro]-gyro_bias;

if(error>gyro_dz || error< -gyro_dz)
{
cum = cum + dt*error*sf_gyro/1000.;  //integrate if |error| > dz
}
ClearTimer(T1); //reset timer after use rate pulse
}
gyro_angle= cum;//scale to 1 deg/sec resolution
}
}
task main()
{

StartTask (Process_gyro);

}

Note:RobotC Debug Competition Modes

Here are some observations regarding the RobotC Debug Competition Control Window:

1) Once a mode is selected it does not change even with a recompile/ redownload , Start/Stop/Start recycling,  clear all or Closing and Reloading RobotC.

2) Selecting DISABLE while in AUTONOMOUS or DRIVER CONTROL will not change the mode.

3) Start/Stop/Start recycling will only run pre autonomous when you are in the DISABLE mode to begin with.

4)Start/Stop/Start recycling seems to run a clear all no matter what mode you are in.

Note: Running simultaneous script utility functions

In regards to 599_Competition_Scripting_Template:

Suppose you want to combine two script steps into one and run them simultaneously.       Here is a sample script that moves a distance and has a 2 second timeout in case the move doesn’t finish.

void Script_name_1()
{
switch(step)
{

case 1:

int done1= Move_dist_ch1(300, 50); //command a rate of pwm 50 and move to dist1 = 300
int done2=Ms_timer_T1(2000) ;  // waits 2 seconds before done

done = (done1>0  ||  done2>0) ; //  Sets done = 1 when either done1 or done2 is completed else o.

break;

default:
Reset_scripts();
}
}

done = (done1>0 || done2>0)  is equivalent to the following  if test and is easier.
if(done1>0 || done2>0)
{done = 1;}
else
{done = 0;}

We had trouble with this method in the past because we did not set done = 0 as the default.  This was normally set to zero by the  utility function  returning a zero.  E.g.  done = Move_dist_ch1(..) .

Changing this to done1 = Move_dist_ch1(..)  did not reset done = 0 so we have make sure to use the above coding.

Updated 599 Competition Scripting Template

Here is a draft copy of rev 2 template.  It creates a new function ..Call_script(int name) that allows calling scripts from within a script and cleans up a few areas by taking user modified code areas and isolating them to separate functions.

I. e.   Script_name_map()  was created to let the user map his function names to the function call.  This code used to be buried within Run_script().  Now it is called by Run_script();

Also,  Reset_auto_flags() contains code that used to be buried within Reset_scripts();   Reset_scripts now just calls this function.

599_Comptetition_Scripting_Template_rev 2.c

Notes on using Cortex A/D for discrete inputs

Revised: 12/13/2010

Ok, I admit it, I was so used to using the PIC 5v processor , that I assumed that the 5v pull-up resistor would give a digital 4096 (12 bit max value)  reading with an open pin (no connection).     The cortex, however, only outputs around 250 counts .    This is understandable when you examine the  following interface facts:

1) the cortex is a 3.3 v ARM processor

2) there is 470k pull up resistor connected between the input pin and a 5v supply

3)there is a 20k pull down resistor connected between the A/D input and ground.

4) there is a series input resistor of 10k connected between the input pin and the A/D input.

So with no input, we would expect a digital value of     5v*(20k)/(470k+20k + 10k) *4096/3.3v=248 . This is just ohms law applied to series resistors working as a voltage divider.  This is indeed close to what I see using the debug with a  potentiometer sensor type.

With a plug shorting the input to ground ,  the debug shows a digital value of 5 .   This should be zero in a perfect world since the  input to the 10k series resistor is  now grounded and the 470k cannot pull up the potential since it too has been grounded.

If you want to use an A/D as a discrete input port connected to a touch sensor or plug then the RobotC logic should  use the following code:

bool b_input = SensorValue(pot) < 125; // Reads true when plug in or sw closed

I would like RobotC  to offer the option to connect a touch sensor to an A/D.  But right now only the POTENTIOMETER is appropriate.

Using a pot input to generate discrete states for selecting autonomous scripts.

A simple equation that uses modulo arithmetic can be used to generate an integer number of inputs.

Let  max_no_inputs and script_number  be integers.

then

script_number =( SensorValue[pot]*max_no_inputs)/4096 + 1;

script_number will run from 1 to max_no_inputs based upon the pot setting.  Here we are making use of the integer division truncation to generate an integer output.  We just have to add a pointer and some division marks on the pot to indicate what area of the pot we are in.  In the past I have use a 12 tooth gear filed to a point and glued a dial to the pot face.

Using plugs to create digital states:

Suppose we want four states:  1, 2, 3 , 4

This requires two boolean inputs generated from two A/D ports  plug1 and plug2.

b1 = SensorValue(plug1)<125   and b2 = SensorValue(plug2)<125

Now we can map these states to integers in two ways:

1) If statements

if(!b1 && !b2)  { script_no = 1;}   // no plugs in

else if(b1 && !b2)  {script_no = 2;} // plug1 in , plug2 out

else if(!b1 && b2) { script_no = 3;} // plug2 in , plug1 out

else   {script_no = 4;}  // both plug1 and plug2 in

2 Forming an integer using binary loading of a word.

int script_no =  1*b1 + 2*b2  + 1  ;

If we add another plug3 to get b3 then we can get 8 states;

int script_no =  1*b1 + 2*b2  + 4*b3 + 1  ;

Pot Nonlinearity note:

The counts are not linear with pot rotation due to the fact that the series and pull-down resistors are about the same magnitude of the Vex potentiometer (10k) and they load the pot down.  One can derive the formula for the exact counts using circuit theory.   If you ignore the 470k pull-up resistor (it has negligible current draw when a pot is present) then the counts are given by

counts = 4096*5/3.3*( p*2)/((1-p)*(p+ 3) + 3*p)  where p is the pot ratio [0 to 1]

If p = 1 ( pot outputs 5 volts)    counts = 4096*10/9.9  or about full value

If p= 0 (pot outputs 0 volts)   counts =  0

if p = .5     counts = 4096* 5/3.3*1/3.25= 4096*( .466)

The .466 would be .5 if the pot wasn’t loaded down.  So it is about a 6% error.     Correction software could use the inverse of the above counts eq and solve for p given counts.  This would be the real ratio for a given count.

Note: Adding some braking to the move_encoder(.) function

Most of the 599 teams are using gear ratios that increase speed and decrease motor drag force.    As a result, the move_encoder(…) function provides a lot of overshoot due to the robot momentum since it is not a PID loop.    Typically, a 24 in/s top speed can produce 6 to 9 in of overshoot.  So we need some braking that is also adjustable.  I will do a few calculations to estimate how long it takes a typical robot to slow down from the max speed when given a full reverse command.   The best way is to measure the deceleration response, however,  it can be estimated analytically by using  some assumed dynamics for the robot/motor system.

Analysis:

Robot assumptios:

wt = 10lb

max_tq = 26 inlb (2 393 motors per side with 13 inlb per side )

gr = 84/60= 7/5  (wheel speed to motor speed)

g = 12*32.2= 386.4 in/s/s  (gravity)

r_in = 2.  ( wheel radius)

max_accel = g*max_tq/(gr*r_in*wt) = 359 in/s/s or roughly 0.9 g’s

max_speed = 100rpm*2pi/60*gr*r_in= 29.3  in/s

tau = max_speed/max_accel =  .081 sec  (motor time constant)

Assuming that the motor is a first order lag,  the speed response  to a step reverse command  input  of  (v_0 – v_final)  follows the equation

v =  v_final + (v_0 -v_final)*exp(-t/tau)

where v_0 is the initial speed and v_final is the final steady state speed.

Here , v_0 = max_speed and v_final = -max_speed  and we want to solve for t that makes v= 0.

So substituting and solving for t

exp(-t/tau) = -v_final/(v_0-v_final)   or

t = -tau*ln(.5) = .69*tau

Thus the time to drop the speed to zero is   t =  .69*.081 = .056 sec

The average speed over this period is about max_speed/2 so the

stopping_dist  ~= max_speed*t/2= 29.3*.056/2 = .85 in

Discussion:

The robot can decelerate from max speed to zero speed in less than .06 sec  and  the over shoot should be less than .85 in.     We know that a heavier robot, e.g. 20 lbs , will have twice the tau, hence  for design purposes, I’ll assume  a max reverse brake command  with a nominal .1 second pulse and make the pulse duration adjustable  to accommodate different weight robots.    We can add enough time for the robot to back-up and have zero overshoot.     This tweeking  can also be done automatically by a PID controller with a high gain but the timed pulse technique  has the advantage of a minimum time controller since it uses maximum command rather than proportional commands.   The PID approach  will be a topic of future discussions.

The max_acceleration did not take in to account the dynamic friction of the drive train and we would expect the actual robot to slow a little faster since the 393 motors have about 1 inlb of drag torque.

Braking software code:

We require a parametric timed pulse (nominally .1 sec) of max motor reverse command (127).   A timer is needed to put in a signed pulse after the distance goal has been crossed in the Move_encoder (..) code.    When using Move_encoder(..) we would just set the goal distance and then tweek the brake time to minimize the overshoot without reversing.

If pwm_brake = o then braking calculations can be bypassed and the robot is free to coast with just motor drag to slow it.

Move_encoder(..)  with braking changes might look like this:

UPDATED 12/5 to add b_brake_done logic to keep output latched when using Move_encoder with other utility functions in the same step. Changed the name to Move_&brake_encoder(..)

int Move_&brake_encoder(int goal , int pwm_input , int brake_time10 = 10)
{
// test routine similar to what might be used for encoders
// goal is the incremental distance you want to go from where you are
// goal is + or – relative distance in dist_10in units (deci inches)
// brake_time10 is the reverse motor brake pulse time in hundredths of a sec
// pwm_fwd , dist_10in , bsys_reset, bauto_move are global
static int target_dist = 0;
static int target_time = 0;
static bool b_move_done = false;

static bool b_brake_done = false;
if(bsys_reset)
{
//initialize stuff for output
bauto_move = true; // take control of the motors that are used in this function
target_dist = dist_10in+goal; //compute goal in reference axis units.  Input goal is relative to where you are
if(goal<0)
{
pwm_fwd = -pwm_input;//move in direction of goal
}

else
{
pwm_fwd = pwm_input;
pwm_brake = -pwm_brake; //need to reverse brake sign
}
b_move_done = false;

b_brake_done = false;
return 0;
}
else
{
if( (goal<0 && dist_10in > target_dist || goal>0 && dist_10in < target_dist )&& !b_move_done) return 0;
else
{

// start braking
if(!b_move_done)
{
target_time = time10[T1] + brake_time10;
b_move_done = true; // keeps the target_time from being reset while we wait
pwm_fwd = pwm_brake; //brake in direction opposite of goal
}

else if(time10[T1]< target_time && brake_time10  !=0 && !b_brake_done) return 0; // wait for braking
else
{//done braking so exit

b_brake_done = true;

bauto_move = false; //return control to joystick
pwm_fwd = 0 ; //set the speed to zero again
return 1;// sets done = 1
}
}

}
}

Formulas and Checks for Encoder Derived Outputs

For Autonomous operation,  two  variables derived from encoder position are very helpful.  I usually prefer to work in engineering units rather than encoder counts.   The two are:

dist = average of left and right encoder in inches, positive when moving forward.

psi_deg = difference between left and right encoders in degrees of heading , positive increase when turning right.

If enc_rt and enc_lt are in counts (360 per revolution) then  in software we compute

dist = (enc_rt+ enc_lt)/2 * dist_per_cnt.

heading  psi_deg = (enc_lt-enc_rt)*deg_per_cnt.

where

enc_lt = SensorValue(left_encoder)  (RobotC notation)

enc_rt = SensorValue(right_encoder) (RobotC notation)

dist_per_cnt = 2*pi*radius_wheel/360 cnts

and

deg_per_cnt = 57.3*dist_per_cnt/dist_wheel_base

Test Procedure:

Simple two step procedure to verify that these are implemented ok:

First compute the distance a wheel should go in one revolution:

d_rev = 2*pi*radius_wheel

Second compute the angle the robot would turn if the a left or right wheel turned one revolution while the other was stationary.

psi_deg_rev = 57.3*d_rev/dist_between_wheels.

Debug Outputs:

Now… put robot on blocks

Turn the right wheel one revolution forward:  Check  psi_deg = – psi_deg_rev.    ,  dist = d_rev/2.

Next turn the left wheel one revolution forward: Check psi_deg = 0  , dist = d_rev.

For a typical robot with 4 in wheels with a wheel base of 14 inches

dist_rev = 12.6 in , psi_deg_rev = 57.3*12.56/14 = 51.4 deg

dist_per_cnt = 12.6/360= .035    , deg_per_cnt = 51.4/360 = .142

 

Integer vs Floating Point Implementation

The above formulas are ok if using a floating point implementation.  If  an integer implementation is used , I usually select a precision of 1 deg and .1 inches (deci inch).  To remind myself of the output units in deci inchs I rename dist as   dist_10in.  So the calculations are done in floating point and then converted to integer.  The new Cortex allows floating point.

int dist_10in=10.*(float)(enc_lt+enc_rt)/2.*dist_per_cnt

int psi_deg = (float)(enc_lt-enc_rt)*deg_per_cnt

 

Working with two joysticks and a Cortex

I did a little testing with the legacy joysticks.  It turns out that the exact code that we use for the Vexnet will work on channels 5 and 6 if you want discrete outputs.

Channels 5 and 6 put out a -127 when the upper button is pushed(nearest the antenna) and 127 when the lower button is pushed if you call them with this code:

// Bottom Buttons – Joystick 1 crystal
//RT_5Down =  vexRT[Ch5]; // Returns 127 or 0
//RT_5Up =    vexRT[Ch5]; // Returns -127 or 0
//RT_6Down =  vexRT[Ch6]; // Returns 127 or 0
//RT_6Up =    vexRT[Ch6]; // Returns -127 or 0

If you call them with the vexnet joystick code… you get 1 or 0 for the crystal joysticks.

I.e.

// Side Buttons Vexnet,Bottom Buttons Crystal – Joystick 1
RT_5Down =  vexRT[Btn5D]; // Returns 1 or 0
RT_5Up =    vexRT[Btn5U]; // Returns 1 or 0  Up is nearest the antenna on crystal
RT_6Down =  vexRT[Btn6D]; // Returns 1 or 0
RT_6Up =    vexRT[Btn6U]; // Returns 1 or 0  Up is nearest the antenna on crystal

Debug still has the vexnet joystick connected to the programming port.  When the crystal rx is plugged in to either rx1 or rx2 it automatically disables the vexnet joystick but still allows debug through the programming port.

Here is a c test program that contains a proposed Joystick_599() function that reads both joysticks of either type.  Nothing has to be changed in the code to use the crystal joysticks.

The only difference in names is the use of RT2 for joystick 2… E.G.

RT_5Down for joystick 1

RT2_5Down for joystick 2

More stuff on Joysticks

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******************************************************