Trouble with Arduino + Leap motion - controlled quad

Discussion in 'Custom Programming' started by Gabriel, Apr 10, 2016.

  1. 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
     
  2. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    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?
     
  3. Gabriel

    Gabriel Well-Known Member

    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?
     
  4. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    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: Apr 10, 2016
  5. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    #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?
     
  6. Gabriel

    Gabriel Well-Known Member

    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
     
  7. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    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.
     
  8. Gabriel

    Gabriel Well-Known Member

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

    Is there another way to do that?
     
  9. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    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)
     
  10. Gabriel

    Gabriel Well-Known Member

    I'm confused.... how do I change the pulse period? What pulse period does the kk require?
     
  11. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    That is probably already set fine as it is standard, but IDK about the averaging.
     
  12. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    For some reason the High worked a lot better for me (smoother) when I was reading RC input from my hobby Rx
     
  13. Gabriel

    Gabriel Well-Known Member

    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?
     
  14. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    Not for mac.
     
  15. Gabriel

    Gabriel Well-Known Member

    Any for Windows? I'll have to edit the code a bit by I can make it work...
     
  16. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    I normally just use a python IDE, never had to do exactly what you are doing
     
  17. Gabriel

    Gabriel Well-Known Member

    OK.. I'll look into it... Any links you can recommend to point me in the right direction?
     
  18. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

  19. Gabriel

    Gabriel Well-Known Member

    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?
     
    GJH105775 likes this.
  20. GJH105775

    GJH105775 Avid Linux User, and U.S. Air Force 1C6 Moderator

    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.
     

Share This Page