Leapduino.java

package seniordesign;
import java.time.LocalDateTime;

import com.leapmotion.leap.Controller;
public class Leapduino
{
//Main
public static final void main(String args[])
{  


   //Initialize serial communications.
   RS232Protocol Serial = new RS232Protocol();
   
   Serial.connect("COM4");
   //Initialize the Leapduino listener.
   LeapduinoListener leap = new LeapduinoListener(Serial);
   Controller controller = new Controller();
   controller.addListener(leap);
   while (true) {
	    try {
	        Thread.sleep(1000);
	    } catch (InterruptedException e) {
	        e.printStackTrace();
	    }
   }
}
}

LeapduinoListener.java

package seniordesign; 
import java.time.LocalDateTime;
import java.util.ArrayList;
import java.util.List;

import com.leapmotion.leap.*; 

public class LeapduinoListener extends Listener
{  
	private int posTable = 0;
	private double len1 = 50.0;
	private double len2 = 50.0;
	private double avgx = 0;
	private double avgy = 0;
	private double avgz = 0;
	private long timeSent;
	private long timeNow;
	private ArrayList<Float> dataListx = new ArrayList<Float>(200);
	private ArrayList<Float> dataListy = new ArrayList<Float>(200);
	private ArrayList<Float> dataListz = new ArrayList<Float>(200);
	private double theta1;
	private double theta2;
	
	

//Serial port that we'll be using to communicate with the Arduino.
private RS232Protocol serial; 
//Constructor
public LeapduinoListener(RS232Protocol serial)
{
   this.serial = serial;
   this.timeSent=0;
} 
//Member Function: onInit
public void onInit(Controller controller)
{
   System.out.println("Initialized");
} 
//Member Function: onConnect
public void onConnect(Controller controller)
{
   System.out.println("Connected");
} 
//Member Function: onDisconnect
public void onDisconnect(Controller controller)
{
   System.out.println("Disconnected");
} 
//Member Function: onExit
public void onExit(Controller controller)
{
   System.out.println("Exited");
} 
//calculate the law of cosines
public double lawOfCosines(double a,double b, double c){
	
	return (float) Math.acos(((((a*a) + (b*b))) - (c*c))/(2*a*b));
}
//calculate the distance between the two points
public double distance( double y, double z){

	return Math.sqrt((z*z) + (y*y));
}

//calculate theta 1 
public double angle1(double z, double y){
	
	double dist = distance(z,y);
	double D1 = Math.atan2(y, z);
	double D2 = lawOfCosines(dist, len1, len2); 
	double A1 = D1+ D2;
	return A1;
	
}
//public double inverseKinematics1(double y,double z){
//	double hypotenuse = Math.sqrt((y*y)+(z*z));
//	double a = Math.atan(y/z);
//	double b = Math.acos(((len1*len1)+(hypotenuse*hypotenuse)-(len2*len2))/(2*len1*len2));
//	double theta1 = deg(a+b);
//	
//	return theta1;
//	
//}
//public double inverseKinematics2(double y, double z){
//	double hypotenuse = Math.sqrt((y*y)+(z*z));
//	double c = Math.acos(((len2*len2)+(len1*len1)-(hypotenuse*hypotenuse))/(2*len1*len2));
//	double theta2 = 180-deg(c);
//	return theta2;
//}
//calculate theta 2
public double angle2(double z, double y){
	
	double dist = distance(z,y);
	double A2 = lawOfCosines(len1, len2, dist);
	return A2;
}

public double deg(double rad){
	
	return (rad * 180)/Math.PI;
}
Vector leapToWorld(Vector leapPoint, InteractionBox iBox)
{
	//leapPoint.getZ() *= -1.0f;
	
	Vector origin = new Vector(0, 0, 0); //TODO: origin is now taking into account set up. (0.0) -> edge of the game board
	//we have a max reach of 330 along the Z axis
	
    //leapPoint.setZ((float) (leapPoint.getZ() * -1.0)); //right-hand to left-hand rule
    Vector normalized = iBox.normalizePoint(leapPoint, true);
    
    normalized = normalized.plus(origin); //re-center origin
    return normalized.times((float) 100.0); 
}




//Member Function: onFrame
public void onFrame(Controller controller)
{
	
//Get the most recent frame.
   Frame frame = controller.frame(); //set an object to the current frame
   Frame previous = controller.frame(1); //set an object to the previous frame
   Hand hand = frame.hands().frontmost(); //read the hand in the current frame
   Hand hand1 = previous.hands().frontmost(); //read the hand in the previous frame
   Boolean handIsEqual = hand.equals(hand1); //compare the hand in the previous frame 
   
   InteractionBox box = frame.interactionBox();
   Vector leapPos = new Vector (hand.palmPosition());
   Vector realPos = new Vector (leapToWorld(leapPos,box));

//Gather data for taking an average over 200ms
//   timeNow = System.currentTimeMillis();
//   if((timeNow-timeSent) < 500)
//	{	

//		   dataListx.add(x);
//		   dataListy.add(y);
//		   dataListz.add(z);
//		
//	}else{
//		timeSent = timeNow;
//		double sumx=0;
//		double sumy=0;
//		double sumz=0;
//		for(int i=0;i<dataListx.size();i++){
////			if (Math.abs(dataListx.get(i-1)-dataListx.get(i))>5){ 
////				double temp = dataListx.get(i-1);
////				dataListx.add(i, (float) (temp+5));				
////				}
////			if (Math.abs(dataListy.get(i-1)-dataListy.get(i))>5){ 
////				double temp = dataListy.get(i-1);
////				dataListy.add(i, (float) (temp+5));				
////				}
////			if (Math.abs(dataListz.get(i-1)-dataListz.get(i)>5){ 
////				double temp = dataListz.get(i-1);
////				dataListz.add(i, (float) (temp+5));				
////				}
//			sumx+=dataListx.get(i);
//			sumy+=dataListy.get(i);
//			sumz+=dataListz.get(i);
//		}
//		
//		avgx=sumx/(double)dataListx.size();
//		avgy=sumy/(double)dataListy.size();
//		avgz=sumz/(double)dataListz.size();
//
//		dataListx.clear();
//		dataListy.clear();
//		dataListz.clear();
//		
//		}
   
   //if (avgy >90.0 ){avgy = 90;}
//   if (avgz == 0.0){avgz = 1;}
//// if (avgy < 5 && avgz < 5){
////	   theta1 = 0;
////	   theta2 = 180;
////   }
//   if (avgy < 5 && avgz > 90){
//	
//	   theta1 = 110;
//	   theta2 = 40;
//   }
//   float x = realPos.getX();
//   float y = realPos.getY();
//   float z = realPos.getZ();
   //x = (float) (x-50.0);
   //y = (float) (y-50.0);
   //z = (float) (z-50.0);
   
   //Verify a hand is in view.
   
   if (frame.hands().count() > 0)
   {
	   float x = hand.palmPosition().getX();
	   float y = realPos.getY();
	   float z = realPos.getZ();
	 if (x>32.5){x=(float) 32.5;}
	 if (y>74.5){y=(float) 74.5;}
	 if (z>61.5){z=(float) 61.5;}
	 
//	   
//
     float graspAngle = hand.grabAngle();  
//
     double ang1 = angle1(z, y);
     theta1 = deg(ang1);
     double ang2 = angle2(z, y);
     theta2 = deg(ang2);
     theta1 = 180-theta1;
     
     if (theta1>110){theta1=110;}
     if (theta1<0){theta1=0;}
     theta2 = Math.abs(180-theta2);
     if (theta2<40){theta2=40;}
     //if (theta2<180){theta2=0;}
     
     
     
//   
//     double yy = 0;
//     double zz = 100;
//     
//     double ang1 = angle1(zz,yy);
//     double theta1 = deg(ang1);
//
//     
//     double ang2 = angle2(zz,yy);
//     double theta2 = deg(ang2);
  
     
     int count = 0;
     
     
    // float rotationIntentFactor = hand.rotationProbability(previous);
     
     
     if (handIsEqual){
    	 if (posTable == 0 && count == 0){
    		 posTable = 180;
    		 count = 1;
    	 }else if (posTable == 180 && count == 0){
    		 posTable =0;
    		 count = 1;
    	 }
     }
//   
     double normalize = 3.0;
     double n = 100*normalize;
     double X = 1.5 + 2 * x / n;
     double baseAngle = Math.cos(X)*90.0;

     
     Math.round(theta1);
     Math.round(theta2);
     Math.round(baseAngle);
     Math.round(graspAngle);
     if (baseAngle>180){baseAngle=180;}
     if (baseAngle<0){baseAngle=0;}
//     
     
     
//Send the data via serial     
//note the scaling factor here
     String Theta1 = (String.valueOf(theta1));
     
//note the scaling factor here
     String Theta2 = (String.valueOf(theta2));
     
     String BaseAngle = (String.valueOf(baseAngle));
     String GraspAngle = (String.valueOf(graspAngle));
     String PosTable = (String.valueOf(posTable));
     //------------------------------------------------------------
     
    	 
    	 
    	 serial.write(Theta1);
         serial.write(",");
         serial.write(Theta2);
         serial.write(",");
         serial.write(BaseAngle);
         serial.write(",");
         serial.write(GraspAngle);
         serial.write(",");
         serial.write(PosTable);    	 
 
     //System.out.println("X: " + x + "    Y: " + y + "     Z: " + z);
     //System.out.println("theta1: " + Theta1 + "     theta2: " + Theta2 + "    base angle: " + BaseAngle);
         System.out.println("base angle:  " + BaseAngle);
     //Give the Arduino some time to process our data.
 
     try{ Thread.sleep(140); }
     catch (InterruptedException e) { e.printStackTrace(); }
   }
}
}

RS232Protocol.java

package seniordesign;
import jssc.SerialPort;
import jssc.SerialPortEvent;
import jssc.SerialPortEventListener;
import jssc.SerialPortException; 
public class RS232Protocol
{
//Serial port we're manipulating.
private SerialPort port; 
//Class: RS232Listener
public class RS232Listener implements SerialPortEventListener  
{
   public void serialEvent(SerialPortEvent event)
   {
     //Check if data is available.
     if (event.isRXCHAR() && event.getEventValue() > 0)
     {
    	 
       try
       {
         int bytesCount = event.getEventValue();
         System.out.print(port.readString(bytesCount));
       }          
       catch (SerialPortException e) { e.printStackTrace(); }
     }
   }
}
//Member Function: connect
public void connect(String newAddress)
{
   try
   {
     //Set up a connection.
     port = new SerialPort(newAddress);    
     //Open the new port and set its parameters.
     port.openPort();
     port.setParams(38400, 8, 1, 0);          
     //Attach our event listener.
     port.addEventListener(new RS232Listener());
   }  
   catch (SerialPortException e) { e.printStackTrace(); }
}
//Member Function: disconnect
public void disconnect()
{
   try { port.closePort(); }  
   catch (SerialPortException e) { e.printStackTrace(); }
}
//Member Function: write
public void write(String text)
{
   try { port.writeBytes(text.getBytes()); }  
   catch (SerialPortException e) { e.printStackTrace(); }
}
}