text: #pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
  #pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
  #pragma config(Sensor, dgtl8,  sonarSensor,    sensorSONAR_cm)
  #pragma config(Motor,  port2,           rightMotor,    tmotorServoContinuousRotation, openLoop)
  #pragma config(Motor,  port3,           leftMotor,     tmotorServoContinuousRotation, openLoop, reversed)
  //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  
  /*
     while true:
     while sensor value > threshold:
     move_fowards()
     turn(90 degrees)
     */
  
  static const float M = 2.896;
  
  int move(int direction, int speed) {
      if (vexRT[Btn7L] && vexRT[Btn7D])
          return -1;
  
      motor[leftMotor] = speed * direction;
      motor[rightMotor] = speed * direction;
  
      return 0;
  }
  
  void wait(int milliseconds) {
      motor[leftMotor] = 0;
      motor[rightMotor] = 0;
      wait1Msec(milliseconds);
  }
  
  void turn(int degrees, int speed) {
      /*  Degrees is between -180 and 180
          Positive numbers are counter-clockwise
          Negative numbers are clockwise
  
          Turning either 180 degrees or -180 degrees is a full circle
          90 deg turn is full left
          -90 deg is full right
  
          Left turn = leftMotor backwards, rightMotor forwards
          Right turn = rightMotor backwards, leftMotor fowards
          */
  
      //Reset the encoders
      SensorValue[leftEncoder] = 0;
      SensorValue[rightEncoder] = 0;
  
      int rotations = degrees * M;
      if (rotations > 0) {
          //Left turn
          while (SensorValue[rightEncoder] < rotations){
              motor[leftMotor] = -speed;
              motor[rightMotor] = speed;
          }
      }
      else {
          //Right turn
          while (SensorValue[leftEncoder] > rotations * 1.25){
              motor[leftMotor] = speed;
              motor[rightMotor] = -speed;
          }
      }
  }
  
  task main()
  {
      while (1) {
          if (SensorValue[sonarSensor] > 40 || SensorValue[sonarSensor] < 0) {
              int exitcode = move(1, 64);
              if (exitcode == -1)
                  goto end;
          }
          else {
              wait(500);
  
              turn(90, 50);
          }
      }
  end:;
  }
  