My Cart

How to Control a Servo through the Serial Port Using Bluetooth Connectivity Kit

Difficulty Level: Intermediate -

1. Introduction

This tutorial allows the user to move a servo via Bluetooth with an SPP (Serial Port Profile) application.

Ingredients:

Preparation Time: 30 minutes

Buy now
NOTE: Depending on the platform chosen to develop the project, the ingredients and the schematics shown in this tutorial can vary.

This project can be developed with Arduino or Intel Galileo. It is also compatible with Raspberry Pi using the Raspberry Pi to Arduino shields connection bridge.

For further information about the Bluetooth Pro Shield, consult the main tutorial.

Step 1: Connection

Connect the XBee shield to Arduino and then connect Bluetooth PRO module or you can connect it directly to Raspberry Pi connection bridge. Connect the servo as you can see in the next diagram.

Connect Black/Brown wire of the servo to GND, Red wire to 3.3 volt and the remaining wire, Yellow/Orange, to digital pin 9.

Step 2: The Code

Arduino:

Code:
/*  
 *  Bluetooth module PRO
 *  
 *  Copyright (C) Libelium Comunicaciones Distribuidas S.L. 
 *  http://www.libelium.com 
 *  
 *  This program is free software: you can redistribute it and/or modify 
 *  it under the terms of the GNU General Public License as published by 
 *  the Free Software Foundation, either version 3 of the License, or 
 *  (at your option) any later version. 
 *  
 *  This program is distributed in the hope that it will be useful, 
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
 *  GNU General Public License for more details.
 *  
 *  You should have received a copy of the GNU General Public License 
 *  along with this program.  If not, see http://www.gnu.org/licenses/. 
 *  
 *  Version:           1.0
 *  Design:            David GascĂłn 
 *  Implementation:    Luis Miguel MartĂ­
 */
 
#include <Servo.h>

char error[] = "SYNTAX ERROR";
char msg[100];
char dev;
char mac[] = "**:**:**:**:**:**";
uint32_t period;
boolean sending = false;
int turn = 0;
byte deg;
Servo myServo;

void setup() {
  
  Serial.begin(115200);
  delay(1000);
  Serial.print("Setting up...");
  while (sendATcommand("AT", "OK", error, 1000) == 2);
  while (sendATcommand("SET BT AUTH * 0000", "", error, 1000) == 2);
  Serial.print("Starting...");
  delay(1000);
  while (!pair(mac));
  delay(2000);
  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  waitApp(mac);
  sending = true;
  period = millis();
  myServo.attach(3);
  deg = 15;
  myServo.write(deg);
  
}

void loop() {

  if(Serial.available()>0){
    turn = checkMessage();
    switch(turn){
      
      case 0:
        break;
      case -1:
        if(deg-1>0 && deg-1<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case 1:
        if(deg+1>0 && deg+1<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case -2:
        if(deg-2>0 && deg-2<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case 2:
        if(deg+2>0 && deg+2<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case -3:
        if(deg-3>0 && deg-3<180){
                  deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case 3:
        if(deg+3>0 && deg+3<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
       case 4:
        myServo.write(1); 
        deg = 1;
        break;
      case 5:
        myServo.write(90); 
        deg = 90;
        break;
      case 6:
        myServo.write(179); 
        deg = 179;
        break;
    }
    while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  }

}

int8_t sendATcommand(char* ATcommand, char* expected_answer, char* error_answer, unsigned int timeout) {
  uint8_t x = 0,  answer = 0;
  char response[100];
  unsigned long previous;
  memset(response, '\0', 100);    // Initialize the string
  delay(100);
  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  if (ATcommand != NULL)  Serial.println(ATcommand);  // Send the AT command
  x = 0;
  previous = millis();
  do {
    if (Serial.available() != 0) {
      response[x] = Serial.read();
      x++;
      if (strstr(response, expected_answer) != NULL) {
        Serial.println(expected_answer);
        while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
        answer = 1;
        Serial.println("Expected response");
      } else if (strstr(response, error_answer) != NULL) {
        answer = 2;
        while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
      }
    }
    // Waits for the asnwer with time out
  }
  while ((answer == 0) && ((millis() - previous) < timeout));
  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  return answer;
}

byte pair(char* device) {
  // call to you phone (bluetooth MAC)
  char aux_string[35];
  sprintf(aux_string, "CALL %s 1101 RFCOMM\r\n", device);
  while (sendATcommand(aux_string, "CALL 0", error, 1000) == 2);
  //while (sendATcommand("CALL 8c:3a:e3:ec:77:60 1101 RFCOMM", "CALL 0", error, 1000) == 2);
  //  delay(500);
  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  byte ans = 0;
  while (ans == 0) {
    ans = getResponse("NO CARRIER 0 ERROR 305 SDC_OPEN_SEARCH_FAILED_PAGE_TIMEOUT", 1000);
  }
  ans = 0;
  sprintf(aux_string, "PAIR %s\r\n", device);
  char aux_string2[35];
  sprintf(aux_string2, "PAIR %s OK\r\n", device);
  while (ans == 2 || ans == 0) {
    ans = sendATcommand(aux_string, aux_string2 , error, 1000);
  }
  if (ans == 1) return 1;
  else return 0;
}

byte waitApp(char* device) {
  byte ans = 0;
  char aux_string[40];
  sprintf(aux_string, "RING 0 %s 1 RFCOMM", device);
  while (ans == 0) {
    ans = getResponse(aux_string, 1000);
  }
  if (ans == 1) return 1;
  else return 0;
}

int8_t getResponse(char* expected_answer, unsigned int timeout) {
  uint8_t x = 0,  answer = 0;
  char response[100];
  unsigned long previous;
  memset(response, '\0', 100);    // Initialize the string
  delay(100);
  // Clean the input buffer
  x = 0;
  previous = millis();
  do {
    if (Serial.available() != 0) {
      response[x] = Serial.read();
      x++;
      if (strstr(response, expected_answer) != NULL) {
        Serial.println(expected_answer);
        answer = 1;
        Serial.println("Expected response");
      }
    }
    // Waits for the asnwer with time out
  }
  while ((answer == 0) && ((millis() - previous) < timeout));
  return answer;
}

int checkMessage(){

  byte i=0;
  while(Serial.available()>0){
  
    msg[i]=Serial.read();
    i++;
    delay(10);
  }  
  i=0;  
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '1'){
        if (msg[i+3] == '#'){
          return -1;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '1'){
        if (msg[i+3] == '#'){
          return 1;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '2'){
        if (msg[i+3] == '#'){
          return -2;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '2'){
        if (msg[i+3] == '#'){
          return 2;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '3'){
        if (msg[i+3] == '#'){
         Serial.print("L3");
          return -3;
         
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '3'){
        if (msg[i+3] == '#'){
          return 3;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '0'){
      if (msg[i+2] == '#'){
        return 4;
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '9'){
      if (msg[i+2] == '0'){
        if (msg[i+3] == '#'){
          return 5;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '1'){
      if (msg[i+2] == '8'){
        if (msg[i+3] == '0'){
          if (msg[i+4] == '#'){
            return 6;
          }
        }
      }
    }
  }
  return 0;
}
        

Raspberry Pi:

Code:
/*  
 *  Bluetooth module PRO
 *  
 *  Copyright (C) Libelium Comunicaciones Distribuidas S.L. 
 *  http://www.libelium.com 
 *  
 *  This program is free software: you can redistribute it and/or modify 
 *  it under the terms of the GNU General Public License as published by 
 *  the Free Software Foundation, either version 3 of the License, or 
 *  (at your option) any later version. 
 *  
 *  This program is distributed in the hope that it will be useful, 
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
 *  GNU General Public License for more details.
 *  
 *  You should have received a copy of the GNU General Public License 
 *  along with this program.  If not, see http://www.gnu.org/licenses/. 
 *  
 *  Version:           1.0
 *  Design:            David GascĂłn 
 *  Implementation:    Jorge Casanova, Luis MartĂ­n
 */
 
#include "arduPi.h"

int SERVO = 3;
int grados;
unsigned int us;

char error[] = "SYNTAX ERROR";
char msg[100];
char dev;
char mac[] = "8c:3a:e3:ec:77:60";
uint32_t period;
boolean sending = false;
int turn = 0;
byte deg;

int8_t sendATcommand(const char* ATcommand, const char* expected_answer, const char* error_answer, unsigned int timeout);
byte pair(char* device);
byte waitApp(char* device);
int8_t getResponse(char* expected_answer, unsigned int timeout);
int checkMessage();
void Servo(int grados);

void setup() {	
	
  pinMode(SERVO, OUTPUT);
  Serial.begin(19200);
  delay(1000);
  printf("Configurando...\n");
  while (sendATcommand("AT", "OK", error, 1000) == 2);
  while (sendATcommand("SET BT AUTH * 0000", "", error, 1000) == 2);

  printf("Iniciando...\n");
  delay(1000);
  while (!pair(mac));
  delay(2000);
  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  waitApp(mac);
  sending = true;
  period = millis();
  deg = 15;
}

void loop() {

  if(Serial.available()>0){
    turn = checkMessage();
    switch(turn){
	  int tiempo;
      
      case 0:
        break;
      case -1:
        if(deg-1>0 && deg-1<180){
		  int tiempo = millis();
          while (millis()-tiempo < 2) {
            deg = deg+turn;
            Servo(deg);
		}
        }
        break;
      case 1:
        if(deg+1>0 && deg+1<180){
		  int tiempo = millis();
          while (millis()-tiempo < 2) {
            deg = deg+turn;
            Servo(deg); 
		}
        }
        break;
      case -2:
        if(deg-2>0 && deg-2<180){
		  int tiempo = millis();
          while (millis()-tiempo < 2) {
            deg = deg+turn;
            Servo(deg); 
		}
        }
        break;
      case 2:
        if(deg+2>0 && deg+2<180){
		  int tiempo = millis();
          while (millis()-tiempo < 2) {
            deg = deg+turn;
            Servo(deg); 
		}
        }
        break;
      case -3:
        if(deg-3>0 && deg-3<180){
		  int tiempo = millis();
          while (millis()-tiempo < 2) {
            deg = deg+turn;
            Servo(deg);
		}
        }
        break;
      case 3:
        if(deg+3>0 && deg+3<180){
		  int tiempo = millis();
          while (millis()-tiempo < 2) {
            deg = deg+turn;
            Servo(deg);
		}
        }
        break;
       case 4:
        tiempo = millis();
        while (millis()-tiempo < 1000) {
          Servo(1); 
          deg = 1;
	  }
        break;
      case 5:
        tiempo = millis();
        while (millis()-tiempo < 1000) {
          Servo(90); 
          deg = 90;
	  }
        break;
      case 6:
        tiempo = millis();
        while (millis()-tiempo < 1000) {
          Servo(179); 
          deg = 179;
	  }
        break;
    }
    while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  }
//    checkDisconnect();
}

int8_t sendATcommand(const char* ATcommand, const char* expected_answer, const char* error_answer, unsigned int timeout) {
  uint8_t x = 0,  answer = 0;
  char response[100];
  unsigned long previous;
  memset(response, '\0', 100);    // Initialize the string
  delay(100);
  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  if (ATcommand != NULL)  Serial.println(ATcommand);  // Send the AT command
  x = 0;
  previous = millis();
  do {
    if (Serial.available() != 0) {
      response[x] = Serial.read();
      x++;
      if (strstr(response, expected_answer) != NULL) {
        Serial.println(expected_answer);
        while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
        answer = 1;
        printf("Expected response\n");
      } else if (strstr(response, error_answer) != NULL) {
        answer = 2;
        while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
      }
    }
    // Waits for the asnwer with time out
  }
  while ((answer == 0) && ((millis() - previous) < timeout));
  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  return answer;
}

byte pair(char* device) {
  // call to you phone (bluetooth MAC)
  char aux_string[35];
  sprintf(aux_string, "CALL %s 1101 RFCOMM\r\n", device);
  while (sendATcommand(aux_string, "CALL 0", error, 1000) == 2);
  //while (sendATcommand("CALL 8c:3a:e3:ec:77:60 1101 RFCOMM", "CALL 0", error, 1000) == 2);
  //  delay(500);
  while ( Serial.available() > 0) Serial.read();   // Clean the input buffer
  byte ans = 0;
  while (ans == 0) {
    ans = getResponse("NO CARRIER 0 ERROR 305 SDC_OPEN_SEARCH_FAILED_PAGE_TIMEOUT", 1000);
  }
  ans = 0;
  sprintf(aux_string, "PAIR %s\r\n", device);
  char aux_string2[35];
  sprintf(aux_string2, "PAIR %s OK\r\n", device);
  while (ans == 2 || ans == 0) {
    ans = sendATcommand(aux_string, aux_string2 , error, 1000);
  }
  if (ans == 1) return 1;
  else return 0;
}

byte waitApp(char* device) {
  byte ans = 0;
  char aux_string[40];
  sprintf(aux_string, "RING 0 %s 1 RFCOMM", device);
  while (ans == 0) {
    ans = getResponse(aux_string, 1000);
  }
  if (ans == 1) return 1;
  else return 0;
}

int8_t getResponse(char* expected_answer, unsigned int timeout) {
  uint8_t x = 0,  answer = 0;
  char response[100];
  unsigned long previous;
  memset(response, '\0', 100);    // Initialize the string
  delay(100);
  // Clean the input buffer
  x = 0;
  previous = millis();
  do {
    if (Serial.available() != 0) {
      response[x] = Serial.read();
      x++;
      if (strstr(response, expected_answer) != NULL) {
        Serial.println(expected_answer);
        answer = 1;
        printf("Expected response\n");
      }
    }
    // Waits for the asnwer with time out
  }
  while ((answer == 0) && ((millis() - previous) < timeout));
  return answer;
}

int checkMessage(){

  byte i=0;
  while(Serial.available()>0){
    msg[i]=Serial.read();
    i++;
    delay(10);
  }  
  i=0;  
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '1'){
        if (msg[i+3] == '#'){
          return -1;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '1'){
        if (msg[i+3] == '#'){
          return 1;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '2'){
        if (msg[i+3] == '#'){
          return -2;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '2'){
        if (msg[i+3] == '#'){
          return 2;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '3'){
        if (msg[i+3] == '#'){
          return -3;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '3'){
        if (msg[i+3] == '#'){
          return 3;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '0'){
      if (msg[i+2] == '#'){
        return 4;
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '9'){
      if (msg[i+2] == '0'){
        if (msg[i+3] == '#'){
          return 5;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '1'){
      if (msg[i+2] == '8'){
        if (msg[i+3] == '0'){
          if (msg[i+4] == '#'){
            return 6;
          }
        }
      }
    }
  }
  return 0;
}

void Servo(int grados){

    us = (grados*11) + 500;      // Convert angle to microseconds
    digitalWrite(SERVO, HIGH);
    delayMicroseconds(us);
    digitalWrite(SERVO, LOW);
    delay(50);                   // Refresh cycle of servo

}


int main (){
	setup();
	while(1){
		loop();
	}
	return (0);
}

        

Intel Galileo:

Code:
/*  
 *  Bluetooth module PRO
 *  
 *  Copyright (C) Libelium Comunicaciones Distribuidas S.L. 
 *  http://www.libelium.com 
 *  
 *  This program is free software: you can redistribute it and/or modify 
 *  it under the terms of the GNU General Public License as published by 
 *  the Free Software Foundation, either version 3 of the License, or 
 *  (at your option) any later version. 
 *  
 *  This program is distributed in the hope that it will be useful, 
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
 *  GNU General Public License for more details.
 *  
 *  You should have received a copy of the GNU General Public License 
 *  along with this program.  If not, see http://www.gnu.org/licenses/. 
 *  
 *  Version:           1.0
 *  Design:            David GascĂłn 
 *  Implementation:    Jorge Casanova, Luis MartĂ­n
 */
 
#include <Servo.h>

char error[] = "SYNTAX ERROR";
char msg[100];
char tempmess[40];
char dev;
char mac[] = "**:**:**:**:**:**";
char temper[6];
uint32_t period;
float tdegrees;
boolean sending = false;
int turn = 0;
byte deg;
Servo myServo;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial1.begin(115200);
  delay(1000);
  Serial.print("Setting up...");
  while (sendATcommand("AT", "OK", error, 1000) == 2);
  while (sendATcommand("SET BT AUTH * 0000", "", error, 1000) == 2);

  Serial.print("Starting...");
  delay(1000);
  while (!pair(mac));
  delay(2000);
  while ( Serial1.available() > 0) Serial1.read();   // Clean the input buffer
  waitApp(mac);
  delay(1000);
  while ( Serial1.available() > 0) Serial1.read(); 
  sending = true;
  period = millis();
  myServo.attach(9);
  deg = 15;
  myServo.write(deg);
}

void loop() {
  // put your main code here, to run repeatedly:

  if(Serial1.available()>0){
    turn = checkMessage();
    switch(turn){
      
      case 0:
        break;
      case -1:
        if(deg-1>0 && deg-1<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case 1:
        if(deg+1>0 && deg+1<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case -2:
        if(deg-2>0 && deg-2<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case 2:
        if(deg+2>0 && deg+2<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case -3:
        if(deg-3>0 && deg-3<180){
                  deg = deg+turn;
          myServo.write(deg); 
        }
        break;
      case 3:
        if(deg+3>0 && deg+3<180){
          deg = deg+turn;
          myServo.write(deg); 
        }
        break;
       case 4:
        myServo.write(1); 
        deg = 1;
        break;
      case 5:
        myServo.write(90); 
        deg = 90;
        break;
      case 6:
        myServo.write(179); 
        deg = 179;
        break;
    }
    while ( Serial1.available() > 0) Serial1.read();   // Clean the input buffer
  }
//    checkDisconnect();
}

int8_t sendATcommand(char* ATcommand, char* expected_answer, char* error_answer, unsigned int timeout) {
  uint8_t x = 0,  answer = 0;
  char response[100];
  unsigned long previous;
  memset(response, '\0', 100);    // Initialize the string
  delay(100);
  while ( Serial1.available() > 0) Serial1.read();   // Clean the input buffer
  if (ATcommand != NULL)  Serial1.println(ATcommand);  // Send the AT command
  x = 0;
  previous = millis();
  do {
    if (Serial1.available() != 0) {
      response[x] = Serial1.read();
      x++;
      if (strstr(response, expected_answer) != NULL) {
        //Serial1.println(expected_answer);
        while ( Serial1.available() > 0) Serial1.read();   // Clean the input buffer
        answer = 1;
        Serial.println("Expected response");
      } else if (strstr(response, error_answer) != NULL) {
        answer = 2;
        while ( Serial1.available() > 0) Serial1.read();   // Clean the input buffer
      }
    }
    // Waits for the asnwer with time out
  }
  while ((answer == 0) && ((millis() - previous) < timeout));
  while ( Serial1.available() > 0) Serial1.read();   // Clean the input buffer
  return answer;
}

byte pair(char* device) {
  // call to you phone (bluetooth MAC)
  char aux_string[35];
  sprintf(aux_string, "CALL %s 1101 RFCOMM\r\n", device);
  while (sendATcommand(aux_string, "CALL 0", error, 1000) == 2);
  
  //while (sendATcommand("CALL 8c:3a:e3:ec:77:60 1101 RFCOMM", "CALL 0", error, 1000) == 2);
  //  delay(500);
  while ( Serial1.available() > 0) Serial1.read();   // Clean the input buffer
  byte ans = 0;
  while (ans == 0) {
    ans = getResponse("NO CARRIER", 1000);
  }
  ans = 0;
  sprintf(aux_string, "PAIR %s\r\n", device);
  char aux_string2[35];
  sprintf(aux_string2, "PAIR %s OK\r\n", device);
  while (ans == 2 || ans == 0) {
    ans = sendATcommand(aux_string, aux_string2 , error, 1000);
  }
  if (ans == 1) return 1;
  else return 0;
}

byte waitApp(char* device) {
  byte ans = 0;
  char aux_string[40];
  sprintf(aux_string, "RING 0 %s 1 RFCOMM", device);
  while (ans == 0) {
    ans = getResponse(aux_string, 1000);
  }
  if (ans == 1) return 1;
  else return 0;
}

int8_t getResponse(char* expected_answer, unsigned int timeout) {
  uint8_t x = 0,  answer = 0;
  char response[100];
  unsigned long previous;
  memset(response, '\0', 100);    // Initialize the string
  delay(100);
   // Clean the input buffer
  x = 0;
  previous = millis();
  do {
    if (Serial1.available() != 0) {
      response[x] = Serial1.read();
      x++;
      if (strstr(response, expected_answer) != NULL) {
        //Serial.println(expected_answer);
        answer = 1;
        Serial.println("Expected response");
      }
    }
    // Waits for the asnwer with time out
  }
  while ((answer == 0) && ((millis() - previous) < timeout));
  return answer;
}

int checkMessage(){

  byte i=0;
  while(Serial1.available()>0){
  
    msg[i]=Serial1.read();
    i++;
    delay(10);
  }  
  i=0;  
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '1'){
        if (msg[i+3] == '#'){
          return -1;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '1'){
        if (msg[i+3] == '#'){
          return 1;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '2'){
        if (msg[i+3] == '#'){
          return -2;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '2'){
        if (msg[i+3] == '#'){
          return 2;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'l'){
      if (msg[i+2] == '3'){
        if (msg[i+3] == '#'){
          return -3;
         
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == 'r'){
      if (msg[i+2] == '3'){
        if (msg[i+3] == '#'){
          return 3;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '0'){
      if (msg[i+2] == '#'){
        return 4;
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '9'){
      if (msg[i+2] == '0'){
        if (msg[i+3] == '#'){
          return 5;
        }
      }
    }
  }
  if (msg[i] == '#'){
    if (msg[i+1] == '1'){
      if (msg[i+2] == '8'){
        if (msg[i+3] == '0'){
          if (msg[i+4] == '#'){
            return 6;
          }
        }
      }
    }
  }
  return 0;
}
        

Links and Documentation

Related Tutorials

Datasheets and Manuals