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:
My Java side code which sends the outputs to the arduino:
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
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
