using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Linq; using System.Text; using System.Threading.Tasks; using System.Windows.Forms; using Leap; using MonoBrick.EV3; /* This Project is an integration of two technologies, the Leap Motion sensor and the Lego EV3. This project references to the libraries from the Leap Motion SDK and the MonoBrick communication libraries to control the EV3 via Bluetooth. Created by German Vargas, Ph.D. Chair, Department of Mathematics College of Coastal Georgia One College Drive Brunswick, GA 31520 P: (912) 279-5918 gvargas@ccga.edu March 2014 */ namespace LeapMotionEV3 { //Declare the delegate that we will use to to update the frame from the LeapMotion public delegate void frameUpdateDelegate(Brick ev3); //Declare the delegate that we will use to update the UI (display text in the different text boxes) and control the EV3 public delegate void UIupdateDelegate(Brick ev3, float HPitch, float HYaw, int HFingerCount, int HCount, float HPosistionY, int bottomTachoReading); public partial class Form1 : Form { //Declare the LeapController public static Controller controller = new Controller(); bool listen; int bottomTachoReading; public Form1() { InitializeComponent(); } //************************** //Method that updates the UI and sends information to the EV3 //************************** void UIupdate(Brick ev3, float HPitch, float HYaw, int HFingerCount, int HCount, float HPositionY, int bottomTachoReading) { float HPitchN, HYawN, idleBuffer, HPositionYN; int powerB, powerC, HPositionYNint; //Idle threshold to avoid having to be perfectly flat with your palm to stop the motors idleBuffer = 0.3F; //UI update for debugging and visualization purposes textBox1.Text = HPitch.ToString(); textBox2.Text = HYaw.ToString(); textBox3.Text = HFingerCount.ToString(); //************************** //TRANSFORMATION TO EV3 DATA //************************** //Bounding and Normalizing of the Pitch and Yaw data, from [-0.3, 0.3] to [-1, 1] HPitchN = -Math.Min(Math.Max(HPitch / 0.3F, -1), 1); HYawN = Math.Min(Math.Max(HYaw / 0.3F, -1), 1); //Creation of an idle state to avoid having to be perfectly flat with your palm to stop the motors if (Math.Abs(HPitchN) <= idleBuffer) HPitchN = 0; if (Math.Abs(HYawN) <= idleBuffer) HYawN = 0; //Remapping (idleBuffer, 1] to (0,1] if (HPitchN > idleBuffer) HPitchN = (HPitchN - idleBuffer) / (1 - idleBuffer); if (HYawN > idleBuffer) HYawN = (HYawN - idleBuffer) / (1 - idleBuffer); //Remapping [1,-idleBuffer) to [1,0) if (HPitchN < -idleBuffer) HPitchN = (HPitchN + idleBuffer) / (1 - idleBuffer); if (HYawN < -idleBuffer) HYawN = (HYawN + idleBuffer) / (1 - idleBuffer); //Power to go to the the Left (B) and Right (C) motors powerB = (int)(Math.Min(Math.Max(HPitchN + HYawN * 0.5, -1), 1) * 100); powerC = (int)(Math.Min(Math.Max(HPitchN - HYawN * 0.5, -1), 1) * 100); //Cancel all power if no fingers or no hands are detected if (HFingerCount == 0 | HCount ==0) { powerB=0; powerC=0; } //Send power information to the EV3 Motors ev3.MotorB.On((sbyte)powerB); ev3.MotorC.On((sbyte)powerC); //Bounding and Normalizing of the Hand Position, from [150, 200] to [0, 1] HPositionYN = Math.Min(Math.Max((HPositionY-150) / 50F, 0), 1); HPositionYN = 1 - HPositionYN; // Map 1->0 and 0->1 HPositionYN = (float)(((int)HPositionYN*10)/10F);// Limit to steps of 0.1 (use of casting instead of the method round) //Mapping of the normalized state to the TachoReading corresponding to 0 (arm in top position) and bottomTachoReading (arm in lower position) HPositionYNint = (int)((float)bottomTachoReading * HPositionYN); textBox7.Text = HPositionYNint.ToString(); // For UI visualization and debugging purposes. //Raise the arm if no fingers or no hands are detected if (HFingerCount == 0 | HCount == 0) HPositionYNint = 0; ev3.MotorA.MoveTo(5,HPositionYNint,false); //Information is sent to the UI textBox4.Text = powerB.ToString(); textBox5.Text = powerC.ToString(); //This portion can be used to use other input from the user, like FingerCount, to further control the EV3 switch (HFingerCount) { case 2: textBox6.Text = "Hello"; //Add EV3 behavior; break; case 3: textBox6.Text = "Good Bye"; //Add EV3 behavior; break; default: textBox6.Text = "Waiting for Signal"; break; } } //************************** //Method that Updates the frame //************************** void frameUpdate(Brick ev3) { float HPitch, HYaw, HPositionY; int HFingerCount, HCount; while (listen) { //Get a new frame form the LeapMotion Controller Frame leapFrame = controller.Frame(); HPitch = leapFrame.Hands.Frontmost.Direction.Pitch; //Hand Pitch reading HYaw = leapFrame.Hands.Frontmost.Direction.Yaw; //Hand Yaw Reading HFingerCount = leapFrame.Hands.Frontmost.Fingers.Count;// Finger Count reading HCount = leapFrame.Hands.Count; //Hand Count reading HPositionY = leapFrame.Hands.Frontmost.PalmPosition.y; //Position of the palm in the Y axis to be used to controll the arm //Invoke the method that updates the UI Invoke(new UIupdateDelegate(UIupdate), new object[] { ev3, HPitch, HYaw, HFingerCount, HCount, HPositionY, bottomTachoReading }); //Wait for n milliseconds before continuing System.Threading.Thread.Sleep(50); } //Turn off Motors and close connection ev3.MotorA.Off(); ev3.MotorB.Off(); ev3.MotorC.Off(); ev3.Connection.Close(); } private void Form1_Load(object sender, EventArgs e) { } private void button1_Click(object sender, EventArgs e) { listen = true; //Define the new EV3 Brick, and open its connection. var ev3 = new Brick("com3"); ev3.Connection.Open(); //Evaluation of tachometer at the position corresponding to the lower position of the arm ev3.MotorA.ResetTacho(); ev3.MotorA.On(-100,100,false); //Turns on the motor and does not break upon completion System.Threading.Thread.Sleep(500); ev3.MotorA.Off(); bottomTachoReading = ev3.MotorA.GetTachoCount();//The arm is in the lower position // textBox7.Text = bottomTachoReading.ToString(); // System.Threading.Thread.Sleep(10000); //Declare a new update method to invoke the work in a new thread frameUpdateDelegate update = frameUpdate; update.BeginInvoke(ev3, null, null); } private void button2_Click(object sender, EventArgs e) { listen = false; } } }