Trouble with Arduino + Leap motion - controlled quad

Gabriel

Gabriel

Well-Known Member
Hi. I'm trying to use my arduino and leap motion controller to control my quad. I've attached the arduino to my kk2 flight controller so that it can output signals to the kk2. Currently, I'm just trying to adjust the throttle signal to the quad based on the output from the leap motion.

My arduino code which outputs the signals to the kk2:

Code: 
#include <string.h>
#include <Servo.h>

Servo throttle;
Servo ail;
Servo elev;
Servo rud;

void setup()
{
 
Serial.begin(9600);



throttle.attach(5);
ail.attach(10);
elev.attach(9);
rud.attach(6);

throttle.write(0);
   rud.write(1187);
   Serial.println("ARMED");

}

void loop()
{

int data[4];
 
if (Serial.available()) {
 
   data[0] = (Serial.parseInt());
  Serial.print("Data0: ");
  Serial.println(data[0]);

  data[1] = (Serial.parseInt());
  Serial.print("Data1: ");
  Serial.println(data[1]);

   data[2] = (Serial.parseInt());
  Serial.print("Data2: ");
  Serial.println(data[2]);
 
   data[3] = (Serial.parseInt());
  Serial.print("Data3: ");
  Serial.println(data[3]);


  throttle.write(data[0]);

Serial.println("DID IF");

delete data;

}
  Serial.print("Data0: ");
    Serial.println(data[0]);
Serial.println("end");
 
}

My Java side code which sends the outputs to the arduino:

Code: 
import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.SerialPort;
import com.leapmotion.leap.*;

import java.io.IOException;

import com.leapmotion.leap.Gesture.State;
import java.io.OutputStream;

import com.leapmotion.leap.Controller;
import com.leapmotion.leap.Frame;
import com.leapmotion.leap.Listener;
//import java.lang.Math;

class SampleListenerMain extends Listener {

    public int posmapxold = 0;
    public int posmapyold = 0;
    public int posmapzold = 0;
   
   
public void onInit(Controller controller) {
System.out.println("Initialized");

}

public void onConnect(Controller controller) {
System.out.println("Connected");
controller.enableGesture(Gesture.Type.TYPE_SWIPE);
controller.enableGesture(Gesture.Type.TYPE_CIRCLE);
controller.enableGesture(Gesture.Type.TYPE_SCREEN_TAP);
controller.enableGesture(Gesture.Type.TYPE_KEY_TAP);


}

public void onDisconnect(Controller controller) {
System.out.println("Disconnected");
}

public void onExit(Controller controller) {
System.out.println("Exited");
}
public void onFrame(Controller controller) {

   
Frame frame = controller.frame();

Hand hand = frame.hands().rightmost();
float positionx = hand.palmPosition().getX();
System.out.println("X: " + positionx);

float positiony = hand.palmPosition().getY();
System.out.println("Y:" + positiony);

float positionz = hand.palmPosition().getZ();
System.out.println("Z: " + positionz);

System.out.println("Hands: " + frame.hands().count());

int in_min = -117;
int in_max = 117;
int out_min = 900;
int out_max = 1900;
int in_min2 = -73;
int in_max2 = 73;

int positionmapx = (int) ((positionx - in_min) * (out_max - out_min) / (in_max - in_min)) + out_min;
int positionmapy = (int) ((positiony - in_min) * (out_max - out_min) / (in_max - in_min)) + out_min;
int positionmapz = (int) ((positionz - in_min) * (out_max - out_min) / (in_max2 - in_min)) + out_min;


if (frame.hands().count() == 0) {
    //LeapDrone.writeToArduino("0");
}



String data = Integer.toString(positionmapy)+":"+ Integer.toString(positionmapx)+":"+Integer.toString(positionmapz);

System.out.println("data: " + data);


System.out.println("Old X: " + posmapxold);

System.out.println("Difference X: " + Math.abs(positionmapx - posmapxold));

if (Math.abs(positionmapx - posmapxold) >= 5 && posmapxold > 0) {


    LeapDrone.writeToArduino(data);
//LeapDrone.writeToArduino("100:100:100:100");


}

if (Math.abs(positionmapy - posmapyold) >= 5 && posmapyold > 0) {
   

    LeapDrone.writeToArduino(data);
   


}

if (Math.abs(positionmapz - posmapzold) >= 5 && posmapzold > 0) {
   

    LeapDrone.writeToArduino(data);
   
   
}


posmapxold = positionmapx;
posmapyold = positionmapy;
posmapzold = positionmapz;

System.out.println("New-Old X: " + posmapxold);



GestureList gestures = frame.gestures();
for (int i = 0; i < gestures.count(); i++) {
    Gesture gesture = gestures.get(i);
   
   
   
}
}
}

class LeapDrone {
static OutputStream out = null;

public static void main(String[] args) {
//Connect to Arduino BT
   
try
{
(new LeapDrone()).connect("/dev/cu.usbmodem1421");
}
catch ( Exception e )
{
e.printStackTrace();
System.exit(0);
}

// Create a sample listener and controller
SampleListenerMain listener = new SampleListenerMain();
Controller controller = new Controller();

// Have the sample listener receive events from the controller
controller.addListener(listener);
// Keep this process running until Enter is pressed
System.out.println("Press Enter to quit...");
try {
System.in.read();
} catch (IOException e) {
e.printStackTrace();
}

// Remove the sample listener when done
controller.removeListener(listener);
}

void connect ( String portName ) throws Exception {

CommPortIdentifier portIdentifier = CommPortIdentifier.getPortIdentifier(portName);
if ( portIdentifier.isCurrentlyOwned() )
{
System.out.println("Error: Port is currently in use");
}
else
{
CommPort commPort = portIdentifier.open(this.getClass().getName(),2000);

if ( commPort instanceof SerialPort )
{
SerialPort serialPort = (SerialPort) commPort;
serialPort.setSerialPortParams(9600,SerialPort.DATABITS_8,SerialPort.STOPBITS_1,
SerialPort.PARITY_NONE);
out = serialPort.getOutputStream();
}
else
{
System.out.println("Error: Only serial ports are handled by this example.");
}
}
}

public static void writeToArduino(String data)
{
String tmpStr = data;
byte bytes[] = tmpStr.getBytes();
try {
out.write(bytes);
} catch (IOException e) { }
}
}

The Issue:

The issue I have is that when I run my code, the motors sort of just randomly turn on and off again at seemingly random intervals. If I send '1400' throttle once through the arduino serial terminal, the motors stay on, but when my code continuously outputs '1400' throttle, the motors keep stopping and starting. I've really hit a wall here... I don't know what I'm doing wrong..

Any help is greatly appreciated!


Thanks,

-Gabriel
 
Gabriel said:
Hi. I'm trying to use my arduino and leap motion controller to control my quad. I've attached the arduino to my kk2 flight controller so that it can output signals to the kk2. Currently, I'm just trying to adjust the throttle signal to the quad based on the output from the leap motion.

My arduino code which outputs the signals to the kk2:

Code: 
#include <string.h>
#include <Servo.h>

Servo throttle;
Servo ail;
Servo elev;
Servo rud;

void setup()
{

Serial.begin(9600);



throttle.attach(5);
ail.attach(10);
elev.attach(9);
rud.attach(6);

throttle.write(0);
   rud.write(1187);
   Serial.println("ARMED");

}

void loop()
{

int data[4];

if (Serial.available()) {

   data[0] = (Serial.parseInt());
  Serial.print("Data0: ");
  Serial.println(data[0]);

  data[1] = (Serial.parseInt());
  Serial.print("Data1: ");
  Serial.println(data[1]);

   data[2] = (Serial.parseInt());
  Serial.print("Data2: ");
  Serial.println(data[2]);

   data[3] = (Serial.parseInt());
  Serial.print("Data3: ");
  Serial.println(data[3]);


  throttle.write(data[0]);

Serial.println("DID IF");

delete data;

}
  Serial.print("Data0: ");
    Serial.println(data[0]);
Serial.println("end");

}

My Java side code which sends the outputs to the arduino:

Code: 
import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.SerialPort;
import com.leapmotion.leap.*;

import java.io.IOException;

import com.leapmotion.leap.Gesture.State;
import java.io.OutputStream;

import com.leapmotion.leap.Controller;
import com.leapmotion.leap.Frame;
import com.leapmotion.leap.Listener;
//import java.lang.Math;

class SampleListenerMain extends Listener {

    public int posmapxold = 0;
    public int posmapyold = 0;
    public int posmapzold = 0;
 
 
public void onInit(Controller controller) {
System.out.println("Initialized");

}

public void onConnect(Controller controller) {
System.out.println("Connected");
controller.enableGesture(Gesture.Type.TYPE_SWIPE);
controller.enableGesture(Gesture.Type.TYPE_CIRCLE);
controller.enableGesture(Gesture.Type.TYPE_SCREEN_TAP);
controller.enableGesture(Gesture.Type.TYPE_KEY_TAP);


}

public void onDisconnect(Controller controller) {
System.out.println("Disconnected");
}

public void onExit(Controller controller) {
System.out.println("Exited");
}
public void onFrame(Controller controller) {

 
Frame frame = controller.frame();

Hand hand = frame.hands().rightmost();
float positionx = hand.palmPosition().getX();
System.out.println("X: " + positionx);

float positiony = hand.palmPosition().getY();
System.out.println("Y:" + positiony);

float positionz = hand.palmPosition().getZ();
System.out.println("Z: " + positionz);

System.out.println("Hands: " + frame.hands().count());

int in_min = -117;
int in_max = 117;
int out_min = 900;
int out_max = 1900;
int in_min2 = -73;
int in_max2 = 73;

int positionmapx = (int) ((positionx - in_min) * (out_max - out_min) / (in_max - in_min)) + out_min;
int positionmapy = (int) ((positiony - in_min) * (out_max - out_min) / (in_max - in_min)) + out_min;
int positionmapz = (int) ((positionz - in_min) * (out_max - out_min) / (in_max2 - in_min)) + out_min;


if (frame.hands().count() == 0) {
    //LeapDrone.writeToArduino("0");
}



String data = Integer.toString(positionmapy)+":"+ Integer.toString(positionmapx)+":"+Integer.toString(positionmapz);

System.out.println("data: " + data);


System.out.println("Old X: " + posmapxold);

System.out.println("Difference X: " + Math.abs(positionmapx - posmapxold));

if (Math.abs(positionmapx - posmapxold) >= 5 && posmapxold > 0) {


    LeapDrone.writeToArduino(data);
//LeapDrone.writeToArduino("100:100:100:100");


}

if (Math.abs(positionmapy - posmapyold) >= 5 && posmapyold > 0) {
 

    LeapDrone.writeToArduino(data);
 


}

if (Math.abs(positionmapz - posmapzold) >= 5 && posmapzold > 0) {
 

    LeapDrone.writeToArduino(data);
 
 
}


posmapxold = positionmapx;
posmapyold = positionmapy;
posmapzold = positionmapz;

System.out.println("New-Old X: " + posmapxold);



GestureList gestures = frame.gestures();
for (int i = 0; i < gestures.count(); i++) {
    Gesture gesture = gestures.get(i);
 
 
 
}
}
}

class LeapDrone {
static OutputStream out = null;

public static void main(String[] args) {
//Connect to Arduino BT
 
try
{
(new LeapDrone()).connect("/dev/cu.usbmodem1421");
}
catch ( Exception e )
{
e.printStackTrace();
System.exit(0);
}

// Create a sample listener and controller
SampleListenerMain listener = new SampleListenerMain();
Controller controller = new Controller();

// Have the sample listener receive events from the controller
controller.addListener(listener);
// Keep this process running until Enter is pressed
System.out.println("Press Enter to quit...");
try {
System.in.read();
} catch (IOException e) {
e.printStackTrace();
}

// Remove the sample listener when done
controller.removeListener(listener);
}

void connect ( String portName ) throws Exception {

CommPortIdentifier portIdentifier = CommPortIdentifier.getPortIdentifier(portName);
if ( portIdentifier.isCurrentlyOwned() )
{
System.out.println("Error: Port is currently in use");
}
else
{
CommPort commPort = portIdentifier.open(this.getClass().getName(),2000);

if ( commPort instanceof SerialPort )
{
SerialPort serialPort = (SerialPort) commPort;
serialPort.setSerialPortParams(9600,SerialPort.DATABITS_8,SerialPort.STOPBITS_1,
SerialPort.PARITY_NONE);
out = serialPort.getOutputStream();
}
else
{
System.out.println("Error: Only serial ports are handled by this example.");
}
}
}

public static void writeToArduino(String data)
{
String tmpStr = data;
byte bytes[] = tmpStr.getBytes();
try {
out.write(bytes);
} catch (IOException e) { }
}
}

The Issue:

The issue I have is that when I run my code, the motors sort of just randomly turn on and off again at seemingly random intervals. If I send '1400' throttle once through the arduino serial terminal, the motors stay on, but when my code continuously outputs '1400' throttle, the motors keep stopping and starting. I've really hit a wall here... I don't know what I'm doing wrong..

Any help is greatly appreciated!


Thanks,

-Gabriel
Click to expand...
WHat do you see in the serial monitor? Are you seeing the PWM values jump around? Also if you go into the receiver test on the KK what are you seeing?


Have you calibrated the throttle range?
 
WHat do you see in the serial monitor? Are you seeing the PWM values jump around? Also if you go into the receiver test on the KK what are you seeing?


Have you calibrated the throttle range?
Click to expand...

Ok. So I looked in the receiver test.

When I type '1400' into the Serial terminal, throttle changes to '43' as expected and stays there. When I send '1400' using my java-side code, it flashes between '43' and '0' very quickly.... I've looked through my code and there's nowhere where I send '0'.....

What could be causing this?
 
Gabriel said:
Ok. So I looked in the receiver test.

When I type '1400' into the Serial terminal, throttle changes to '43' as expected and stays there. When I send '1400' using my java-side code, it flashes between '43' and '0' very quickly.... I've looked through my code and there's nowhere where I send '0'.....

What could be causing this?
Click to expand...
I assume the Java application is establishing a serial connection with the Arduino? Sometimes this jumping happens with PWM on Arduinos if you don't smooth it out.



"throttle.write(0);" Are you using this to wait before sending the KK info?
 
Last edited:
#include <string.h>
#include <Servo.h>

Servo throttle;
Servo ail;
Servo elev;
Servo rud;

void setup()
{

Serial.begin(9600);



throttle.attach(5);
ail.attach(10);
elev.attach(9);
rud.attach(6);

throttle.write(0);
rud.write(1187);
Serial.println("ARMED");

}

void loop()
{

int data[4];

if (Serial.available()) {

data[0] = (Serial.parseInt());
Serial.print("Data0: ");
Serial.println(data[0]);

data[1] = (Serial.parseInt());
Serial.print("Data1: ");
Serial.println(data[1]);

data[2] = (Serial.parseInt());
Serial.print("Data2: ");
Serial.println(data[2]);

data[3] = (Serial.parseInt());
Serial.print("Data3: ");
Serial.println(data[3]);


throttle.write(data[0]);

Serial.println("DID IF");

delete data;

}
Serial.print("Data0: ");
Serial.println(data[0]);
Serial.println("end");

}

So I assume that this is all that you have handling throttle info? Can you check to see that the controller is giving the Arduino a clean, steady input?
 
Yes. They're connected via serial.

I assume the Java application is establishing a serial connection with the Arduino? Sometimes this jumping happens with PWM on Arduinos if you don't smooth it out.



"throttle.write(0);" Are you using this to wait before sending the KK info?
Click to expand...

What do you mean by 'jumping'? Sadly, I cannot see the input that the arduino is receiving.... I can see what the Java is outputting. But I can't see what the arduino recieves....

The throttle.write(0) is part of the arming sequence for the kk2. It's only executed once in the code.

-Gabriel
 
Gabriel said:
Yes. They're connected via serial.



What do you mean by 'jumping'? Sadly, I cannot see the input that the arduino is receiving.... I can see what the Java is outputting. But I can't see what the arduino recieves....

The throttle.write(0) is part of the arming sequence for the kk2. It's only executed once in the code.

-Gabriel
Click to expand...
You may try to set it all up and open your serial monitor(s), move the control around and see what it shows. Does it mess up there, or is it just when the KK2.1.5 is receiving? What I was trying to figure out is where the problem begins.
 
You may try to set it all up and open your serial monitor(s), move the control around and see what it shows. Does it mess up there, or is it just when the KK2.1.5 is receiving? What I was trying to figure out is where the problem begins.
Click to expand...

I can't open the serial monitor while the java program is running on the serial port.

Is there another way to do that?
 
Gabriel said:
I can't open the serial monitor while the java program is running on the serial port.

Is there another way to do that?
Click to expand...
Well as I assume your serial connection is simplex (only one sends and the other receives). Then it should mean that if you read the Java's output while the controller is running, and it looks fine as you move the sliders (use another java client, simple python client, or whatever you need to see that). If it checks out fine then you should be able to substitute that one value (try high and low) and see if the KK has a problem with either. You may be using the wrong input range, or wrong pulse period, or sending the wrong average type (you have low average and high average)
 
Well as I assume your serial connection is simplex (only one sends and the other receives). Then it should mean that if you read the Java's output while the controller is running, and it looks fine as you move the sliders (use another java client, simple python client, or whatever you need to see that). If it checks out fine then you should be able to substitute that one value (try high and low) and see if the KK has a problem with either. You may be using the wrong input range, or wrong pulse period, or sending the wrong average type (you have low average and high average)
Click to expand...

I'm confused.... how do I change the pulse period? What pulse period does the kk require?
 
For some reason the High worked a lot better for me (smoother) when I was reading RC input from my hobby Rx
 
Then it should mean that if you read the Java's output while the controller is running, and it looks fine as you move the sliders (use another java client, simple python client, or whatever you need to see that).
Click to expand...

I tried to read the input over bluetooth on my other mac, but it kept saying there was no input to the port... Do you know of any program that can read the data being transmitter on a serial port?
 
I normally just use a python IDE, never had to do exactly what you are doing
Click to expand...

OK.. I'll look into it... Any links you can recommend to point me in the right direction?
 
https://www.arduino.cc/en/Tutorial/SecretsOfArduinoPWM
Click to expand...

Ok! So I was thinking about it and I realized the 9600 baud rate that I set wasn't nearly as high as the kk2 expects. I changed to baud rate to 115200 baud and it worked! No more stopping!

Now... I'm just having this issue: I tried to also write the aileron and elevator values, but it's doing the stopping thing again.... I increased the baud rate to 230400 (the max of the RXTX library I'm using). It's a bit better but it's still doing that.... Is there anything I can do besides using a different library?
 
Gabriel said:
Ok! So I was thinking about it and I realized the 9600 baud rate that I set wasn't nearly as high as the kk2 expects. I changed to baud rate to 115200 baud and it worked! No more stopping!

Now... I'm just having this issue: I tried to also write the aileron and elevator values, but it's doing the stopping thing again.... I increased the baud rate to 230400 (the max of the RXTX library I'm using). It's a bit better but it's still doing that.... Is there anything I can do besides using a different library?
Click to expand...
Could set somethnig up to where the last known value is sent, so that you can set a rate that the KK will expect, while only updating the actual value as fast as the library will allow.


Example if you got updates on something every 10 seconds but needed to send something every 10 seconds you could send the contents of variable A once every 5 seconds, but only update them once every 10 seconds.
 
You must log in or register to reply here.
Back
Top