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.