[solved]Controlling Servos Using Serial

Hi,

I’m trying to control servo motors using bluetooth(serial), but I have come across an issue that has had me baffled for hours.

I am using an app called Bluetooth electronics by keuwl, and long story short, the arduino receives a sequence of bytes of the following format on the serial port: @BX111,Y111b! ,Where @ is the start byte, B is a method of telling that this packet contains the data regarding the servos, and the numbers represent the angle of the X and Y servos. The ! is the end byte. I will explain why the b is there in a moment.

On plain sight this is very easy to implement, since we know that if we save the packet to a byte array bytes, bytes[3] to bytes[5] will be the numbers for the angle of the X servo, and bytes[8] to bytes[10] will be the numbers for the Y servo. And then all we need to do is subtract ‘0’ from the following bytes(to convert them from ASCII to decimal, since ‘0’ in ASCII is 48 in decimal) and multiply them by a power of 10 before finally summing them to convert the value into decimal format.
for example, for an input @BX111,Y111b! the angle for the X servo in decimal would be:

X=(bytes[3]-'0')*100+(bytes[4]-'0')*10+(bytes[5]-'0');

However, the problem is that placeholders aren’t used, as in no preceding 0’s are sent. for example the app sends @BX11,Y111b! and not @BX011,Y111b!

Here’s the code that I have come up with so far, it works for some inputs but not others. For instance, if the transmitted X value has 3 digits then the received value becomes 0. I left a comment in the code highlighting where I think the problem is. My current approach was inspired by the following post(look at the code for the state machine):

http://www.gammon.com.au/forum/?id=11425

The packet receive protocol was taken from the following post:

Coming back to why b is in the sequence, its purpose is to tell the code that there are no more number bits since I am using the isdigit() function. (can’t use the end byte as it is removed from the sequence after decoding).

#include <Servo.h>

#define motorA1  3 // Pin  2 of L293
#define motorA2  5  // Pin  7 of L293
#define motorB1  6 // Pin 10 of L293
#define motorB2  9  // Pin 14 of L293

#define startMarker '@'
#define endMarker '!'
#define specialByte 253
#define maxMessage 16
#define baseservopin 4
#define shoulderservopin 7
Servo baseservo,shoulderservo;

int BTin=0;
int maxspeed=0;
byte data[maxMessage];
byte tempBuffer[maxMessage];
byte bytesRecvd = 0;
byte dataRecvCount = 0;
boolean inProgress = false;
boolean allReceived = false;

void setup() {
  pinMode(motorA1, OUTPUT);
  pinMode(motorA2, OUTPUT);
  pinMode(motorB1, OUTPUT);
  pinMode(motorB2, OUTPUT);
  Serial.begin(9600);
  baseservo.attach(baseservopin);
  shoulderservo.attach(shoulderservopin);
}

void loop() {
/*  if(Serial.available()>0){
    BTin=Serial.read();
    getSerialData();
  switch(BTin){
    case'L':
      analogWrite(motorA1, maxspeed);
      digitalWrite(motorA2, 0);
      break;
    case'M':
      analogWrite(motorA2, maxspeed);
      digitalWrite(motorA1, 0);
      break; 
    case'R':
      analogWrite(motorB1, maxspeed);
      digitalWrite(motorB2, 0);
      break;
    case'N':
      analogWrite(motorB2, maxspeed);
      digitalWrite(motorB1, 0);
      break;
    case's':
      digitalWrite(motorA2,0);
      digitalWrite(motorA1, 0);
      break; 
    case't':
      digitalWrite(motorB2,0);
      digitalWrite(motorB1, 0);
      break;
    case'S':
      maxspeed=Serial.parseInt();
      if(Serial.read()=='S'){break;}         
  }
  
}*/
   getSerialData();
   processData();
}

void processData() {
    // processes the data that is in data[]
  byte n,m;
  byte anglex=0,angley=0;
  if (allReceived) {
    if(data[0]=='B'){
      //This is the most crucial part of the code(well I think)
      for(n=2;n<5;n++){
        if(isdigit(data[n])){
          anglex *=10;
          anglex +=data[n]-'0';
        }
        else{
          n +=2;               
          break;
        }
      }
      //Serial.println(n);
      for(m=n;m<(n+3);m++){
        if(isdigit(data[m])){
          angley *=10;
          angley +=data[m]-'0';
        }
        else{               
          break;
        }
      }     
      //Serial.println(anglex);
     // Serial.println(angley);
      baseservo.write(anglex);
      shoulderservo.write(angley);
      allReceived = false;
    } 
  }
}

void getSerialData() {

     // Receives data into tempBuffer[]
     //   saves the number of bytes that the PC said it sent - which will be in tempBuffer[1]
     //   uses decodeHighBytes() to copy data from tempBuffer to data[]
     
     // the Arduino program will use the data it finds in data[]

  if(Serial.available() > 0) {

    byte x =Serial.read();
    if (x == startMarker) { 
      byte x = startMarker;
      bytesRecvd = 0; 
      inProgress = true;
     //  blinkLED(2);
      // debugToPC("start received");
    }
      
    if(inProgress) {
      tempBuffer[bytesRecvd] = x;
      bytesRecvd ++;
    }

    if (x == endMarker) {
      inProgress = false;
      allReceived = true;
      
        // save the number of bytes that were sent
//      dataSentNum = tempBuffer[1];
  
      decodeHighBytes();
    }
  }
}

void decodeHighBytes() {

  //  copies to data[] only the data bytes i.e. excluding the marker bytes and the count byte
  //  and converts any bytes of 253 etc into the intended numbers
  //  Note that bytesRecvd is the total of all the bytes including the markers
  dataRecvCount = 0;
  for (byte n = 1; n < bytesRecvd - 1 ; n++) { // 2 skips the start marker and the count byte, -1 omits the end marker
    byte x = tempBuffer[n];
    if (x == specialByte) {
       // debugToPC("FoundSpecialByte");
       n++;
       x = x + tempBuffer[n];
    }
    data[dataRecvCount] = x;
    dataRecvCount ++;
  }
}
void blinkLED(byte numBlinks) {
    for (byte n = 0; n < numBlinks; n ++) {
      digitalWrite(13, HIGH);
      delay(200);
      digitalWrite(13, LOW);
      delay(200);
    }
}

Thank you for bearing with me, and I greatly appreciate you spending your time and effort to help .

However, the problem is that placeholders aren’t used, as in no preceding 0’s are sent. for example the app sends @BX11,Y111b! and not @BX011,Y111b!

That is NOT a problem. The data of interest is between the X and the comma and between the Y and the b. Find the X, the comma, the Y, and the b, and you’ll know where the data of interest is.

Or, make the app send the data the way you want.

void loop() {
/*  if(Serial.available()>0){
    BTin=Serial.read();
    getSerialData();
  switch(BTin){
    case'L':
      analogWrite(motorA1, maxspeed);
      digitalWrite(motorA2, 0);
      break;
    case'M':
      analogWrite(motorA2, maxspeed);
      digitalWrite(motorA1, 0);
      break;
    case'R':
      analogWrite(motorB1, maxspeed);
      digitalWrite(motorB2, 0);
      break;
    case'N':
      analogWrite(motorB2, maxspeed);
      digitalWrite(motorB1, 0);
      break;
    case's':
      digitalWrite(motorA2,0);
      digitalWrite(motorA1, 0);
      break;
    case't':
      digitalWrite(motorB2,0);
      digitalWrite(motorB1, 0);
      break;
    case'S':
      maxspeed=Serial.parseInt();
      if(Serial.read()=='S'){break;}         
  }
 
}*/

I thought about putting a line that said “Do not bother reading this”, several dozen lines of irrelevant rambling, and a line that said “Resume reading here”. But, I got over it. I see that you didn’t.

      for(n=2;n<5;n++){

You should NOT assume that 2 and 5 are the correct values.

    byte x =Serial.read();
    if (x == startMarker) {
      byte x = startMarker;
      bytesRecvd = 0;
      inProgress = true;
     //  blinkLED(2);
      // debugToPC("start received");
    }

If the value read from the serial port is the start marker, create another variable with the same name, and assign it a value. Then, never use that variable. I’ll bite. Why are you doing that?

    if(inProgress) {
      tempBuffer[bytesRecvd] = x;
      bytesRecvd ++;
    }

Store the character in the array containing the useful data.

    if (x == endMarker) {

Then, see if that was a good idea. Hmmm…

Thank you for replying.

That is NOT a problem. The data of interest is between the X and the comma and between the Y and the b. Find the X, the comma, the Y, and the b, and you'll know where the data of interest is.

I thought about that, but I don't know how to implement it. I read about atoi but I was confused since I have 2 integers in the string. Do you have any recommendations or know of any posts where other people were trying to do the same thing?

I can't change the app, its one I found on google play.

I thought about putting a line that said "Do not bother reading this", several dozen lines of irrelevant rambling, and a line that said "Resume reading here". But, I got over it. I see that you didn't.

Yeah my bad, I'm still new to the forums I'll keep that in mind.

You should NOT assume that 2 and 5 are the correct values.

I used 2 and 5 since the 3rd bit always has to be a digit, and I just guessed that since I break out of the for loop when there a byte isn't a digit. I couldn't think of any other way to do it

If the value read from the serial port is the start marker, create another variable with the same name, and assign it a value. Then, never use that variable. I'll bite. Why are you doing that?

.

The code was nicked from the post I mentioned (I read through it and made sure I understood it though), I assumed it was best to not modify it but yeah it makes no sense thanks for pointing that out.

Then, see if that was a good idea. Hmmm...

I don't understand what you meant here. Are you trying to say that I shouldn't store the marker bytes from the beginning? What the code does is that it stores everything it gets in a buffer then decodes it and puts it in a data array.

A quick side question, is it better to use a buffer or a state machine (like in the first link I posted) to process the serial input? Or more broadly, how would you recommend processing serial data?

I thought about that, but I don't know how to implement it.

strchr() will find the first occurrence of a character in a string. 'X' is a character. tempBuffer is not a string, but could be if you kept it NULL terminated.

The difference between the pointer to the character and the start of the string is the position.

http://www.cplusplus.com/reference/cstring/strchr/

I don't understand what you meant here. Are you trying to say that I shouldn't store the marker bytes from the beginning? What the code does is that it stores everything it gets in a buffer then decodes it and puts it in a data array.

You should look at Robin2's serial input basics thread. You do not need to store the start or end markers in the array. The array should only contain the useful data.

A quick side question, is it better to use a buffer or a state machine (like in the first link I posted) to process the serial input? Or more broadly, how would you recommend processing serial data?

The "best" approach is the one you understand. I intuitively understand saving the data until I have a full packet, and then parsing the packet. If you intuitively understand the state machine concept, then that is the best approach FOR YOU.

Since you are not getting or storing a huge amount of information, and (probably) want to discard any data that is in a malformed packet, saving the data until the whole packet arrives, and then parsing it is probably a better approach.

Have a look at the parse example in Serial Input Basics. It probably already does most of what you require. Just change the start and end markers to match what you are using.

...R

Oh you are my savior, that post is exactly what I needed, I wish I had found it earlier.

Here's what I came up with, I modified the parseData() function to support the the input format I'm using.

void parseData() {      // split the data into its parts

    char * strtokIndx; // this is used by strtok() as an index

    strtokIndx = strtok(tempChars,'X');      // get the first part - the string
    strcpy(instruction, strtokIndx);    //copies the byte after the start marker, which tells us what the 

   data in the packet is for
 
    strtokIndx = strtok(strtokIndx, ","); // this continues where the previous call left off
    anglex = atoi(strtokIndx);     // convert this part to an integer

    strtokIndx = strtok(NULL, "Y");
    
    angley = atoi(strtokIndx);     // convert this part to a float

}

Thank you PaulS and Robin2 and I hope you have a great day.