Code for the Cart and Pole Problem

// This is the experimental code for the cart-and-pole problem. 

// Give names to the drive motor and the rotation and touch sensors. 

#define DRIVE OUT_B 
#define ANGLE SENSOR_1 
#define FRONT SENSOR_2 
#define REAR SENSOR_3 

/* 
   We need to calculate the difference between vertical and the angle
   of the pole.  We assume that the cart is on a level surface.  The
   location of the two touch sensors determines two extreme angles
   with respect to the vertical of the pole.  We assume that these
   angles are equal but opposite in sign.  If either of these
   assumptions is false, then we can expect some problems.  The TEMP
   variable is used in calibrating the controller and determining the
   value of the rotation sensor that corresponds to the pole being in
   the vertical position. 
*/

int TEMP ;
int RANGE ;
int TARGET ; 
int RESPONSE ;

/* 
   We introduce a proportionality constant to experiment with various
   proportional controllers. It might be a better idea let this be a
   variable and then use the ability of NQC to send a message to the
   robot to change this variable so you can run several experiments
   without having to recompile and download code for each experiment.
*/

#define PCONST 2

#define ERROR (TARGET - ANGLE) 

task main() {
    /* Initialize the sensor inputs. */
    SetSensor( ANGLE, SENSOR_ROTATION ) ;
    SetSensor( FRONT, SENSOR_TOUCH ) ;
    SetSensor( REAR, SENSOR_TOUCH ) ;
    /* Indicate to the robot handler that he or she should move the
     * pole to the rear-most position thereby depressing the touch 
     * sensor with the probe attached to the pole for that purpose. */
    PlaySound( SOUND_DOUBLE_BEEP ) ;
    /* Determine the reading of the rotation sensor when the pole 
     * is leaning against the rear touch sensor. */
    ClearSensor( REAR ) ;
    until ( REAR == 1 ) Wait(25) ;
    TEMP = ANGLE ;
    /* Indicate that the reading was successfully taken. */
    PlaySound( SOUND_UP ) ;
    /* Pause half a second. */
    Wait(50) ;
    /* Indicate to the robot handler that he or she should move the
     * pole to the front-most position. */
    PlaySound( SOUND_DOUBLE_BEEP ) ;
    /* Determine the reading of the rotation sensor when the pole is 
     * leaning against the front touch sensor and use this together with 
     * TEMP to determine a reading for the vertical position of the pole. */
    ClearSensor( FRONT ) ;
    until ( FRONT == 1 ) Wait(25) ;
    TARGET = TEMP - (TEMP - ANGLE) / 2 ;
    /* Also calculate the full range of readings for the angle of the pole. */
    RANGE = abs( TEMP - ANGLE ) / 2 ;
    /* Indicate that the reading was successfully taken. */
    PlaySound(SOUND_DOWN) ;
    /* Allow the robot handler time to raise the pole into the starting 
     * position which probably should be close to vertical for best results. */
    Wait(200) ;
    /* Set the front and rear touch sensors to pulse mode and clear them. */
    SetSensor( FRONT, SENSOR_PULSE ) ;
    SetSensor( REAR, SENSOR_PULSE ) ;
    ClearSensor( FRONT ) ;
    ClearSensor( REAR ) ; 
    /* Announce that the robot is ready to start. */
    PlaySound( SOUND_LOW_BEEP ) ;
    /* Until the pole hits the front or rear touch sensor the robot tries 
     * to balance the pole by moving the cart backwards and forwards. */
    until ( FRONT > 0 || REAR > 0 ) {
        /* Set the response proportional to the absolute value of the error. The 
         * argument to SetPower must be an integer between 1 and 7 inclusive. */
        RESPONSE = PCONST * ((8 * ERROR) / RANGE) ;
        if ( RESPONSE != 0 ) {
            if ( abs(RESPONSE) > OUT_FULL ) {
                SetPower( DRIVE, OUT_FULL ) ; 
            } else {
                SetPower( DRIVE, abs(RESPONSE) ) ; 
            }
        } ;
        if ( RESPONSE > 0 ) {
            Fwd( DRIVE ) ;
            On( DRIVE ) ;
        } else if ( RESPONSE < 0 ) {
            Rev( DRIVE ) ;
            On( DRIVE ) ;
        }
        else Float( DRIVE ) ;
    } ;
    /* If the pole hits the forward or rear touch sensors, 
     * float the drive motor allowing the cart to roll freely. */
    Float( DRIVE ) ;
}