Controlling Braccio with keyboard through serial communication with Processing

Hi,
I have been trying to make this work for a week now but I think I went a bit over my head for being my first project outside the Arduino Starter Kit. I would appreciate some more eyes over my code and if possible tell me where am I going wrong.

The goal:
To control my Braccio with the keyboard of my laptop using serial communication between the code of my Arduino Uno and the code in Processing. The servo motors on the Braccio should be able to move at the same time. This means that the Processing should be able to send multiple variables and the Arduino should be able to read multiple variables.

My Processing Code:

/* BraccioKeyboardController v3.1
*/

//Import serial library and declare serial name
import processing.serial.*;
Serial myPort;

//Variables:
int keys=12;
                  //{0    ,1    ,2    ,3    ,4    ,5    ,6    ,7    ,8    ,9    ,10   ,11   }
                  //{RIGHT,LEFT ,UP   ,DOWN ,D    ,A    ,W    ,S    ,E    ,Q    ,CTRL ,SHIFT}
int keyStatus[]={0,0,0,0,0,0,0,0,0,0,0,0};

void setup(){
  size(100,100);
  
  //Setup the serial port
  String portName = Serial.list()[0];
  myPort = new Serial(this, portName, 9600);
}

void draw(){
  background(0,0,0);
  //Write the values of the keyStatus on the serial port and print the values so I can see it.
  for(int i=0; i<=keys-1; i++){
    myPort.write(keyStatus[i]);
    print(keyStatus[i]);
    if(i<keys-1){
      myPort.write(',');
      print(',');
    }
    if(i==keys-1){
      myPort.write('\n');
      println();
    }
  }
}

void keyPressed() {
  //Read the pressed keys and assign value 1 to indicate the the key is pressed
  if (key==CODED){
    if(keyCode==RIGHT){keyStatus[0]=1;}
    if(keyCode==LEFT){keyStatus[1]=1;}
    if(keyCode==UP){keyStatus[2]=1;}
    if(keyCode==DOWN){keyStatus[3]=1;}
    if(keyCode==CONTROL){keyStatus[10]=1;}
    if(keyCode==SHIFT){keyStatus[11]=1;}
  }
  else{
    if(key=='d'){keyStatus[4]=1;}
    if(key=='a'){keyStatus[5]=1;}
    if(key=='w'){keyStatus[6]=1;}
    if(key=='s'){keyStatus[7]=1;}
    if(key=='e'){keyStatus[8]=1;}
    if(key=='q'){keyStatus[9]=1;}
  }
}

void keyReleased(){
  //Read the pressed keys and assign value 0 to indicate the the key is pressed
  if (key==CODED){
    if(keyCode==RIGHT){keyStatus[0]=0;}
    if(keyCode==LEFT){keyStatus[1]=0;}
    if(keyCode==UP){keyStatus[2]=0;}
    if(keyCode==DOWN){keyStatus[3]=0;}
    if(keyCode==CONTROL){keyStatus[10]=0;}
    if(keyCode==SHIFT){keyStatus[11]=0;}
  }
  else{
    if(key=='d'){keyStatus[4]=0;}
    if(key=='a'){keyStatus[5]=0;}
    if(key=='w'){keyStatus[6]=0;}
    if(key=='s'){keyStatus[7]=0;}
    if(key=='e'){keyStatus[8]=0;}
    if(key=='q'){keyStatus[9]=0;}
  }
}

The output from Processing:
0,0,0,0,0,0,0,0,0,1,0,0
0,0,0,0,0,0,0,0,0,1,0,0
0,0,0,0,0,0,0,0,1,1,0,0
0,0,0,0,0,0,0,0,1,1,0,0

  • The value 1 appears when a key is pressed, value 0 appears when a key is unpressed.

  • If more than one key is pressed you will see more than one value 1

  • Every few rows, Processing skips a line feed(\n) and shows something like this:
    0,0,0,0,0,0,0,0,0,1,0,00,0,0,0,0,0,0,0,1,1,0,0

  • Every few rows, Processing skips a few fields of the array that I am printing and it shows something like this:
    0,0,0,0,0,0,0,0,0,1,0,0
    0,0,0,0,0,0,0,0,
    0,0,0,0,0,0,0,0,0,1,0,0

My Arduino code:

#include <Braccio.h>
#include <Servo.h>

Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_ver;
Servo wrist_rot;
Servo gripper;

//Constants and variables to hold input from the serial port
int keysIndex = 0;
const int KEYS = 12;
int input[KEYS] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

//Constants and variables to hold the angles of the motors
const int MOTORS = 6;
                  //{M1,M2,  M3,  M4, M5, M6}
int angle[MOTORS] = {0 , 45, 180, 180, 90, 90};

//Constant to control the angle increment when pressing a key
const int increment = 25;

void setup() {
  //De-activate Braccio´s soft start
  pinMode(12, OUTPUT);
  digitalWrite(12, HIGH);
  Braccio.begin(SOFT_START_DISABLED);

  //Setup the serial port
  Serial.begin(9600);
}

void loop() {
  //Read the input from Processing through the serial port:
  if (Serial.available() > 0) {
    char ch = Serial.read();

    //Read fields between comas(,):
    if (ch == '0' || ch == '1') {
      input[keysIndex] = (ch - '0');
    }
    else if (ch == ',') {             //if it is a coma(,) then change field
      if (keysIndex < KEYS - 1) {
        keysIndex++;
      }
    }
    else if (ch == '\n') {           //if it is a line feed(\n) then end read.

      //For each field in the input array, evaluate if it is a 0 or a 1 and midify the angles of the motors
      for (int e = 0; e <= KEYS - 1; e++) {
        switch (e) {
          case 0:                               // M1 clockwise: RIGHT
            if (input[e] == 1) {
              angle[0] = angle[0] + increment;
              if (angle[0] > 180) {
                angle[0] = 180;
              }
            }
            break;
          case 1:                               // M1 counterclockwise: LEFT
            if (input[e] == 1) {
              angle[0] = angle[0] - increment;
              if (angle[0] < 0) {
                angle[0] = 0;
              }
            }
            break;
          case 2:                               // M2 front tilt: UP
            if (input[e] == 1) {
              angle[1] = angle[1] + increment;
              if (angle[1] > 165) {
                angle[1] = 165;
              }
            }
            break;
          case 3:                               // M2 back tilt: DOWN
            if (input[e] == 1) {
              angle[1] = angle[1] - increment;
              if (angle[1] < 15) {
                angle[1] = 15;
              }
            }
            break;
          case 4:                               // M3 front tilt: D
            if (input[e] == 1) {
              angle[2] = angle[2] + increment;
              if (angle[2] > 180) {
                angle[2] = 180;
              }
            }
            break;
          case 5:                               // M3 back tilt: A
            if (input[e] == 1) {
              angle[2] = angle[2] - increment;
              if (angle[2] < 0) {
                angle[2] = 0;
              }
            }
            break;
          case 6:                              // M4 front tilt: W
            if (input[e] == 1) {
              angle[3] = angle[3] + increment;
              if (angle[3] > 180) {
                angle[3] = 180;
              }
            }
            break;
          case 7:                             // M4 back tilt: S
            if (input[e] == 1) {
              angle[3] = angle[3] - increment;
              if (angle[3] < 0) {
                angle[3] = 0;
              }
            }
            break;
          case 8:                            // M5 clockwise: E
            if (input[e] == 1) {
              angle[4] = angle[4] + increment;
              if (angle[4] > 180) {
                angle[4] = 180;
              }
            }
            break;
          case 9:                            // M5 clokwise: Q
            if (input[e] == 1) {
              angle[4] = angle[4] - increment;
              if (angle[4] < 0) {
                angle[4] = 0;
              }
            }
            break;
          case 10:                           // M6 grip close: Ctrl
            if (input[e] == 1) {
              angle[5] = angle[5] + increment;
              if (angle[5] > 127) {
                angle[5] = 127;
              }
            }
            break;
          case 11:                           // M6 grip open: Shift
            if (input[e] == 1) {
              angle[5] = angle[5] - increment;
              if (angle[5] < 52) {
                angle[5] = 52;
              }
            }
            break;
        }
      }
      //Write the new angles to the motors
                         //(step delay,M1       ,M2       ,M3       , M4      , M5      , M6      );
      Braccio.ServoMovement(20        , angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]);
    }
    keysIndex=0;
  }
}

The output from Arduino:

  • The expected output would be several servos moving at the same time… obviously, that is not happening.

Hope this was enough info. Looking forward to hearing your comments.

Did you do the tutorial? https://www.arduino.cc/en/Guide/Braccio