#use "iROBOsim-v1-1\iROBOsim.ic"

// Global variables.

float pi = 3.1416;

// Wheel base, distance between wheels.

float wheel_base = 12.0;

// Wheel radius in centimeters.

float wheel_radius = (5.6 / 2.0);

// One wheel revolution in centimeters.

float one_wheel_rev = (pi * wheel_radius * 2.0);
float ticks_per_cm = 1100.0 / one_wheel_rev;

//
// PURPOSE: Turn robot left 90 degrees.
//
// RETURN: = 0 if successful.
//         != 0 if error.
//

int turn(float turn_angle)
{
    float turn_dist;
    long turn_ticks;
    
    turn_dist = ((2.0 * pi * wheel_base) * (turn_angle/360.0)) *
      1.05;
    printf("Turn distance = %f.\n", turn_dist);
    turn_ticks = (long) (ticks_per_cm * turn_dist);
    printf("Turn ticks = %d.\n", turn_ticks);
    rsim_mrp(1, 250, turn_ticks);
    rsim_bmd(1);  // Wait for completion of turn.
}


void main()
{
    int count;
    long dist_ticks;  // Distnace to move forward.
    
    // Initialize robosim.
    
    iROBOinit(RULERWORLD);
    iROBOtrail = 1;  // Draw trail as robot moves.
    printf("One wheel revolution = %f centimeters.\n",
           one_wheel_rev);
    
    dist_ticks = (long) (ticks_per_cm * 90.0);
    count = 0;
    
    while(count < 5) {
        rsim_mrp(0, 1000, dist_ticks);
        rsim_mrp(1, 1000, dist_ticks);
        
        // Wait unitl moved expected distance.
        
        rsim_bmd(0);
        rsim_bmd(1);
        
        turn(90.0);
        count++;
    }
    
    printf("Program complete.\n");
}