wireless robotic hand with feedback mechanism problem

Hey guys, I’ve built a robotic arm controlled by 5 servo motors, 40 gram servos controlled wirelessly through xbee s1 modules. I control the servos with flex sensors attached on a glove. Now, the robot also has force sensors on it, which when sufficient force is applied, drives a vibration motor on the control glove. The problem is that it never works at the same time. Either the force sensor and vibration motor work and the motors dont, or the flex sensors and the motors work. Only rarely can I get them both to work side by side. Most of the times, I notice that when I power on the circuit, the motors go from 0 degree position to 90 degree position, and stay there, regardless of which sensor value I change. Please help me out. Here’s the code for the glove and the robot, as well as the circuit diagrams attached.

//Glove code

int val1, val2, val3, val4, val5; 
int inbyte, sens1, sens2, sens3, sens4, sens5, outbyte; 
int mot1, mot2, mot3, mot4, mot5;
int vib1=3, vib2=5, vib3=6, vib4=9, vib5=10; // variables to hold the pin number of the connecting sensors in arduino



 
void setup()
{
  Serial.begin(9600);  
  pinMode(A0, INPUT);  
  pinMode(A1, INPUT);  
  pinMode(A2, INPUT);  
  pinMode(A3, INPUT);  
  pinMode(A4, INPUT);  
  pinMode(vib1,OUTPUT);
  pinMode(vib2,OUTPUT);
  pinMode(vib3,OUTPUT);
  pinMode(vib4,OUTPUT);
  pinMode(vib5,OUTPUT);
}                          


void loop()
{
  val1 = analogRead(A0); /*
  val2 = analogRead(A1);  *
  val3 = analogRead(A2);  *These lines use the analogRead() function to read analog values from the      pins A0 to A4 and store them in variables val1-val5
  val4 = analogRead(A3);  *
  val5 = analogRead(A4);  */
  Serial.println(2000);   /* 
  delay(1);                *
  Serial.println(val1);    * Here, the sensor values stored in val1-val5 from previous lines are sent via the Serial.println() function to the robotic hand to drive its motors. 
  delay(1);                * The initial 2000 and 2001 at the end are used as identifiers for the data so that the sensor values can be sent as a packet without being lost.
  Serial.println(val2);    * A delay of 1 ms is given so that the receiver end has time to receive all the data without any desynchronization. 
  delay(1);                * After the data packet is sent, the serial port is cleared using Serial.flush()
  Serial.println(val3);    *
  delay(1);                *
  Serial.println(val4);    *
  delay(1);                *
  Serial.println(val5);    *
  delay(1);                *
  Serial.println(2001);    *
  delay(1);                *
  Serial.flush();          */
  inbyte = Serial.parseInt();  /*
  sens1 = Serial.parseInt();    *After transmitting the flex sensor values to the robot, these lines are used to receive and store the incoming sensor values from 
  sens2 = Serial.parseInt();    * the robotic hand. Here, the force values from the sensors on the robot are received here. The variable inbyte and outbyte are used to 
  sens3 = Serial.parseInt();    * receive the identifiers for this data packet. In this case, the robot sensor values packet is marked by a '4000' in the start and '4001' 
  sens4 = Serial.parseInt();    * at the end. The function Serial.parseInt() outputs the first integer value received in the Serial port. 
  sens5 = Serial.parseInt();    *
  outbyte = Serial.parseInt();  */
  if(inbyte == 4000 && outbyte == 4001)  /*
  {                                       * Here, the received sensor values are checked for integrety by using the data identifiers 'inbyte' and 'outbyte' and the if statement
  mot1 = map(sens1, 0, 920, 0, 255);      * if the data received is from the force sensors in the robot hand, then these values are mapped to the variables mot1-mot5, which 
  mot2 = map(sens2, 0, 920, 0, 255);      * are used to drive the vibration motors present in each fingertip of the glove for haptic feedback. The variables mot1-mot5 are 
  mot3 = map(sens3, 0, 920, 0, 255);      * then used to drive the motors using the analogWrite() function and the vib1-vib5 variables which represent the pin numbers in arduino 
  mot4 = map(sens4, 0, 920, 0, 255);      * of the respective vibration motor. 
  mot5 = map(sens5, 0, 920, 0, 255);      *
  analogWrite(vib1,mot1);                 *
  analogWrite(vib2,mot2);                 *
  analogWrite(vib3,mot3);                 *
  analogWrite(vib4,mot4);                 *
  analogWrite(vib5,mot5);                 */
}
}
//Robot arm code
int inbyte, outbyte;                            //variables for marking the start and end of sensor values
int flex1,flex2,flex3,flex4,flex5;              // variables for holding the flex sensor values received from the glove
int sens1,sens2,sens3,sens4,sens5;              // variables for storing the force sensor values present in the robot
#include <Servo.h>                                    

Servo finger1, finger2, finger3, finger4, finger5;  
int servoPin1 = 3;                                  /*
int servoPin2 = 5;                                   * Assign the various PWM pins on arduino to the variables 
int servoPin3 = 6;                                   * servoPin1 - servoPin5. These will be used to attach the servo objects to their 
int servoPin4 = 9;                                   * respective pins
int servoPin5 = 10;                                  */



void setup() {    
  Serial.begin(9600);                 
  finger1.attach(servoPin1);
  finger2.attach(servoPin2);
  finger3.attach(servoPin3);
  finger4.attach(servoPin4);
  finger5.attach(servoPin5);
  
 // set each servo pin to output and also declare each of the analog pins as input pins 
                                        
  pinMode(servoPin1, OUTPUT);
  pinMode(servoPin2, OUTPUT);
  pinMode(servoPin3, OUTPUT);
  pinMode(servoPin4, OUTPUT);
  pinMode(servoPin5, OUTPUT);
  pinMode(A0,INPUT);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  pinMode(A4,INPUT);
}



void loop() {
     inbyte = Serial.parseInt();       /*
     flex1 = Serial.parseInt();         * In this part of the program, the flex sensor values from the glove needed to
     flex2 = Serial.parseInt();         * know the amount by which the motors should move is received from the serial port.
     flex3 = Serial.parseInt();         * To prevent data loss and desynchronization between glove and robot data, we mark the
     flex4 = Serial.parseInt();         * data packet with 'identifiers'. In this case, the variables 'inbyte' and 'outbyte' 
     flex5 = Serial.parseInt();         * store those values. The Serial.parseInt() is used to assign the value of the first integer value incoming from the serial port to each variable
     outbyte = Serial.parseInt();       */
     if(inbyte == 2000 && outbyte == 2001)                                     // only proceeds if the identifier values are correct, which signifies that the right set of sensor values has been received from the glove
     {
      /*  Defines "pos" variables as being proportional to the flex inputs.
       *  The ~350 to ~550 value range seemed adequate for my sensors, but will change
       * depending upon the resistance used, the quality of each sensor, etc  
       */
  int pos1 = map(flex1, 365, 544, 0, 180);
  pos1 = constrain(pos1, 0, 180);
  int pos2 = map(flex2, 335, 521, 0, 180);
  pos2 = constrain(pos2, 0, 180);
  int pos3 = map(flex3, 285, 521, 0, 180);
  pos3 = constrain(pos3, 0, 180);
  int pos4 = map(flex4, 335, 555, 0, 180);
  pos4 = constrain(pos4, 0, 180);
  int pos5 = map(flex5, 290, 525, 0, 180);
  pos5 = constrain(pos5, 0, 180);
  
  //Tell servos to move by the amount specified in the "pos" variables
  
  finger1.write(pos1);
  finger2.write(pos2);
  finger3.write(pos3);
  finger4.write(pos4);
  finger5.write(pos5);
  delay(1);
  Serial.flush(); //clear serial port for next operation
}

//Now, measure and store the force sensor values in the variables sens1-sens5 

sens1 = analogRead(A0);
sens2 = analogRead(A1);
sens3 = analogRead(A2);
sens4 = analogRead(A3);
sens5 = analogRead(A4);

// send the sensor values stored previuosly using Serial.println() along with the identifier values '4000' and '4001' to the glove

Serial.println(4000);
delay(1);
Serial.println(sens1);
delay(1);
Serial.println(sens2);
delay(1);
Serial.println(sens3);
delay(1);
Serial.println(sens4);
delay(1);
Serial.println(sens5);
delay(1);
Serial.println(4001);
delay(1);
Serial.flush();
}

Your code cannot work if most of it is commented out. Remove all excess occurrences of /* and */.

  mot1 = map(sens1, 0, 920, 0, 255);      * if the data received is from the force sensors in the robot hand, then these values are mapped to the variables mot1-mot5, which
  mot2 = map(sens2, 0, 920, 0, 255);      * are used to drive the vibration motors present in each fingertip of the glove for haptic feedback. The variables mot1-mot5 are
  mot3 = map(sens3, 0, 920, 0, 255);      * then used to drive the motors using the analogWrite() function and the vib1-vib5 variables which represent the pin numbers in arduino
  mot4 = map(sens4, 0, 920, 0, 255);      * of the respective vibration motor.
  mot5 = map(sens5, 0, 920, 0, 255);      *

Why isn't this done on the sending end?

 Serial.flush(); //clear serial port for next operation

Rubbish. That is NOT what that function does. RTFM.

Why do you think you need to delay after buffering the outgoing data?

DrDiettrich: Your code cannot work if most of it is commented out. Remove all excess occurrences of /* and */.

yeah I just put them here so people would know what I'm trying to do

PaulS:  mot1 = map(sens1, 0, 920, 0, 255);      * if the data received is from the force sensors in the robot hand, then these values are mapped to the variables mot1-mot5, which  mot2 = map(sens2, 0, 920, 0, 255);      * are used to drive the vibration motors present in each fingertip of the glove for haptic feedback. The variables mot1-mot5 are  mot3 = map(sens3, 0, 920, 0, 255);      * then used to drive the motors using the analogWrite() function and the vib1-vib5 variables which represent the pin numbers in arduino  mot4 = map(sens4, 0, 920, 0, 255);      * of the respective vibration motor.  mot5 = map(sens5, 0, 920, 0, 255);      *

Why isn't this done on the sending end?

 Serial.flush(); //clear serial port for next operation

Rubbish. That is NOT what that function does. RTFM.

Why do you think you need to delay after buffering the outgoing data?

good point, I guess I was mistaken about Serial.flush(). I understood it as a command to clear the serial buffer, which I wanted to do before the next serial operation. As for the delay, I'm only giving it so there would be no issues regarding synchronization. So basically what I'm trying to do, is to send a bunch of data from say point A to point B using xbee, and then after processing that, send another set of data from point B to point A.

sreshtiyer: yeah I just put them here so people would know what I'm trying to do

These comments tell the compiler what you want not to do :-]

DrDiettrich: These comments tell the compiler what you want not to do :-]

Well, I have removed them now. But I figured out what my main problem is. Turns out, there's some problem with the communication between the xbee modules. The glove is sending the values as expected, but the values arent being received correctly at the robot end. Only rarely do the values correctly match up. But the weird thing is, when I print the variables inbyte and outbyte before the 'if' statement at the receiver end, the values are being received correctly. but if I try doing the same after the 'if' statement, the serial monitor is blank. So can you suggest me a reliable way to send packets of data over xbee?

If the Serial Monitor does not show any output, the println statements are never reached. Check your comments again.

DrDiettrich: If the Serial Monitor does not show any output, the println statements are never reached. Check your comments again.

Yeah that's true. This time, I removed the comments and also made one MAJOR change. Instead of sending the data as is, I first stored them in an array and THEN sent them one by one using a for loop. This fixed all the problems I had. So far, communication between the two xbees seems to be working fine. But what I dont get is why does it need to be in an array for it to work? Why didnt it work earlier?

Show the code that didn't work, without the array.