Preset arm heights in EasyC v4

We are planning to program an arm to reach 3 pre-set heights (in EasyC v4). During operator control, we would like to use 3 of the 4 buttons on Channel 7 or 8 to trigger the arm to these heights. I’ve looked at a few threads on this topic and have a few questions.

  1. We’ve looked at using the Joystick to digital input but were wondering if it would be more efficient to use an interrupt, since we wouldn’t be continually using the “jump to level 1/2/3” function as much as we use direct control of motors with joystick buttons. I don’t have a very good concept of interrupts and am not sure how the buttons on Channel 7 could be used to trigger one, so if this is a good solution, any explanation would be appreciated.

  2. Is it better to program the arm to continue raising until it reaches a certain height (ignoring operator control), or to allow O/C to occur simultaneously? Or would it be better to ignore O/C input but have an “override” condition (perhaps the 4th unused button) that kicks the program out of its arm-raising task and restores Operator Control in an emergency?

  3. Any opinions or cautions on using Switch/Case rather If/Then/Else? We’ve used the latter, but not the former.

  4. Are there any multi-tasking issues that I should be aware of?

  5. Any suggestions on threads on this topic? The ones I’ve been using are:

This is the first year in quite a while when we’ve had the mechanics of the robot completed more than a week before competition and programmers who are potentially capable of the above, so these are uncharted waters for us.](

You can program it to where you do not need your “kill switch” if you want to call it that. We have our button commands and they work along with the basic operator control. How experienced are you/your programmers? My team mate has figured it out and I have been programming for quite a while and I can make my way around pretty efficiently and I still do not understand the programming logic of it. It is far to complicated for a beginner to be trying to accomplish on there own. Doing so will envolve either a shaft encoder or potentiometer on both sides and a bumper/ limit switch on the bottoms. You could be better off just building a really stable arm that you can drive easily. I will see if I could possibly get you an example of the code that we have in EasyC V4.

The proper way to do this is to take a series of inputs, and determine a desired state request, then take the state request, and attempt to achieve it.

The first portion requires you to have a variable to store the desired state, and a few lines of code in your main loop which polls the inputs and sets the desired state to the calibrated position when the button is pressed. If no button is pressed, you can leave the desired state as it is.

The second portion takes the desired state and feedback sensor, and drives the motor. This portion is always running, and always taking the sensor feedback and desired state, and driving the motor.

Additions include manual overrides (killing the output of the feedback controller before it reaches the motor, and replacing it with the manual input), and input diagnostics on the sensor (checking to see if the sensor is in range, shorted, or non-existant).

The basic form of feedback controller is a proportional controller, where the output to the motor is proportional to the error in the signal (the difference between the desired state and the sensor feedback). Some people implement full PID (Proportional Integral Derivative) controllers, others don’t. Start with P, you probably don’t need I or D.

This entire thing can be done without tasks at all. I would write the HMI (Human Machine Interface) code in the main loop, and write the feedback controller in its own function, and call it every loop.

Edit: It’s not really that hard. The actual code is as follows (in psudocode):

//Variables used overall
int setpoint,sensor,error,p,motor = 0;
/* HMI code section in operatorControl*/
if(btn1) setpoint = 1024; //This is a calibrated position
if(btn2) setpoint = 2048; //This is another calibrated position
if(btn3) setpoint = 4098; //This is a third calibrated position

/* Feedback control code section in its own function */
sensor = GetAnalogValue...
error = setpoint - sensor;
p = error * kp; //kp is a calibrated gain
motor = p;//Add the terms - There is only one term

GetJoystickDigital would be the function to use. An interrupt watcher is for detecting changes on a digital IO port.

I would allow operator control to override the arm moving so that if the driver changes their mind then there is no delay in getting control back. This is personal preference, there’s no right or wrong. It also depends on how operator control is done, ie. analog joystick or say up/down buttons.

If you end up with many if/then/else statements ie. if A else if B else if C else if D … Then a switch statement is usually better. In the case of checking multiple buttons I would usually combine them into one variable and then use a switch statement, this allows easier detection of multiple buttons being pressed at the same time. Lets look at pressing button 8 left and right, the easyc code for this may look like (b_left and b_right are variables)

b_left = GetJoystickDigital( 1, 8, 3 );
b_right = GetJoystickDigital( 1, 8, 4 );

b_left and b_right could then be combined into one variable (buttons) as follows.

buttons = b_left + (b_right * 2);

buttons could have 4 possible values, 0 = no buttons pressed, 1 = left button, 2 = right button and 3 = both buttons.

you could then use a switch statement

switch( buttons )
    case 0:
        // no buttons
    case 1:
        // do something for the left button
    case 2:
        // do something for the right button
    case 3:
        // how do we handle both buttons ?

Anyway, it just a code style thing. If then else statements will work also.

Well, multi-tasking is a whole other topic but in general don’t end up running the same small piece of code that stops all other code from executing. Remember that the operator expects a response to their input in about 1/10 of a second so if your code goes off and does not check, for example, the joystick for a long time driving will be hard.

Whether you need proportional control (or a full blown PID loop) will depend on the sensors available and the mechanical design of the arm. If the arm will stay balanced with no motor driving it you may just need to compare pot/encoder feedback to a desired position and then shut off the motor when you reach it.

I’m probably a lower intermediate programmer, and the students are advanced beginners (worked 2 years with LEGO NXT, so they have basic concepts but not EasyC syntax).

We’re already using a potentiometer to lift to a preset level for autonomous, so sensor use is not a problem. Why would we need potentiometer on both sides, if the 2 sides are lifting in tandem? We do have 2 quad encoders on the driving base for navigation.

Since the function is called every loop (and the motor is reset every loop), it appears that the driver must keep the button pushed until the arm reaches its destination, rather than touching the button once and having the arm rise to the set point. Is that correct?

Also, in your code example, would it be advisable to have some error tolerance, so that the motor isn’t continuously running if the error is small but not exactly zero? I’m guessing that it would be pretty hard to have zero error, but I’m not sure whether very low levels of motor output (probably at stall level) would be a problem.

@jpearman, thanks for your clearcut explanations and examples. This will save us a lot of time.

it will also be easier if you held the joystick button until it reached the preset rather than just tapping it

it will also make life easier if the arm was always higher or below the height (30" goal) so you only need to code in the movement of the arm raising or lowering

if you want all those features, then thats great, but you should start with something simple, and then build on it

i find that “holding” the button is easier because then you wouldnt need a “kill switch”
i still use it today because i have no need for a more “complex” system (which may or may not be better based on personal preference)

As you can see from the replies already there are a number of ways to solve the problem. We have used a couple different methods in the past depending on the game and our experience with programming. in general I walk the students through the design process what type of control do we want ? One year we simply designated a button to reflect a specific arm height as described by aplrd in that post. The following year we opted to build a program that just used 2 buttons one to move up to the next position and one to move down to the next position. If I remember right we had 2 routines one to get operator input and to set up the appropriate ‘action’ for that selection. Another routine was to move the arm based upon the selected action.

We also considered having an ‘override’ capability as a valuable feature. In our case the arm was controlled by the operator manually using the analog joystick, so we checked for joystick input and if the input from the joystick was above a certain threshold (aka deadband) then the robot would drop out of programmed mode and revert to operator control.

as an example consider joystick button 5 up and down are your user inputs to move the arm up or down to the next position and joystick 3 is your manual arm control. to determine if we should drive the motor under program control or joystick control we set a global variable (say armControlMode) also consider using a global variable for the button state say ‘buttons’ you might also consider a global for the desiredArmPosition

Psuedo code follows

testUserInput () {
get value of button5Up;
get value of button5Down;
get value of joystick3;

// I like jpearman’s suggestion of using a case statement so I would follow that suggestion

buttons = button5Up + (button5Down*2);

// now determine arm control mode auto or manual
if ( abs(joystick3) > deadband) {
armControlMode = manual;
else if (buttons > 0) { // a button was pressed
armControlMode = auto;

// Based upon the button pressed select what arm motion you want

switch (buttons)
case 0:
// do nothing special no buttons pressed
// This has the benefit of continuing the motion once it’s started
case 1:
// move up if button5Up is pressed
check if arm is already at upper limit
if not set desiredArmPosition to the next higher value
case 2:
// move down if button5Down is pressed
check if arm is at lowest position
if not set desiredArmPosition to next lower value
case 3:
// define what you want both buttons to do (maybe calibrate the arm position?

} // you can end test for user input here you now

In your control loop for the arm you test the value of ‘buttons’ and take the appropriate action (jpearman has a great example but for completeness…

armControl() {

// use your favorite P, PI, PID control flow here to move the arm

// depending on how well the arm ‘stays’ in a location you can set motors to Zero or put a ‘hold’ value in place once you get to the desired position


Finally in the main loop decide which control mechanism should be called based upon the ‘state’ selected by user input

main () {

while(1) {
testUserInput () // get inputs from driver

if (armControlMode == auto) {
else {
pass joystick3 value to the motor controlling the arm


This is just psuedo code but going through this step helps drive out some more of the the details needed for final code implementation.

I hope this helps.

Cheers Kb

The motor is always set by the function.
The function call is not based on the button state, it is always called.
So, you don’t have to hold the button.

The error tolerance could be a deadband around the motor output, but usually very low power to a motor is common with only P control. An I term handles low steady-state error, but is usually not needed in competitive robotics. Also, if the arm will fall down, you will get a small amount of output power to hold it in place.

I wrote some fairly good P-only arm code (in C) for a few Cortex robots this past year, and posted it here (vex code). It uses an enumerated type to set the desired state, and a manual control override using a joystick. arm_process() is called by the main task every 25ms. arm_map_dig_to_state is fed a digital joystick button and maps it to a state when the button is pressed. This code is more complex than basic code, but includes more features such as input diagnostics on the sensor, a deadband on the output, manual control via a joystick, and such.

Congrats on early completion. Make sure your drivers have time to practice.
Some teams have a separate “programmers bot” to develop code for, without interrupting practice on the real robot.

Re #3 switch opinions:
A twisty maze of if/then/else all alike are hard to navigate later, especially if not well commented.
Switch is nice, but it forces you to have just one variable to check,
and it shouldn’t need the ‘break;’ a listed previously.

Here is pseudo code for a similar technique suggested by a Perl pgm book:

  • Use a fake loop that only repeats 1 time, so you can use “break;” to exit the loop;
  • If (cond1) { do the right thing for cond1; break;}
  • If (cond2) { do the cond2 thing; break;} // always break at end
  • whoops error message, if appropriate, when no conditions were met but you required a condition
  • end of loop
  • other code


  • Since every chunk ends with break, you don’t have to look further down for something else that might be happening inside the then/then/then/else/else

Your q2 about operator control (OC) override has several answers already, but I highlight these key points:

  • OC cant be done without some kind of override to the automatic,
    otherwise, A) Autobutton says do this, B) but OC joystick is idle, so don’t do it.
    A 4th button, or joystick > deadband was suggested as the override.

  • It works well to set/modify a variable, then do single assignment of SetMotor(var) once per loop; Otherwise bad coding will generate jittery motors.
    Autobutton sets variable;
    If override, then change variable to GetJoyStickAnalog
    SetMotor( variable)

you need the “break;” otherwise the next lines of code will also execute.


switch( var )
    case 0:
    case 1:

If var is 0 then both foo() and bar() will run.

Some more code for an auto_lift is posted on my blog. It is similar in structure to Apairds code in that it doesn’t use a separate task. There is an EasyC version included in the post although the text of the post uses Robotc. I always include a dead zone around the position error to quiet the motors once you reach the goal. The post assumes the motors are set to zero when you reach the goal… but typically you need a hold value if the friction/elastic is insufficient to hold the lift. We have often programmed the hold value as a function of the lift position to optimize it.

I have made a code for this for my robot. It goes to all 3 goal heights. I really like it and want to use it in my robot’s competition code but whenever I press the button i can’t control anything else until the button command is over. I’m sure there is a way to fix this.

Does anyone have any input for me? I can post a picture of my code if needed. I need to be able to control my robot at the same time that the button command is running.

If someone could help it would be awesome!

The general idea is that if you have any loops that wait for a condition to be met before continuing, you will block all other functions. You need to make checks for terminating conditions but also keep servicing the joystick and other functions. Post what you have and perhaps we can help you.

This topic keeps coming up so here is a more complete example. This code will drive the robot using left joystick in arcade mode and control a motor to raise/lower an arm. Button 8 Up and Down drive the arm motor to preset positions based on feedback from a pot. Obviously a simple example but hopefully you will get the idea. To make it easier to post here, all the operator code is in a user function called from OperatorControl(), in this way.

The right joystick will override and cancel the button operation. This code does not use any sort of PID loop, it just drives the motor until the pot feedback is close to the requested position. The arm motor speed is saved in a variable for debug purposes (as I don’t know how to read the current speed a motor is running). The LCD is used for debug but it coupd also be a print to screen.

Here is the code, only partially tested as I don’t have any hardware available.

// UserControl.c : implementation file
#include "Main.h"

void MyUserControl()
    int joyCh2, joyCh3, joyCh4;
    int but8U, but8D, but8R, but8L;
    int butPressed;
    int buttonIsPressed = 0;
    int autoArmControl = 0;
    int desiredArmPosition = 0;
    int armPot;
    int motor[10]; // for debugging
    // Init LCD
    InitLCD ( 1 ) ; 
    SetLCDLight ( 1 , 1 ) ; 

    // Do forever
        // simple arcade drive
        joyCh3 = GetJoystickAnalog( 1 , 3 ) ; 
        joyCh4 = GetJoystickAnalog( 1 , 4 ) ; 
        SetMotor ( 1 ,  (joyCh3 + joyCh4) / 2 ) ; 
        SetMotor ( 10 , (joyCh3 - joyCh4) / 2 ) ; 

        // find out which buttons are down
        but8D = GetJoystickDigital ( 1 , 8 , 1 ) ; 
        but8U = GetJoystickDigital ( 1 , 8 , 2 ) ; 
        but8L = GetJoystickDigital ( 1 , 8 , 3 ) ; 
        but8R = GetJoystickDigital ( 1 , 8 , 4 ) ; 

        // combine into a single variable
        butPressed = (but8D << 3) + (but8U << 2) + (but8L << 1) + but8R;

        // Decide what to do
        switch( butPressed )
            case    0:
                // No buttons
                buttonIsPressed = 0;

            case    1:
                // Right button

            case    2:
                // Left button

            case    4:
                // Up button
                if( !buttonIsPressed )
                    buttonIsPressed = 1;
                    desiredArmPosition = 2000;
                    autoArmControl = 1;

            case    8:
                // Down button
                if( !buttonIsPressed )
                    buttonIsPressed = 1;
                    desiredArmPosition = 1000;
                    autoArmControl = 1;

                // All other cases

        // See if we are overiding any buttons
        joyCh2 = GetJoystickAnalog( 1 , 2 ) ; 
        // don't use joystick near 0
        if( Abs( joyCh2 ) > 10 )
            // no automatic control
            autoArmControl = 0;

            // manual control of motor
            SetMotor ( 2 ,  motor[2] = joyCh2 ) ;

            // for debugging
            desiredArmPosition = 9999;
        if( !autoArmControl )
            // stop motor
            SetMotor ( 2 ,  motor[2] = 0 ) ;

        // Read potentiometer on arm
        armPot = GetAnalogInput ( 1 ) ; 

        // Control arm automatically
        if( autoArmControl )
            // Automatic control
            if( Abs( desiredArmPosition - armPot ) < 10 )
                // done
                autoArmControl = 0;
                SetMotor ( 2 ,  motor[2] = 0 ) ;
            if( desiredArmPosition < armPot )
                SetMotor ( 2 ,  motor[2] = 100 ) ;
            if( desiredArmPosition > armPot )
                SetMotor ( 2 ,  motor[2] = -100 ) ;
        // display arm pot,requested position and motor speed
        SetLCDText ( 1 , 1 , "pot %d mot %d", armPot, motor[2] ) ; 
        SetLCDText ( 1 , 2 , "req %d", desiredArmPosition ) ; 

        // wait
        Wait ( 25 ) ; 


jperman’s code is a perfect example of a twisty chain of if/then/else with multiple motor settings.
It works when the code is right.
When small changes break the code, the motor will be set multiple times, and jitter, and I find it hard to debug why that is happening.

If you change all the SetMotor(2,motor[2]=val); commands to just motor[2] = value;
then do one SetMotor( 2, motor[2]); just above the Wait(25);
you will never have a motor jitter problem, even with small wrong changes to if/then/else.
What will happen instead
(with small wrong changes that set motor[2] to different values multiple times)
is that the last set of motor[2] will be effective,
and the earlier settings of motor[2] don’t do anything, so it doesn’t “do what I meant”.
In my small experience, when you get one fixed value output, its easier to debug than when you get a jittering output. hmm, the wait 25 ms at the end will serve almost the same function.

How about this argument then:
With just one SetMotor( 2, motor[2] ); line near the end,
it is also easier to add an E-stop function, slew rate control, linear filtering, max speed clipping, and/or other such features, if you choose.

re SetLCDText %d"; I’ve had trouble with similar PrintTextToGD using format %d, when a change from ‘value 100’ to ‘value 9’ shows ‘value 900’ on the screen.
Maybe LCD (vs GD) does’t have that issue.
I find this format works better for me on PrintTexttoGD
“pot %06d mot %06d :” the trailing space and : act as end of line marker; if characters show up after the :, you can ignore them.
This will generally print ‘value 000100 :’ then ‘value 000009 :’ to keep nice columns.

I think this code also requires holding a button to go to a position, not “fire and forget”.
This feature is partly due to clearing all the variable each time through the main while(1) loop.

I didn’t think it was that bad :slight_smile: One of the problems when writing examples is that you want to try and highlight what you are trying to teach in a simple and straightforward way without to much “clever” code to confuse the reader. I would normally have separated the drive and arm control into different functions, I would also not use SetMotor directly but have a wrapper function around it to store the current state and allow such things as non linear control. Oh well.

Ok, agreed. Unfortunately I added the motor] array right at the end when I realized that EasyC had no GetMotor function, I should probably have deleted all that.

I agree with using %06d (%6d, same thing, no zeros) but found the LCD does not have this issue. SetLCDText seems to overwrite the whole line.

No, you not need to hold the button down. Push the button, the desiredArmPosition is set, the button will not be used again until is is released.

The variables are not cleared each time through the loop. The “MyUserControl” function does not return. The joystick and buttons are checked each time through the loop. The arm potentiometer is checked against the requested arm position each time through the loop. Some variables are set each time but nothing is initialized. Although I don’t have a robot handy to run this on I do check the code runs on the cortex. The only bug I see now is that I think pots have a 10bit range in EasyC (meaning values from 0 to 1023) rather then the 12 bit range in RobotC. This means the arm positions of 2000 is out of range.

Could someone please help me?

There are several good suggestions in this thread and I tried to post a complete example last night. I’m not sure what else to suggest. You could show what you already have and we can comment, or perhaps ask specific questions on what I posted yesterday.

Yes I looked at the other suggestions and I am thinking about trying them soon. Right now I have been working on this program for a while now and I really want to get it just right.

All I need to know is how to make it so that I can still control the bot with the joystick while it is running through the button command. I have tried a hand full of things and I have come up short every time.

Would I need to post my code?
Could you help me with this?

If you want to keep your code private (as most would) then send me a PM, I’m happy to help but it’s hard without seeing what you have so far.