PgmCnt: 000184
Type: 2

#pragma config(Sensor, S1,     signalLight,         sensorCOLORFULL)
#pragma config(Sensor, S2,     touchSensorB,        sensorTouch)
#pragma config(Sensor, S3,     touchSensorC,        sensorTouch)
#pragma config(Sensor, S4,     distanceSensor,      sensorSONAR)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void motorPID(bool motorPID){
  if(motorPID){
    nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;
    nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg;
  }
  else{
    nMotorPIDSpeedCtrl[motorB] = mtrNoReg;
    nMotorPIDSpeedCtrl[motorC] = mtrNoReg;
  }
}

task main(){
  ubyte messageReceived[1];
  int lastMessageReceived;
  setBluetoothOn();
  ClearTimer(T1);
  nxtDisplayCenteredTextLine(2, "Just received:");
  while(true){
    lastMessageReceived = messageReceived[0];
    cCmdMessageRead(messageReceived,1,1);
    if(messageReceived[0] != lastMessageReceived){
      ClearTimer(T1);
    }
    if(time100(T1) <= 600){
      SensorType[signalLight] = sensorCOLORRED;
    }
    else{
      SensorType[signalLight] = sensorCOLORNONE;
      motorPID(true);
      motor[motorB] = 30;
      motor[motorC] = 30;
      ClearTimer(T2);
      while(SensorValue(touchSensorB) == 0 && SensorValue(touchSensorC) == 0 && time100[T2] < 100){}
      motor[motorB] = 0;
      motor[motorC] = 0;
      wait10Msec(50);
      motor[motorB] = -20;
      motor[motorC] = -20;
      wait10Msec(400);
      motor[motorB] = 0;
      motor[motorC] = 0;
      wait10Msec(100);
      nMotorEncoder[motorB] = 0;
      motor[motorB] = 15;
      while(nMotorEncoder[motorB] < 530){}
      motor[motorB] = 0;
      ClearTimer(T2);
      while(SensorValue(touchSensorB) == 0 && SensorValue(touchSensorC) == 0){
         if(SensorValue(distanceSensor) < 43){
           motor[motorB] = 20;
          motor[motorC] = 5;
         }
         else{
           motor[motorB] = 5;
          motor[motorC] = 20;
         }
       }
       motor[motorB] = 30;
      motor[motorC] = 30;
      wait10Msec(100);
      motor[motorB] = 0;
      motor[motorC] = 0;
      motorPID(false);
      while(messageReceived[0] == lastMessageReceived){
        lastMessageReceived = messageReceived[0];
        cCmdMessageRead(messageReceived,1,1);
      }
      motor[motorB] = -40;
      motor[motorC] = -40;
      wait10Msec(300);
      motor[motorB] = 0;
      motor[motorC] = 0;
      nMotorEncoder[motorB] = 0;
      motor[motorB] = 40;
      while(nMotorEncoder[motorB] < 1000){}
      motor[motorB] = 0;
      ClearTimer(T1);
    }
    switch(messageReceived[0]){
      case 0:
        nxtDisplayCenteredTextLine(4, "NOTHING");
        motor[motorB] = 0;
        motor[motorC] = 0;
        motorPID(false);
      break;
      case 1:
        motorPID(true);
        nxtDisplayCenteredTextLine(4, "FORWARD");
        motor[motorB] = 20;
        motor[motorC] = 20;
      break;
      case 2:
        motorPID(true);
        nxtDisplayCenteredTextLine(4, "RIGHT");
        motor[motorB] = 15;
        motor[motorC] = -15;
      break;
      case 3:
        motorPID(true);
        nxtDisplayCenteredTextLine(4, "LEFT");
        motor[motorB] = -15;
        motor[motorC] = 15;
      break;
      case 4:
        nxtDisplayCenteredTextLine(4, "STOP");
        motor[motorB] = 0;
        motor[motorC] = 0;
        motorPID(false);
      break;
    }
    wait10Msec(5);
    alive();
  }
}