Don's Electronic Thing part 2 -- Software V1.0

Here is my sketch (program) for my Don's Electronic Thing V1.0

  I put in some blank sections for some of the other Lego computer bricks.  The program is a little repetitive.  I think I will re-write some of it later.  To use this, download it to your arduino, and open the serial monitor (button in upper right corner of the arduino ide).  When it says "Ready" on the serial monitor, type any commands in the space at the top.  Anyway, here is the current working version:

/*
Don's Electronic Thing Version 1
 -- Drive lego(cr) computer bricks from an Arduino Uno.
 -- Working on:
 MicroScout program 4.
 */

// init variables
String message="Test Message!";
// Pin assignments
int MS1Led=2;
int MS1Sw=3;
int MS2Led=4;
int MS2Sw=5;
int MS3Led=6;
int MS3Sw=7;
int IRTrn=8;
int IRRec=9;
int BTTrn=10;
int BTRec=11;
int BTConLed=12;

// MicroScout direction variables
int MS1=1;
int MS2=1;
int MS3=1;
int MSNum=0;
int MSAct=0;

// Infared variables


// Bluetooth variables



void setup() {
  //pinmodes
  pinMode(MS1Led,OUTPUT);
  pinMode(MS1Sw,INPUT);
  pinMode(MS2Led,OUTPUT);
  pinMode(MS2Sw,INPUT);
  pinMode(MS3Led,OUTPUT);
  pinMode(MS3Sw,INPUT);
  pinMode(IRTrn,OUTPUT);
  pinMode(IRRec,INPUT);
  pinMode(BTTrn,OUTPUT);
  pinMode(BTRec,INPUT);
  pinMode(BTConLed,OUTPUT);
  //serial setup/output ready
  Serial.begin(9600);
  delay(5000); // give user time to open serial monitor.
  Serial.println("Ready");  
}

void loop() {
  // listen for message
  // recieve messages from the serial monitor
  // in future will be from a BT HC-05 module

  while(Serial.available()==0) {
  }
  message=Serial.readString();
  Serial.println("you typed:");
  Serial.println(message);
  //Serial.println(message);
  // process message
  // if message starts with a two letter code then 'gosub'
  // to appropiate 'subroutine'
  if (message.startsWith("MS0")) {
    MS0();
  }
  if (message.startsWith("MS")) {
    MicroScout(); 
  }

  if (message.startsWith("VL")) {
    VLL();
  }

  if (message.startsWith("RS")) {
    RCXSend();
  }

  if (message.startsWith("RR")) {
    RCXRec();
  }

  if (message.startsWith("BM")) {
    Manas();
  }

  if (message.startsWith("YS")) {
    SpybotSend();
  }

  if (message.startsWith("YR")) {
    SpybotRec();
  }

  if (message.startsWith("CS")) {
    ScoutSend();
  }

  if (message.startsWith("CR")) {
    ScoutRec();
  }



  // end of voidloop
}


// code for MS program 4
void MicroScout() {
  /*
 now to pickout the ms number, and function number
   MS0x -- send same command to all MicroScouts.
   MS1x -- MicroScout #1
   MS2x -- MicroScout #2
   MS3x -- MicroScout #3
   0 -- stop motor
   1 -- motor forwards
   2 -- motor backwards
   */
  //Serial.println("MicroScout");

  //  Serial.println(message);

  int mlen=message.length();
  if (mlen==4 ) {

    MSNum=message.charAt(2);
    MSAct=message.charAt(3);
    MSNum=MSNum-48;
    MSAct=MSAct-48;

    //check direction here
    //if needed do reverse blink

    if (MSNum==1) {
      // check for motor direction
      if (MSAct==1 && MS1==2) {
        RevBlink();
        MS1=1;
      }
      if (MSAct==2 && MS1==1) {
        RevBlink();
        MS1=2;
      }

      if (MSAct==0) {
        digitalWrite(MS1Led, LOW);
      }
      if (MSAct==1) {
        digitalWrite(MS1Led, HIGH);
      }
      if (MSAct==2) {
        // microscout go backwards.
        digitalWrite(MS1Led, HIGH);
      }
    }

    if (MSNum==2) {
      // check for motor direction
      if (MSAct==1 && MS2==2) {
        RevBlink();
        MS2=1;
      }
      if (MSAct==2 && MS2==1) {
        RevBlink();
        MS2=2;
      }

      if (MSAct==0) {
        digitalWrite(MS2Led, LOW);
      }
      if (MSAct==1) {
        digitalWrite(MS2Led, HIGH);
      }
      if (MSAct==2) {
        // microscout go backwards.
        digitalWrite(MS2Led, HIGH);
      }
    }
    if (MSNum==3) {
      // check for motor direction
      if (MSAct==1 && MS3==2) {
        RevBlink();
        MS3=1;
      }
      if (MSAct==2 && MS3==1) {
        RevBlink();
        MS3=2;
      }

      if (MSAct==0) {
        digitalWrite(MS3Led, LOW);
      }
      if (MSAct==1) {
        digitalWrite(MS3Led, HIGH);
      }
      if (MSAct==2) {
        // microscout go backwards.
        digitalWrite(MS3Led, HIGH);
      }
    }

  }

  message="";
}

void MS0() {
  // The MicroScout "MS0x" command sends the same action to all 
  // MicroScouts.
  MSAct=message.charAt(3);
  MSAct=MSAct-48;
  message="MS1";
  message.concat(MSAct);
  MicroScout();
  message="MS2";
  message.concat(MSAct);
  MicroScout();
  message="MS3";
  message.concat(MSAct);
  MicroScout();

}

void RevBlink() {
  if (MSNum==1) {
    digitalWrite(MS1Led, HIGH);
    delay(100);
    digitalWrite(MS1Led, LOW);
    delay(100);
    digitalWrite(MS1Led, HIGH);
    delay(100);
    digitalWrite(MS1Led, LOW);
    delay(100);
    digitalWrite(MS1Led, HIGH);
  }
  if (MSNum==2) {
    digitalWrite(MS2Led, HIGH);
    delay(100);
    digitalWrite(MS2Led, LOW);
    delay(100);
    digitalWrite(MS2Led, HIGH);
    delay(100);
    digitalWrite(MS2Led, LOW);
    delay(100);
    digitalWrite(MS2Led, HIGH);
  }
  if (MSNum==3) {
    digitalWrite(MS3Led, HIGH);
    delay(100);
    digitalWrite(MS3Led, LOW);
    delay(100);
    digitalWrite(MS3Led, HIGH);
    delay(100);
    digitalWrite(MS3Led, LOW);
    delay(100);
    digitalWrite(MS3Led, HIGH);
  }
}

// code for MS VLL?
void VLL() {
  Serial.println("VLL");
}

// code for RCX
void RCXSend() {
  Serial.println("RCXSend");
}

void RCXRec() {
  Serial.println("RCXRec");
}

// code for Bionicle Manas
void Manas() {
  Serial.println("Manas");
}

// code for Spybot
void SpybotSend() {
  Serial.println("SpybotSend");
}

void SpybotRec() {
  Serial.println("SpybotRec");
}

void ScoutSend() {
  Serial.println("ScoutSend");
}

void ScoutRec() {
  Serial.println("ScoutRec");
}