Problems with stability

edited July 2014 in Arduino

i have some problems with this code, there are 2 micro servos connected at the Arduino and controlled by Leap Motion, the problem is instability in the motors. The problem is in the code? Anything I can change to improve the stability?

Processing CODE

    import com.onformative.leap.LeapMotionP5;
    import com.leapmotion.leap.Hand;
    import com.leapmotion.leap.Finger;
    LeapMotionP5 leap;

    int angle;
    int angle_2;

    import processing.serial.*;
    Serial port;

    public void setup() {
      // Definindo Interface grafica leap
      size(920, 920, P3D); 
      noFill();
      stroke(255);

      // set LEAP object
      leap = new LeapMotionP5(this); 

      // Definindo porta serial a ser usada
      println("");
      println(Serial.list());
      port = new Serial(this, Serial.list()[2], 9600); }

    public void draw() {
      background(0);
      fill(255);
      for (Hand hand : leap.getHandList()) {
      PVector handPos = leap.getPosition(hand);
      ellipse(handPos.x, handPos.y, 50, 50);
      if (handPos.y > 920) {
        angle = 180;
      }
      else if (handPos.y < 0){
        angle = 0;
      }

        if  (handPos.x > 920){
        angle_2 = 180;
      }
          else if (handPos.x < 0){
          angle_2 = 0;
      }  
      else  {
        angle = int((handPos.y) / 4);
        angle_2 = int((handPos.x) / 4);
      }
      port.write(angle);
      println(angle);
      port.write(angle_2);
      println(angle_2);
      }
    }
    public  void stop()  {
      leap.stop();
    }

I think the problem is in the Arduino code:

#include <Servo.h>

Servo servo1;
Servo servo2;
//int handPos;
int angle;
int angle_2;

void setup() {
  servo1.attach(4);
  servo2.attach(3);
  Serial.begin(9600);
  servo1.write(0);
  servo2.write(180);
}

void loop() {
  byte angle;
  byte angle_2;
  if (Serial.available()) {
    // Read angle from Processing
    angle = Serial.read();
    Serial.println(angle);
    angle_2 = Serial.read();
    Serial.println(angle_2);
    // If fingers in window, read servo angle
    servo1.write(angle_2);
    servo2.write(angle);
    delay(15);
  }
}

Please, if someone can help me. Thanks a lot

Sign In or Register to comment.