[jpearman]

There have been several questions recently about moving part of the robot until it has reached a certain position. I though I would post a RobotC example showing how to drive a motor forwards or backwards while monitoring an encoder. This same idea could be used for raising a lift or anything else where an encoder is used to monitor the rotation of a motor.

Anytime a loop is used waiting for a terminal condition to be found it's important to think about what will happen if something goes wrong. This code also shows how to use a timeout so that if an error occurs, the motor stalls for example, the test will stop after a period of time.

This code is a simple example and does not use multi tasking or the built in timer both of which could be used as part of this.

#pragma config(Sensor, dgtl1,  encoder, sensorQuadEncoder)
#pragma config(Motor,  port2,  MyMotor, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/* Drive motor until an encoder has counted for a number of counts             */
/* or a timeout period has passed.                                             */
/*                                                                             */
/* Assumes the encoder will count up when the motors are running forwards      */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

#define TIMEOUT_CNT_PER_SEC    10
#define MOTOR_FWD_SPEED        64
#define MOTOR_REV_SPEED      (-64)
#define MOTOR_STOP_SPEED        0

int 
DriveByEncoder( int encoder_count, int timeout_in_seconds = 5 )
{
    int  timeout;
    
    // Drive motor until encoder has moved a number counts or
    // timeout_in_seconds seconds have passed

    // Zero the encoder
    SensorValue[ encoder ] = 0;
   
    // Run the motor forwards or backwards
    if( encoder_count > 0 )
        motor[ MyMotor ] = MOTOR_FWD_SPEED;
    else
        motor[ MyMotor ] = MOTOR_REV_SPEED;
        
    // run loop until encoder hits encoder_count counts or timeout reached
    
    for( timeout=(timeout_in_seconds*TIMEOUT_CNT_PER_SEC); timeout > 0; timeout-- )
        {
        // check encoder
        if( encoder_count > 0 )
            {
            // going forwards
            if( SensorValue[ encoder ] >= encoder_count )
                break;
            }
        else
            {
            // going backwards
            if( SensorValue[ encoder ] <= encoder_count )
                break;
            }
        
        // wait 1/10 second
        wait1Msec( 100 );
        }
        
    // Stop the motor
    motor[ MyMotor ] = MOTOR_STOP_SPEED;
    
    // See if we sucessfully found the right encoder value
    if( timeout <= 0 )
        {
        // there was an error - perhaps do something
        // return error
        return (-1);
        }
    else
        // return success
        return 0;
}

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  Test the new function                                                      */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

task main()
{
    // forwards until 200 counts
    DriveByEncoder( 200 );

    // short wait
    wait10Msec( 200 );
    
    // backwards for 3000 counts with a 10 second timeout
    DriveByEncoder( -3000, 10 );
    
    // demo done
    while(true)
        wait1Msec( 10 );
}