Moving until a certain position is reached
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 ); } |
This tip was posted by jpearman over at http://www.vexforum.com/showpost.php?p=222061&postcount=9.
