Hello,
my name is Levin I'm from germany and I go to a technical school.
I have made a project with the Kinect Sensor and the NXT Mindstorm, using gesture I steered the NXT.
But now I have a problem.
If the Kinect recognizes my skeleton it will not work, but when I hold my right hand over my Head the NXT drive.
Sorry if I have errors in spelling :(
Thanks in advance!
Now here my C# Code:
using System;
using System.IO;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using Microsoft.Kinect;
using AForge;
using AForge.Robotics.Lego;
namespace KinectDemo1
{
public partial class MainWindow : Window
{
public MainWindow()
{
InitializeComponent();
}
public KinectSensor sensor;
public NXTBrick nxt = new NXTBrick();
private void Window_Loaded(object sender, RoutedEventArgs e)
{
this.drawingGroup = new DrawingGroup();
this.imageSource = new DrawingImage(this.drawingGroup);
image3.Source = this.imageSource;
foreach (var potentialSensor in KinectSensor.KinectSensors)
{
if (potentialSensor.Status == KinectStatus.Connected)
{
sensor = potentialSensor;
break;
}
}
sensor = (from Sensor in KinectSensor.KinectSensors where Sensor.Status == KinectStatus.Connected select Sensor).FirstOrDefault();
if (sensor != null)
{
label9.Content = "Kinect ist verbunden!";
sensor.ColorFrameReady += new EventHandler<ColorImageFrameReadyEventArgs>(r_ColorFrameReady);
sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
sensor.SkeletonStream.Enable();
sensor.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(SensorSkeletonFrameReady);
sensor.SkeletonFrameReady += this.SensorSkeletonFrameReady;
sensor.Start();
}
else
{
label9.Content = "Kinect nicht verbunden!";
MessageBox.Show("Keine Kinect angeschlossen.", "Fehler", MessageBoxButton.OK, MessageBoxImage.Hand);
}
nxt.Connect("COM3");
int Akkustand;
if (nxt.GetBatteryPower(out Akkustand))
{
label6.Content = Akkustand.ToString();
progressBar1.Value = Akkustand;
}
if (nxt.Connect("COM3"))
{
label8.Content = "Mindstorm ist verbunden!";
}
}
private void Window_Closed(object sender, EventArgs e)
{
if (sensor != null)
{
sensor.Stop();
sensor.Dispose();
}
}
#region RGB-Bild der Kinect
void r_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
{
using (ColorImageFrame frame = e.OpenColorImageFrame())
{
if (frame == null)
return;
byte[] buffer = new byte[frame.PixelDataLength];
frame.CopyPixelDataTo(buffer);
image1.Source = BitmapSource.Create(frame.Width, frame.Height, 96, 96, PixelFormats.Bgr32, null, buffer, frame.Width * frame.BytesPerPixel);
}
}
#endregion
#region SkeletonView
private const float RenderWidth = 800.0f;
private const float RenderHeight = 600.0f;
private const double JointThickness = 3;
private const double BodyCenterThickness = 10;
private const double ClipBoundsThickness = 10;
private readonly Brush centerPointBrush = Brushes.Blue;
private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));
private readonly Brush inferredJointBrush = Brushes.Yellow;
private readonly Pen trackedBonePen = new Pen(Brushes.Green, 6);
private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1);
private DrawingGroup drawingGroup;
private DrawingImage imageSource;
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
using (DrawingContext dc = this.drawingGroup.Open())
{
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
this.DrawBonesAndJoints(skel, dc);
this.nxtDrive(skel);
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(
this.centerPointBrush,
null,
this.SkeletonPointToScreen(skel.Position),
BodyCenterThickness,
BodyCenterThickness);
}
}
}
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
private void DrawBonesAndJoints(Skeleton skeleton, DrawingContext drawingContext)
{
this.DrawBone(skeleton, drawingContext, JointType.Head, JointType.ShoulderCenter);
this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderLeft);
this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderRight);
this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.Spine);
this.DrawBone(skeleton, drawingContext, JointType.Spine, JointType.HipCenter);
this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipLeft);
this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipRight);
this.DrawBone(skeleton, drawingContext, JointType.ShoulderLeft, JointType.ElbowLeft);
this.DrawBone(skeleton, drawingContext, JointType.ElbowLeft, JointType.WristLeft);
this.DrawBone(skeleton, drawingContext, JointType.WristLeft, JointType.HandLeft);
this.DrawBone(skeleton, drawingContext, JointType.ShoulderRight, JointType.ElbowRight);
this.DrawBone(skeleton, drawingContext, JointType.ElbowRight, JointType.WristRight);
this.DrawBone(skeleton, drawingContext, JointType.WristRight, JointType.HandRight);
this.DrawBone(skeleton, drawingContext, JointType.HipLeft, JointType.KneeLeft);
this.DrawBone(skeleton, drawingContext, JointType.KneeLeft, JointType.AnkleLeft);
this.DrawBone(skeleton, drawingContext, JointType.AnkleLeft, JointType.FootLeft);
this.DrawBone(skeleton, drawingContext, JointType.HipRight, JointType.KneeRight);
this.DrawBone(skeleton, drawingContext, JointType.KneeRight, JointType.AnkleRight);
this.DrawBone(skeleton, drawingContext, JointType.AnkleRight, JointType.FootRight);
foreach (Joint joint in skeleton.Joints)
{
Brush drawBrush = null;
if (joint.TrackingState == JointTrackingState.Tracked)
{
drawBrush = this.trackedJointBrush;
}
else if (joint.TrackingState == JointTrackingState.Inferred)
{
drawBrush = this.inferredJointBrush;
}
if (drawBrush != null)
{
drawingContext.DrawEllipse(drawBrush, null, this.SkeletonPointToScreen(joint.Position), JointThickness, JointThickness);
}
}
}
private Point SkeletonPointToScreen(SkeletonPoint skelpoint)
{
DepthImagePoint depthPoint = this.sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(skelpoint, DepthImageFormat.Resolution640x480Fps30);
return new Point(depthPoint.X, depthPoint.Y);
}
private void DrawBone(Skeleton skeleton, DrawingContext drawingContext, JointType jointType0, JointType jointType1)
{
Joint joint0 = skeleton.Joints[jointType0];
Joint joint1 = skeleton.Joints[jointType1];
if (joint0.TrackingState == JointTrackingState.NotTracked ||
joint1.TrackingState == JointTrackingState.NotTracked)
{
return;
}
if (joint0.TrackingState == JointTrackingState.Inferred &&
joint1.TrackingState == JointTrackingState.Inferred)
{
return;
}
Pen drawPen = this.inferredBonePen;
if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
{
drawPen = this.trackedBonePen;
}
drawingContext.DrawLine(drawPen, this.SkeletonPointToScreen(joint0.Position), this.SkeletonPointToScreen(joint1.Position));
}
#endregion
#region Manuelle fahrt des Mindstorm Roboter
private void button1_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 100;
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 0;
nxt.SetMotorState(NXTBrick.Motor.B, motorState);
nxt.SetMotorState(NXTBrick.Motor.C, motorState);
}
private void button1_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 0;
nxt.SetMotorState(NXTBrick.Motor.All, motorState);
}
private void button2_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = -100;
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 0;
nxt.SetMotorState(NXTBrick.Motor.B, motorState);
nxt.SetMotorState(NXTBrick.Motor.C, motorState);
}
private void button2_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 0;
nxt.SetMotorState(NXTBrick.Motor.All, motorState);
}
private void button3_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 0;
motorState.Power = 100;
nxt.SetMotorState(NXTBrick.Motor.B, motorState);
motorState.Power = -100;
nxt.SetMotorState(NXTBrick.Motor.C, motorState);
}
private void button3_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 0;
nxt.SetMotorState(NXTBrick.Motor.All, motorState);
}
private void button4_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 0;
motorState.Power = -100;
nxt.SetMotorState(NXTBrick.Motor.B, motorState);
motorState.Power = 100;
nxt.SetMotorState(NXTBrick.Motor.C, motorState);
}
private void button4_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 0;
nxt.SetMotorState(NXTBrick.Motor.All, motorState);
}
private void button5_MouseEnter(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 360;
motorState.Power = 90;
nxt.SetMotorState(NXTBrick.Motor.A, motorState);
}
private void button5_MouseLeave(object sender, System.Windows.Input.MouseEventArgs e)
{
nxt.Connect("COM3");
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 0;
nxt.SetMotorState(NXTBrick.Motor.All, motorState);
}
#endregion
#region nxtDrive
private void nxtDrive(Skeleton skeleton)
{
SkeletonPoint headPos = skeleton.Joints[JointType.Head].Position;
SkeletonPoint righthandPos = skeleton.Joints[JointType.HandRight].Position;
SkeletonPoint lefthandPos = skeleton.Joints[JointType.HandLeft].Position;
bool LinkeHandÜberKopf = (headPos.Y < lefthandPos.Y);
bool RechteHandÜberKopf = (headPos.Y < righthandPos.Y);
if (nxt.Connect("COM3"))
{
if (RechteHandÜberKopf && LinkeHandÜberKopf)
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 100;
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 0;
nxt.SetMotorState(NXTBrick.Motor.B, motorState);
nxt.SetMotorState(NXTBrick.Motor.C, motorState);
}
else
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 0;
nxt.SetMotorState(NXTBrick.Motor.All, motorState);
}
if (RechteHandÜberKopf)
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 100;
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 0;
nxt.SetMotorState(NXTBrick.Motor.C, motorState);
}
else
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 0;
nxt.SetMotorState(NXTBrick.Motor.All, motorState);
}
if (LinkeHandÜberKopf)
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 100;
motorState.TurnRatio = 50;
motorState.Mode = NXTBrick.MotorMode.On;
motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
motorState.RunState = NXTBrick.MotorRunState.Running;
motorState.TachoLimit = 0;
nxt.SetMotorState(NXTBrick.Motor.B, motorState);
}
else
{
NXTBrick.MotorState motorState = new NXTBrick.MotorState();
motorState.Power = 0;
nxt.SetMotorState(NXTBrick.Motor.All, motorState);
}
}
else
{
MessageBox.Show("Es besteht keine Bluetooth verbindung zum Mindstorm!", "Fehler", MessageBoxButton.OK, MessageBoxImage.Warning);
}
}
#endregion
}
}