6
\$\begingroup\$

Background

A prototype of a firefighting robot has been done recently based on an Arduino microcontroller. It has two servos, one laser LED and 2WD motors. The desktop application is done in C# to control the robot.

Desktop app logic

For controlling two servos, I am using the x-y mouse position on the screen and bind keys from keyboard for controlling the motors. Finally, values of servos, led and motors are combined into a single string like "x140y090s0we" - "x140" - servo1 140degree, "y090" - servo2 90 degree, "s0" - laser is off, "w" - for motors, "e" - end of command.

On the microcontroller side

A loop gathers each character one by one into an array. Then it recognizes appropriate values from [x,1,4,0,y,0,9,0,s,0,w,e] to manipulate some parts of the robot.

Now the problem

The robot works perfect on a 9600 baud rate, when it is connected through a USB cable port. However, when trying to manipulate it using a BLUETOOTH port, lags start to appear. I solved the problem by increasing the baud rate of the HC-06 BLUETOOTH module to 115200, so everything works fine. However, power supply consumption increased as well.

Question

Honestly saying, I don't have much experience in programming. I tried my best. How would you suggest to improve the code so that it shows best performance in 9600 baud rate using bluetooth module?

C# Code:

public partial class Form1 : Form
    {
        SerialPort port;
        public Form1()
        {
            InitializeComponent();
            init();
            FormBorderStyle = FormBorderStyle.None;
            WindowState = FormWindowState.Maximized;
            TopMost = true;

        }
        private void init()
        {
            port = new SerialPort();
            port.PortName = "COM5";
            port.BaudRate = 115200;

            try
            {
                port.Open();
            }
            catch (Exception e1)
            {

                MessageBox.Show(e1.Message);
            }
        }
        int x = 0, y = 0;

        private void Form1_PreviewKeyDown(object sender, PreviewKeyDownEventArgs e)
        {
            if (e.KeyCode == Keys.Escape)
            {
                this.Close();
            }
        }
        string laser = "0";

        private void Form1_MouseDown(object sender, MouseEventArgs e)
        {
            laser = "1";
        }

        private void Form1_MouseUp(object sender, MouseEventArgs e)
        {
            laser = "0";
        }

        private void Form1_KeyDown(object sender, KeyEventArgs e)
        {
            if (e.KeyCode == Keys.W)
                input = "r";
            if (e.KeyCode == Keys.S)
                input = "l";
            if (e.KeyCode == Keys.A)
                input = "b";
            if (e.KeyCode == Keys.D)
                input = "f";
            Display(input);
        }

        private void Form1_KeyUp(object sender, KeyEventArgs e)
        {
            input = "s";
            label4.Text = "";
        }


        string input = "s";

        public void Display(string inp)
        {
            if (inp == "f")
                label4.Text = "Right";
            if (inp == "b")
                label4.Text = "Left";
            if (inp == "l")
                label4.Text = "Back";
            if (inp == "r")
                label4.Text = "Forward";
        }

        string servo = "";
        protected override void OnMouseMove(MouseEventArgs e)
        {
            base.OnMouseMove(e);
            Graphics g = CreateGraphics();
            Pen p = new Pen(Color.Navy);
            Pen erase = new Pen(Color.White);
            x = e.X; y = e.Y;
            double curWidth = this.Width / 180.0 - 0.1; // getting precise coeff
            double x2 = (int)(x / curWidth); //divide form width into equal degree

            double curHeight = this.Height / 180.0 - 0.22223;
            double y2 = (int)(y / curHeight);

            label1.Text = x2.ToString(); label2.Text = y2.ToString();

            string valx = x2.ToString();

            string valy = y2.ToString();


            if(valx.Length != 3)
            {
                for (int i = 0; valx.Length <= 2; i++ )
                {
                    valx = "0" + valx;
                }
            }
            if(valy.Length != 3)
            {
                for (int i = 0; valy.Length <= 2; i++)
                {
                    valy = "0" + valy;
                }
            }
            servo = "x" + valx + "y" + valy + "s" + laser;
            label3.Text = (servo + input + "e").ToString();
            string val = servo + input + "e";  // final string to send
            if (port.IsOpen)
            {
                port.Write(val.ToString());
            }
        }
    }

Arduino code:

#define bluetooth Serial
#include <Servo.h>
// motor one
int enA = 9;
int in1 = 8;
int in2 = 7;
// motor two
int enB = 4;
int in3 = 6;
int in4 = 5;
Servo x, y;

char cmd[100]; //changed value to allow x to work from 10 to 11
int cmdIndex;

void exeCmd() {
  if(cmd[0]=='x') {

       int valx = 0;
       for(int i=1; i<4; i++) {
         valx = valx*10 + (cmd[i]-'0');
       }
       x.write(valx);  
     }   
  if(cmd[4]=='y') {

       int valy = 0;
       for(int i=5; i<8; i++) {
         valy = valy*10 + (cmd[i]-'0');
       }          
       y.write(valy);      
     }  
  if(cmd[8]=='s') {
       if(cmd[9]=='1')
       digitalWrite(10, HIGH);
       else
       digitalWrite(10, LOW);
     }

  if(cmd[10] == 'f')
    {
      Forward();
    }
    if(cmd[10] == 'b')
    {
      Back();
    }
    if(cmd[10] == 's')
    {
      Stop();
    }
    if(cmd[10] == 'r')
    {
      Right();
    }
    if(cmd[10] == 'l')
    {
      Left();
    }
}

void Forward()
{
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}
void Back()
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);  
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);   
}
void Stop()
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);  
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);  
}
void Right()
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);  
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}
void Left()
{
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);  
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);   
}

void setup() {

  delay(1500); // wait for bluetooth module to start

  bluetooth.begin(115200); // Bluetooth default baud is 9600

  pinMode(10, OUTPUT);
  y.attach(12);
  x.attach(11);
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  analogWrite(enA, 100);
  analogWrite(enB, 100);
  cmdIndex = 0;
}


void loop() {

  if(bluetooth.available()) {

    char c = (char)bluetooth.read();

    if(c=='e') {
      cmd[cmdIndex] = 0;
      exeCmd();  // execute the command
      cmdIndex = 0; // reset the cmdIndex
    } else {      
      cmd[cmdIndex] = c;
      if(cmdIndex<99) cmdIndex++;
    }


  }

}
\$\endgroup\$
4
\$\begingroup\$

Power Issue

The increase in current when you increase the baud rate is expected behavior. See this question on the electrical engineering stack exchange for more details. Also see these guidelines on wireless considerations

Arduino Code

Some considerations:

  • if(cmd[10] == 'f'), if(cmd[10] == 'b'), etc. Only one of these is ever going to be active at a time, so replace it with a switch(cmd[10]) instead. The compiler can usually optimize switch statements with a jump table, which is faster than doing multiple sequential branches.

  • valx = valx*10 + (cmd[i]-'0'), valy = valy*10 + (cmd[i]-'0'). You are manually converting string data to a number here. Consider using atoi() if your compiler supports it, or even better, transmit the numbers in binary and just read the bytes into your variable without a loop using a union or casting the array index address into an integer pointer.


Example

Documentation:

C# Code:

// This will transmit mixed ascii and binary data 
byte[] xBytes = BitConverter.GetBytes(1138);
port.Write("x");
port.Write(xBytes, 0, 4);
port.Write("y");
//...

Arduino Code:

int xValue;
if(cmd[0]=='x')
{
    xValue = *(&cmd[1]); // Note that this might have endian issues (bytes out of order)
}

If you have endian issues, you can have the PC swap the order of bytes:

//this order is probably not correct, but you get the idea...
port.Write(xBytes[1], 0, 1);
port.Write(xBytes[0], 0, 1);
port.Write(xBytes[3], 0, 1);
port.Write(xBytes[2], 0, 1);

There are more sophisticated ways to handle the bytes and endianess, but I've deliberately kept it simple here.

\$\endgroup\$
  • \$\begingroup\$ Thank you for your response! Could you give me more details or material to read on "transmitting the numbers in binary and reading the bytes into varialbe without a loop using union or casting the array index address into an integer point"? I highly feel the solution is somewhere there. Thank you. \$\endgroup\$ – Olimjon Mar 8 '16 at 20:21
5
\$\begingroup\$
private void Form1_KeyDown(object sender, KeyEventArgs e)
{
    if (e.KeyCode == Keys.W)
        input = "r";
    if (e.KeyCode == Keys.S)
        input = "l";
    if (e.KeyCode == Keys.A)
        input = "b";
    if (e.KeyCode == Keys.D)
        input = "f";
    Display(input);
}

...

public void Display(string inp)
{
    if (inp == "f")
        label4.Text = "Right";
    if (inp == "b")
        label4.Text = "Left";
    if (inp == "l")
        label4.Text = "Back";
    if (inp == "r")
        label4.Text = "Forward";
}

Each of these methods will continue to evaluate the next if statement(s) even if one returns true; e.g. in Form1_KeyDown, if the key is in fact Keys.W, input will be set to "r". However it then goes on to check whether the Key is equal to S, A, and D, which is unnecessary and inefficient. Same for the other method. Use else if, or a switch statement.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.