Lego Mindstroms Robot War: Team SoundCloud Firware for RobotC

sc_sumo.c

#pragma config(Sensor, S1,     color,               sensorCOLORFULL)
#pragma config(Sensor, S4,     sonarSensor,         sensorSONAR)
#pragma config(Motor,  motorA,          power,         tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          right,         tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  motorC,          left,          tmotorNormal, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*--------------------------------------------------------------------------------------------------------*\
|*                                                                                                        *|
|*                                         - Moving  Forward -                                            *|
|*                                            ROBOTC on NXT                                               *|
|*                                                                                                        *|
|*  This program runs your robot forward for a designated amount of time.                                 *|
|*                                                                                                        *|
|*                                        ROBOT CONFIGURATION                                             *|
|*    NOTES:                                                                                              *|
|*    1)  To change the forward movement's speed, replace the two "100"'s with the desired speed number.  *|
|*    2)  To change the duration of the movement, replace the "4000" with the desired number of           *|
|*        milliseconds.                                                                                   *|
|*                                                                                                        *|
|*    MOTORS & SENSORS:                                                                                   *|
|*    [I/O Port]              [Name]              [Type]              [Description]                       *|
|*    Port C                  motorC              NXT                 Right motor                         *|
|*    Port B                  motorB              NXT                 Left motor                          *|
\*---------------------------------------------------------------------------------------------------4246-*/

string sColorT = "Black";
string sColor = "White";
int iSpeed = 40;

int iSpeedT = 60;
int iTimeT = 1400;
int maxTurn = 180;

int iSonar = 0;
int iSonarMin = 30;

int turnCount;

int state = 0;

void getSonar() {
  iSonar = SensorValue[sonarSensor];
}

void getColor() {
  switch (SensorValue[color]) {
  case BLACKCOLOR:    sColor = "Black";     break;
  case BLUECOLOR:     sColor = "Blue";      break;
  case GREENCOLOR:    sColor = "Green";     break;
  case YELLOWCOLOR:   sColor = "Yellow";    break;
  case REDCOLOR:      sColor = "Red";       break;
  case WHITECOLOR:    sColor = "White";     break;
  default:            sColor = "???";       break;
  }
}

void forward(int speed) {
  motor[motorC] = speed;
  motor[motorB] = speed;
  wait1Msec(5);
}

void turn(int speed) {
  motor[motorC] = speed;
  motor[motorB] = -speed;
  wait1Msec(5);
}

task main() {
  while (true) {
    getSonar();
    nxtDisplayCenteredTextLine(1, "%i", iSonar);
    nxtDisplayCenteredTextLine(2, sColor);

    switch (state) {
    case 0:
      forward(iSpeed);
      getColor();
      if (sColor == sColorT) {
        state = 1; //turn
        turnCount = 0;
      }
      break;
    case 1:
      turn(iSpeedT);
      turnCount += 1;
      if(turnCount > maxTurn  ) {
        state = 2; //STOP turing
      }
      break;
    case 2:
      int speed = (iSonar < iSonarMin) ? 100 : iSpeed;
      if (iSonar < iSonarMin) {
        motor[motorA] = -speed;
      }
      forward(speed);
      getColor();
      if (sColor == sColorT) {
        state = 1; //turn
        turnCount = 0;
      }
      break;
    }


  }
}