Preset arm heights in EasyC v4

Here is some code I used to set 3 different heights using a Pot for values. I can send the Easy C file if you like or you can cut and paste this code.

The values used for upper and lower range will be determined by the starting position of the pot.

This code will allow for 3 buttons to be used to determine a preset height and the fourth button to be used to reset the robot back for driver control. As soon as the fourth button is pushed the driver takes control. If you read the code and have problems understanding please let me know.

I have attached a PDF of the code which I documented to help you understand. Send me email and I can send the project for you to look at.

Dave G.
Latch sample.pdf (18.2 KB)

Agreed: My mistake (again), I failed to notice the doubled forever loops.
while(1) { while(1) { bulk code } }

I was going to offer an alternate opinion on use of flags,
but didn’t have time to do some trial code to see if a flagless example
was possible, and looked any cleaner.

DPbailey’s problem is most likely that he is doing this, which will freeze everything else while the arm is being set to correct position:
while( arm not in right position) { move the arm }

If dpbailey can’t figure out how to fix that after looking at jpearman example code,
then dpbailey should post (or PM) his code.

My code is a little more complicated then that but yes that is my problem. I have scanned jpearman’s code and I haven’t exactly found my answer yet but I will look through it again and if I can’t find my answer I will PM you my code.

Here is a simpler view of the key method:
while (1) {

  • if a button is pressed,
    – set some autoset variable to remember that we want to do something
    – set some autoset variable to remember where the arm should go.
    – nothing else inside this if condition, no motors were changed here.

  • If the arm joystick is in neutral deadbanded position
    – check previously set variables to see if we wanted to do something
    – if so, set the motor to move in correct direction for that and go on

  • else // the arm joystick must be trying to do something,
    erase the autoset variables to cancel the auto movement
    move the arm like the joystick wanted.
    } // wend

Here is a variation of the code from post #14 that uses a timer for the arm control task (still not a PID loop) I post to show that there are alternatives and demonstrate the repeating timer function just for fun. It’s more complex code but achieves the same thing.

/*                                                                             */
/*  A revised version of the code from Post #14                                */
/*                                                                             */
/*                                                                             */
/*  This version uses a repeating timer to control the arm                     */
/*  The arm control is simple, in that a PID loop is not used, but this        */
/*  has more potential to be able to add that                                  */
/*                                                                             */

#include "Main.h"

/*                                                                             */
/*  Define motor and sensor ports                                              */

#define LEFT_DRIVE_MOTOR    1
#define RIGHT_DRIVE_MOTOR   10
#define ARM_MOTOR           2

#define ARM_POT             1
#define LCD_PORT            1
#define LED_PORT            9

// Shadow register for values sent to motors (array element 0 not used)
static int currentMotorValue[11] = {0,0,0,0,0,0,0,0,0,0,0};
/*                                                                             */
/*  Wrapper for SetMotor so we can save the current value                      */

SetMotorJ( int index, int value )
    // bounds check index
    if( (index<1)||(index>10) )
    // Save requested speed
    currentMotorValue[index] = value;

    // Any non linear lut's or slew rate control would be added here

    // Set motor
    SetMotor ( index ,  value ) ;

/*                                                                             */
/*  Get current motor speed                                                    */

GetMotorJ( int index )
    // bounds check index
    if( (index<1)||(index>10) )
        return( currentMotorValue index ] );

/*                                                                             */
/*  Callback that is registered as a repeating timer                           */

#define MYTIMER_TIME    25

void *MyTimer()
    static  int led = 0;

    // For debugging on my system - flash LED
    SetDigitalOutput ( LED_PORT , led = 1 - led ) ;

    // Call the arm control function

/*                                                                             */
/*  Arm control task                                                           */

#define ARM_MANUAL_CTL      10000
#define ARM_STOP_POSITION   10128
#define ARM_AUTO_UP_SPEED   100
#define ARM_AUTO_DN_SPEED   (-100)

// Some preset positions
#define ARM_POSITION_1      200
#define ARM_POSITION_2      300
#define ARM_POSITION_3      400
#define ARM_POSITION_4      500

// How close we will try and position the arm to the requested position

static int desiredArmPosition = ARM_STOP_POSITION;

    int ctlValue;
    int armPot;

    // Read potentiometer on arm
    armPot = GetAnalogInput ( ARM_POT ) ; 
    // New code does not use a flag to indicate auto/manual
    // but rather special values of desiredArmPosition.
    // If desiredArmPosition > 10000 then manual control
    // motor speed is (desiredArmPosition - 10000) - 128;

    if( desiredArmPosition >= ARM_MANUAL_CTL )
        // Manual control
        ctlValue = desiredArmPosition - ARM_STOP_POSITION;
        // Automatic control

        if( Abs( desiredArmPosition - armPot ) < ARM_POSITION_THRESHOLD )
            // done - stop motor
            ctlValue = 0;
            desiredArmPosition = ARM_STOP_POSITION;
        else // Move Up ?
        if( desiredArmPosition < armPot )
            ctlValue = ARM_AUTO_UP_SPEED;
        else // Move down ?
        if( desiredArmPosition > armPot )
            ctlValue = ARM_AUTO_DN_SPEED;

    // Send ctl speed to motor
    SetMotorJ( ARM_MOTOR, ctlValue );

/*                                                                             */

void MyUserControl()
    int joyCh2, joyCh3, joyCh4;
    int but8U, but8D, but8R, but8L;
    int butPressed;
    int buttonIsPressed = 0;
    // Init LCD
    InitLCD ( LCD_PORT ) ; 
    SetLCDLight ( LCD_PORT , 1 ) ; 

    // Init our timer with 25mS repeat
    RegisterRepeatingTimer(  MYTIMER_TIME, MyTimer );

    // Do forever
        // Read analog joysticks
        joyCh2 = GetJoystickAnalog( 1 , 2 ) ; 
        joyCh3 = GetJoystickAnalog( 1 , 3 ) ; 
        joyCh4 = GetJoystickAnalog( 1 , 4 ) ; 

        // simple arcade drive
        SetMotorJ( LEFT_DRIVE_MOTOR  , (joyCh3 + joyCh4) / 2 ) ; 
        SetMotorJ( RIGHT_DRIVE_MOTOR , (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
        // run if no buttons are pressed or we are waiting for a press
        if( !buttonIsPressed || !butPressed )
            // Check for any button or none
            if( butPressed )
                buttonIsPressed = 1;
                buttonIsPressed = 0;
            switch( butPressed )
                case    1:
                    // Right button
                    desiredArmPosition = ARM_POSITION_1;

                case    2:
                    // Left button
                    desiredArmPosition = ARM_POSITION_2;

                case    4:
                    // Up button
                    desiredArmPosition = ARM_POSITION_3;

                case    8:
                    // Down button
                    desiredArmPosition = ARM_POSITION_4;

                    // All other cases

        // See if we are manually controlling the arm
        // don't use joystick near 0
        if( Abs( joyCh2 ) > 10 )
            // Manual control
            // This will override any auto movement
            desiredArmPosition = ARM_STOP_POSITION + joyCh2;
            // If we were in manual control then set to stop
            // other wise ignore.
            if(desiredArmPosition >= ARM_MANUAL_CTL )
                desiredArmPosition = ARM_STOP_POSITION;             

        // display arm pot,requested position and motor speed
        SetLCDText ( LCD_PORT , 1 , "pot %d mot %d", GetAnalogInput ( ARM_POT ), GetMotorJ( ARM_MOTOR ) ) ; 
        SetLCDText ( LCD_PORT , 2 , "req %d", desiredArmPosition ) ; 
        // wait - no need to run any faster
        Wait ( 25 ) ; 

I’m working on a ‘simpler’ version with no flags, but it might be too clever, with just less whitespace. I’ll post it later, and you can poke at it in turn.

Are the following items in your code bugs, or just features that I don’t understand?

int motor[10]; //? should be [11] to get [0…10] right?

if ( !buttonIsPressed ) //? 2 places; why is this needed?

case 4: //? will UP be detected if LEFT is also pressed?

if( desiredArmPosltion > armPot ) //? logically unnecessary test, has to be true, right?

Yes, should be 11, I forget that EasyC is 1 based for motor indexing not 0 (like RobotC)

It’s a flag to only detect the first time a button is pressed, everything is then ignored until that button is released.

No, that was the idea. if up and left are pressed then butPressed will be 6. If you want to use detection of two buttons as a valid condition then perhaps the code would be different.

Should be true, just habit I’m afraid, after so many years of coding I just keep doing things the same way without thinking. Sometimes I forget what the else condition is so I just add it.

Stupid forum software (or me as user) doesn’t copy nested quotes…

re buttonIsPressed:

I don’t see the point. Key-roll-over from Up to Down would ignore the Down.
Is there any harm in setting the desiredArmPosition to the same value
each time through the loop?

re UP and LEFT is not detected as UP

OK, different assumptions on usage.
Its simpler to look at just one button = one function, and ignore bit-merging.
In the unlikely event that you need more events than you have independent buttons, you’ll probably have to start dedicating some buttons to “escape/meta/alt/control/shift/2nd” functions, with a latching feature.

What happens with your code when the arm sags after reaching its desired point?

Here is my restrained whitespace version; I’ve avoided most temptations to clever,
other than demonstrating a dummySwitch instead of twisty chain of Elses.

// derived from jpearman code,  mistakes are my own -- jgraber
// Joystick2 will override and cancel the button operation.
// it just drives the motor until the pot feedback is close to the requested position. 
// The motor speeds are saved as variables, then SetMotor at end
// The LCD is used for debug but it could also be a print to screen.
//Here is the code, it compiles, but not downloaded or tested
#include "Main.h"

void MyUserControl() {
    int joyCh2, joyCh3, joyCh4;
    int but8U, but8D;
    int desiredArmPosition = -1;
    int armPot;
    int motor[11]; // skip motor[0], use motor[1] .. motor[10]
    int dummySwitch = 0; // never changed
    InitLCD (     1 ) ;  
    SetLCDLight ( 1 , 1 ) ; 
    while(1)  {     // Do forever
        // simple arcade drive
        joyCh3 = GetJoystickAnalog( 1 , 3 ) ; 
        joyCh4 = GetJoystickAnalog( 1 , 4 ) ; 
        motor 1]=(joyCh3 + joyCh4) / 2  ;  // converted to late
        motor[10]=(joyCh3 - joyCh4) / 2  ;  // SetMotor( n, motor]) at end

        // find out which buttons are pressed
        but8D = GetJoystickDigital ( 1 , 8 , 1 ) ; // position 200
        but8U = GetJoystickDigital ( 1 , 8 , 2 ) ; // position 100

        // just a bunch (2) of independent switch checks, no ELSEs
        // highest priority from the bottom up
        if ( but8U ) { desiredArmPosition = 200; } // up value
        if ( but8D ) { desiredArmPosition = 100; } // down value

        // See if we are overiding the arm button with joystick
        joyCh2 = GetJoystickAnalog( 1 , 2 ) ; // Arm joystick
        armPot = GetAnalogInput ( 1 ) ;       // arm potentiometer analog port 1
        // # see perl programming v3; aka camel3 p 125, or  blue camel p104
        switch ( dummySwitch ) { // fake switch, just to allow break
            default: { // everything is default; break is jump to end
                if( Abs( joyCh2 ) > 10 ) {  //  joystick > deadband limit check
                    motor[2] = joyCh2 ;     // manual control of motor
                    desiredArmPosition = -9999; // cancel automatic control
                    break; } 
                if ( desiredArmPosition < 0 ) { // reuse as autoArmControl
                    motor[2] = 0;  // stop motor if no auto and joy < deadband
                    break; }  
                if ( Abs( desiredArmPosition - armPot ) < 10 )  { // close enough
                    motor[2] = 0 ; // done, stop arm motor
                    // no need to cancel auto control, since arm might sag later
                    break; } 
                if ( desiredArmPosition < armPot ) { 
                    motor[2] = 100  ; // raise arm at this speed
                    break; }
                if ( desiredArmPosition > armPot ) {
                    motor[2] = -100 ; // lower arm at this speed
                    break; }
                } // end default
            } // end dummySwitch

        // converted to late motor updates to support future code features like E-off, slew control
        SetMotor (  1 , motor 1] ) ; //wheels
        SetMotor ( 10 , motor[10] ) ; //wheels
        SetMotor ( 2 ,  motor 2] ) ; //  arm
        // display arm pot,requested position and motor speed
        SetLCDText ( 1 , 1 , "pot %06d mot %06d  :", armPot, motor[2] ) ; 
        SetLCDText ( 1 , 2 , "req %06d  :", desiredArmPosition ) ; 
        Wait ( 25 ) ; // no point in asking for input faster than motor can update
                      // although button press shorter than 25ms may not register...
        } // wend (1)
} // user control

It allows the joystick to override even when the button is still down. It’s just how I like to do things, edge detection rather than level detection.

Again, a personal preference. More than one button being pressed is an error.

I did not even try and address this. If the arm were to sag then I would use a PID (or perhaps just P) loop. The arm would oscillate if using the simple case.

You can also use

do {
} while(0);  

It will achieve the same thing as the dummy switch.

Anyway, it’s good to show the different approaches. The more examples the students can see the better.

Hey, I was reading over the code given in this thread as well as the Latch_sample given by piniontwister and i just have one question. How would I make it so I have to hold the button to keep the arm moving to a preset height, as opposed to just pressing it once. Also, once I let go of the preset height button, it will resume operator control (even if target height has not been reached)? If i am understanding the code already given, they implement a “one button switch” method of reaching the height, so I was wondering how to do it in terms of a button HOLD. I think I should mention that I am using a joystick (not button) to control arm in operator control. Also, as a side note, are the code sets given been tested to work, or are they missing key information ( I realize motor/sensor ports and channels being used will be different). I am working in EasyC V4.

In the case of the code in post #25 you would probably change a couple of lines of it to something like this.

            // Check for any button or none
            if( butPressed )
                buttonIsPressed = 1;
                    desiredArmPosition = ARM_STOP_POSITION;
                buttonIsPressed = 0;

This simply overrides whatever had been placed into that variable with the special position for manual control when the button was released. It’s just a patch, there may be a better way.

Yes the code was tested but please understand that it’s example code not production/competition code. You may want to change motor speeds or perhaps add additional code to hold an arm in position, this will depend very much on your design. I would not use this code verbatim but study all the examples available and then create your own based on your programming style and goals.

If butPressed is an integer, then how can it be used as a true/false logic operator? I noticed in the code in post #25, you also used buttonIsPressed in the same way, and im not quite sure how that works.


I will let the authors of the C language explain, this is an excerpt from K&R.

To add to the discussion on manual control, I handle manual overrides as follows:

I first process the control loops based on the last set position.

If the joystick is outside of a deadband or state is stateMAN (I used typedef enums, so state is an enumerated type), I then overwrite the motor power output by the control loop above with the joystick value and set stateMAN as the current state. Since this is handled last, pressing the manual joystick overrides everything, and letting go of the stick keeps the command at 0. You then press another button to command a different state, and it goes as commanded without holding the button.

I finally write the actual motor. I keep a copy of what the output would have been, and print it to the debug terminal (along with the current output), depending on what debug data is enabled, so I can see what it is trying to do without letting it do it.

Oh ok, thats interesting. I didn’t know about that, because my only prior experience is with Java, and I haven’t come across such a functionality. Now, I’ve re read your code and i have a question.

butPressed = (but8D << 3) + (but8U << 2) + (but8L << 1) + but8R;

if( !buttonIsPressed || !butPressed )
// Check for any button or none
if( butPressed )
buttonIsPressed = 1;
buttonIsPressed = 0;

Since you have already calculated the butPress value before the if-statements and it can only enter the first if-statement if butPressed is 0, then how come you need to check the butPressed value again, in the if-statement right after? Won’t it still be 0? Overall, I’m just having trouble understanding this portion of the code (and how the original refers to one press and the suggestion at the top of this page makes it so button needs to be held).

Remember there are many ways to skin a cat :slight_smile:

Over the years we all develop a particular programming style, it’s a bit like art, sometimes it can be quirky and this may be one of mine.

Anyway, the if statement will evaluate to true under one of two conditions.

A button is not pressed OR there is no button pressed.
if( !buttonIsPressed || !butPressed )

After the if statement I again test to see if a button is indeed pressed and set the flag indicating this accordingly.

Then I use the conditional switch statement to decide what to do next.

It’s not always important to understand exactly why every line of code was written in a particular way as long as the underlying idea is understood.

Were any of you guys able to recreate Joshua Wade’s autolift/driving code? I looking at it now and I don’t understand how he was able to continue driving while his “autolift” was running.

Do you mean Josh AKA Titan AKA team 1103 ? Are you looking at his code from last year? If so there’s nothing special, Driver_Control has a loop that handles driving and manual lift control, that in turn calls Motor_Drive() where he drives his lift motors, Motor_Drive() also calls Script_Functions() which has a series of state machines that control “scripted” functions such as de-scoring. It’s easier to understand if you export everything as text files rather than trying to use the flowchart view.

That sounded like I was minimizing Josh’s code, I’m not, he did a great job. It’s just that the code is long so looks complicated and can be hard to follow, break it down into it’s constituent pieces and you will find that it has many of the elements that are used by many teams.

What I mean is, how did his lift run at the same time as his drive? I thought that autolift loops temporarily cut off your controls from the drive train.

I’m looking at it now in text mode and it seems like he just breaks up his program into 3 different parts (lift, gripper, drive). I don’t see where they intersect (if they do at all) allowing him to drive while his lift automatically goes to the set position.

Ref: Team 1103 Code thread ;

At the top of driver_control.c there is while(1) This is the main while loop.

  • first get all the joystick inputs to variables
  • //manual lift control, sets a bunch of flags set on variables
  • // drive train does scaled arcade drive, and sets drive motors
  • // script input, sets a bunch more flags and variables
  • last line inside the while loop calls Motor_Drive() function


  • if script != off (same as script is on) call Script_Functions() to set variables
  • a bunch of conditions to set Lift_Motor variable
  • SetMotor 3,4,5,6,9,10 to Lift_Motor variable
  • SetDigitalOutputs for gripper and tilt based on previously set variables
  • SetMotor 1,2,8,7 for the drivetrain motors (again)


  • set a bunch of variables based on other variables

Both Motor_Drive its subroutine Script_Functions are in the main while loop,
so thats where everything intersects.

Its an implementation of SPA: Sense Plan Act.
SetMotor and SetDigitalOutputs are the ACT part, based on variables (not joysticks)
All the mucking about with variables is the PLAN part, and is the most complicated!
Some of the variables are set from Joysticks and Sensors, the SENSE part.

First step is to throw away “tank2” precoded drive train code and make your own.

  • get joystick
  • set variables
  • set motors

Then add the idea of state-machines;

  • keeping variables to remember what you wanted to be doing.
  • Make a list of “states”, and make rules (if/then/else) for when to switch from state to state.

Once you do that, then you have the ability to remember to ignore the joysticks and use the statemachine (instead of joystick) to set the variables for the gripper and lift.