Arduino & Robot IR Codes (Basic Movement Execution)

Overview

This blog post details how the 1st steps of getting the robot to copy the user progresses.


Activity

The activity now the connection to the robot is stable and instruction can be sent and acted upon, it is time to send specific commands depending an the users movement.
All IR code available have been sourced from http://playground.arduino.cc/Main/RoboSapienServer, these are readily available and free to use. As the arudino can only recive one byte at a time sending "LeftArmUp" would be difficult to act upon because each byte sent across would have to be converted in the a string from the character byte then acted upon. Instead of this a series of numbers will be used 01-27. These restrict the amount of data sent across, reduced the chance of losing bytes and reducing latency. Each command will be given a unique 2 digit code that will be sent from the Kinect program to the Arduino. These can be seen below

//|     Action   | IR code |Send Code 
// RSTurnRight       0x80   01
// RSRightArmUp      0x81   02
// RSRightArmOut     0x82   03
// RSTiltBodyRight   0x83   04
// RSRightArmDown    0x84   05
// RSRightArmIn      0x85   06
// RSRightHandSweep  0xC1   07
// RSRightHandStrike2 0xC3  08
// RSWalkForward     0x86   09
// RSWalkBackward    0x87   10
// RSTurnLeft        0x88   11
// RSLeftArmUp       0x89   12
// RSLeftArmOut      0x8A   13
// RSTiltBodyLeft    0x8B   14
// RSLeftArmDown     0x8C   15
// RSLeftArmIn       0x8D   16
// RSStop            0x8E   17
// RSWakeUp          0xB1   18
// RSBurp            0xC2   19
// RSRightHandStrike 0xC0   20
// RSNoOp            0xEF   21
// RSHigh5           0xC4   22
// RSFart            0xC7   23
// RSLeftHandStrike  0xC8   24
// RSLeftHandSweep   0xC9   25
// RSWhistle         0xCA   26
// RSRoar            0xCE   27

Once a particular "send code" is sent the arduino will receive it. To determine which code to send to the robot the arduino will analyse the incoming bytes. The process has been split in to two stages the 1st, analysis the 1st number (0-2) and identify's this as the ID and sets a varible to tell the program that the 1st stage is complete. Then by checking the stage number and the ID it will then determine which IR code to send the robot based upon the next character byte (between 0-9). The code below shows how the stage is identified and set then uses "use cases"to act upon the 2nd number.

//thisChar is the incoming byte from the input buffer (kinect program)
if(stage == 0 )
        {//Get 1st number
           id=thisChar;
           stage = 1;
        }
else if(stage == 1 && id == '0')
        {//00-09
          Serial.print("!Stage1 ID0!");
          Serial.print(thisChar);
           switch (thisChar) 
           {
             Serial.print("in switch");
            case '0':           
              RSSendCommand(RSRoar);
              Serial.print("first");
              stage=0;
              break;
              Serial.print("");
            case '1':
              RSSendCommand(RSTurnRight);    
              Serial.print("second");
              stage=0;
              break;
            case '2':
              RSSendCommand(RSRightArmUp);    
              Serial.print("second");
              stage=0;
              break;

This will allow the arduino to send the IR codes to the robot depending on the numerical number sent from the Kinect Program.

Now that the arduino can send the IR codes based onthe "send codes". The Kinect program must be able to detect what actions the user is performing and act upon them.
There are around 20 movement actions that need to be detected. To detect these, algorithms will be created to detect the X,Y, and Z position of joints and if they are in a particular alignment then send a command the the robot. An example of this it the turn right command seen below. If the left   shoulders Z position is further forward then the right shoulder then send the "01" command to the Arduino. However because the Kinect is so sensitive a buffer range was added of 0.13M. This meant that the user could move a little bit without the robot turning right becuase keeping a user shoulder exactly aligned is almost imposible when standing.

if ((skeleton.Joints[JointType.ShoulderLeft].Position.Z + 0.13) > skeleton.Joints[JointType.ShoulderRight].Position.Z)
       {
           String command = "01";
           
           commandLog.Text = command + "\n" + commandLog.Text;
           if (connected == true)
              {
                     Thread thread = new Thread(send);
                     thread.Start(command);
               }
        }

To stop the robot turning the user would simply return to a position where the shoulders are almost aligned (not triggering the if statement) by using a stop command for the robot. The code below is a revised version of the above code. It includes the stop function and uses a variable that has the code to send to the robot rather then just numbers (i.e RSTurnRight = 01). It also uses the activeCommand array by converting the command code to an integer and using it as the array ID to set it to true or false.

if ((skeleton.Joints[JointType.ShoulderLeft].Position.Z + 0.13) > skeleton.Joints[JointType.ShoulderRight].Position.Z)
  {   //Run action

      int commandInt = Convert.ToInt32(RSTurnRight);
      commandLog.Text = RSTurnRight + "\n" + commandLog.Text;
      if (connected == true)
      {
          Thread thread = new Thread(send);
          thread.Start(RSTurnRight);
          activeCommand[commandInt] = true;
      }
  }
  else
  {   //Stop
      
      commandLog.Text = RSStop + "\n" + commandLog.Text;
      if (connected == true)
      {
          Thread thread = new Thread(send);
          thread.Start(RSStop);
          int commandInt = Convert.ToInt32(RSTurnRight);
          activeCommand[commandInt] = false;
      }
  }

It has become clear that more understanding of how I want the move to be executed is need and will now look in to how the arms move.