private void init() { try { rp = new RobotPostureProxy(ip, 9559); rp.goToPosture("StandZero", 1); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } try { mp = new MotionProxy(ip, port); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } RArm = new Arms(); LArm = new Arms(); }
public static void MoveHead(MotionProxy motionProxy, float?headPitch, float?headYaw, bool isAbsolute, float time = 1.5f) { if (motionProxy == null || (!headPitch.HasValue && !headYaw.HasValue)) { return; } List <string> names = new List <string>(); List <float[]> times = new List <float[]>(); List <float[]> keys = new List <float[]>(); if (headPitch.HasValue) { names.Add("HeadPitch"); keys.Add(new[] { headPitch.Value }); times.Add(new[] { time }); } if (headYaw.HasValue) { names.Add("HeadYaw"); keys.Add(new[] { headYaw.Value }); times.Add(new[] { time }); } var methodID = motionProxy.post.angleInterpolation(names, keys, times, isAbsolute); motionProxy.wait(methodID, 0); }
public void Connect(string ip) { try { _motion = new MotionProxy(ip,9559); // --------------- prepare limits -------------------------- // three floats as an ArrayList for each joint in the chain // min,max,maxNoLoadSpeedPerCycle ArrayList limits = (ArrayList)_motion.getLimits("Head"); ArrayList yawLimits = (ArrayList)limits[0]; ArrayList pitchLimits = (ArrayList)limits[1]; _minYaw = (float)yawLimits[0]; _maxYaw = (float)yawLimits[1]; _minPitch = (float)pitchLimits[0]; _maxPitch = (float)pitchLimits[1]; Console.WriteLine("MinYaw: " + _minYaw + " MaxYaw:" + _maxYaw + " MinPitch: " + _minPitch + " MaxPitch:" + _maxPitch); // give the joints some stiffness _motion.stiffnessInterpolation("Head",1.0f,1.0f); } catch(Exception e) { Console.Out.WriteLine("HeadPanning.Connect exception: " + e); } }
private void init() { try { rp = new RobotPostureProxy(ip, 9559); rp.goToPosture("StandZero", 1); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } try { mp = new MotionProxy(ip, port); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } RArm = new Arms(); LArm = new Arms(); }
public void init() { //make sure nao is ready to move try { rp = new RobotPostureProxy(ip, 9559); rp.goToPosture("StandZero", 1); Console.WriteLine("Connected to Nao with IP: " + ip); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } try { mp = new MotionProxy(ip, port); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } Arms RArm = new Arms(); }
static void ExecuteOnNao(List<float[]> naoSeq, bool isJointAction) { MotionProxy mp = new MotionProxy("127.0.0.1", 9559); // do stuff with naoskeletons and real NAO foreach (float[] act in naoSeq) { float LShoulderRoll = act[(int)AngleConverter.NaoJointAngle.LShoulderRoll]; float LShoulderPitch = act[(int)AngleConverter.NaoJointAngle.LShoulderPitch]; float LElbowYaw = act[(int)AngleConverter.NaoJointAngle.LElbowYaw]; float LElbowRoll = act[(int)AngleConverter.NaoJointAngle.LElbowRoll]; Console.WriteLine("LShoulderRoll: " + LShoulderRoll); Console.WriteLine("LShoulderPitch: " + LShoulderPitch); /* mp.setAngles("LElbowYaw", LElbowYaw, 0.4f); mp.setAngles("LElbowRoll", LElbowRoll, 0.4f); mp.setAngles("LShoulderRoll", LShoulderRoll, 0.4f); mp.setAngles("LShoulderPitch", LShoulderPitch, 0.4f);*/ String[] joints = new String[(int)AngleConverter.NaoJointAngle.count]; joints[(int)AngleConverter.NaoJointAngle.LElbowRoll] = "LElbowRoll"; joints[(int)AngleConverter.NaoJointAngle.LElbowYaw] = "LElbowYaw"; joints[(int)AngleConverter.NaoJointAngle.LShoulderPitch] = "LShoulderPitch"; joints[(int)AngleConverter.NaoJointAngle.LShoulderRoll] = "LShoulderRoll"; joints[(int)AngleConverter.NaoJointAngle.RElbowRoll] = "RElbowRoll"; joints[(int)AngleConverter.NaoJointAngle.RElbowYaw] = "RElbowYaw"; joints[(int)AngleConverter.NaoJointAngle.RShoulderPitch] = "RShoulderPitch"; joints[(int)AngleConverter.NaoJointAngle.RShoulderRoll] = "RShoulderRoll"; mp.angleInterpolationWithSpeed(joints, act, 0.3f); System.Threading.Thread.Sleep(50); } Console.Read(); }
public Bewegen(RobotPostureProxy rpp, MotionProxy motion, MainWindow mw, TextToSpeechProxy tts) { this.rpp = rpp; this.motion = motion; this.mw = mw; this.tts = tts; }
public void init() { //make sure nao is ready to move try { rp = new RobotPostureProxy(ip, 9559); rp.goToPosture("StandZero", 1); Console.WriteLine("Connected to Nao with IP: " + ip); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } try { mp = new MotionProxy(ip, port); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } Arms RArm = new Arms(); }
public NaoKinectProxy(String ip, int port) { _naoIP = ip; _naoPort = port; //Initiating connection and balancing Nao _naoMotion = new MotionProxy(_naoIP, port); }
public bool connect(string ip, bool onlymotion = false) { this.IP = ip; bool b = false; try { proxyMotion = new MotionProxy(ip, Port); if (onlymotion == false) { bool bc = proxyMotion.setCollisionProtectionEnabled("Arms", false); if (!bc) Console.WriteLine("Failed to disable collision protection"); StartMotorMonitor(); proxyBehaviorManager = new BehaviorManagerProxy(ip, Port); // Turn off the LEDs proxyLed = new LedsProxy(ip, Port); List<string> ledgroups = proxyLed.listGroups(); foreach (string ledgroup in ledgroups) { proxyLed.off(ledgroup); } // Speech proxyTTS = new TextToSpeechProxy(ip, Port); if (ip != "127.0.0.1") { // Sentinel proxySentinel = new SentinelProxy(ip, Port); // Turning off "Motor hot!" proxySentinel.enableHeatMonitoring(false); } // camera proxyCamera = new VideoDeviceProxy(ip, Port); // IdleMovement InitIdleMovement(); } b = true; } catch (Exception e) { Console.WriteLine("MotionProxy.Connect Exception: " + e); b = false; } IsConnected = b; return b; }
public static void StiffnessOn(MotionProxy proxy) { // We use the "Body" name to signify the collection of all joints var pNames = "Body"; var pStiffnessLists = 1.0f; var pTimeLists = 1.0f; proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists); }
//Control RArm with all Joints public static void controlRArm(MotionProxy mp, float RSP, float RSR, float RER, float REY, float RWY) { //Joint Controll //Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x) String[] names = { "RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw" }; float[] newangles = { RSP, RSR, RER, REY, RWY }; float fractionMaxSpeed = 0.1f; mp.setAngles(names, newangles, fractionMaxSpeed); }
public Gaze(string ip, int port) : this() { this.ip = ip; this.port = port; motion = new MotionProxy(ip, port); bezierMotion = new Bezier(ip, port); }
//Control RArm with all Joints public static void controlRArm(MotionProxy mp, float RSP, float RSR, float RER, float REY, float RWY) { //Joint Controll //Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x) String[] names = { "RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw" }; float[] newangles = { RSP, RSR, RER, REY, RWY }; float fractionMaxSpeed = 0.1f; mp.setAngles(names, newangles, fractionMaxSpeed); }
private void button_connect_to_IP_Click(object sender, EventArgs e) { this.tts = new TextToSpeechProxy(this.textBox_IP.Text, 9559); this.motion = new MotionProxy(this.textBox_IP.Text, 9559); this.BackColor = Color.AliceBlue; this.tts.say("connected."); this.motion.setStiffnesses("Body", 0); }
//Control LArm with all Joints public static void controlLArm(MotionProxy mp, float LSP, float LSR, float LER, float LEY, float LWY) { //Joint Controll //Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x) String[] names = { "LShoulderPitch", "LShoulderRoll", "LElbowRoll", "LElbowYaw", "LWristYaw" }; float[] newangles = { LSP, LSR, LER, LEY, LWY }; float fractionMaxSpeed = 0.1f; mp.setAngles(names, newangles, fractionMaxSpeed); }
//Control LArm with all Joints public static void controlLArm(MotionProxy mp, float LSP, float LSR, float LER, float LEY, float LWY) { //Joint Controll //Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x) String[] names = { "LShoulderPitch", "LShoulderRoll", "LElbowRoll", "LElbowYaw", "LWristYaw" }; float[] newangles = { LSP, LSR, LER, LEY, LWY }; float fractionMaxSpeed = 0.1f; mp.setAngles(names, newangles, fractionMaxSpeed); }
/// <summary> /// Default constructor. /// </summary> public Grabber() { NaoState.Instance.OnConnect += BuildCameraAndProcessor; NaoState.Instance.OnDisconnect += ClearCameraAndProcessor; if (NaoState.Instance.Connected) { BuildCameraAndProcessor(NaoState.Instance.IP.ToString(), NaoState.Instance.Port); } motion = Proxies.GetProxy <MotionProxy>(); posture = Proxies.GetProxy <RobotPostureProxy>(); }
public void Initialisierung(String ip, Int32 port) { motion = new MotionProxy(ip, port); rpp = new RobotPostureProxy(ip, port); tts = new TextToSpeechProxy(ip, port); Bewegen = new Bewegen(rpp, motion, mw, tts); Start Start = new Start(); //Nao geht in die Startposition Start.Startposition(rpp, tts); }
public void Initialisierung(String ip, Int32 port) { motion = new MotionProxy(ip, port); rpp = new RobotPostureProxy(ip, port); tts = new TextToSpeechProxy(ip, port); Nao_Start Nao_Start = new Nao_Start(); //Nao geht in die Startposition Nao_Start.Startposition(rpp, tts); }
private void button_connect_Click(object sender, EventArgs e) { string ip = this.textBox1.Text; this.tts = new TextToSpeechProxy(ip, 9559); this.motion = new MotionProxy(ip, 9559); this.posture = new RobotPostureProxy(ip, 9559); this.navigation = new NavigationProxy(ip, 9559); Console.Beep(); this.label1.Text += "\nconnected."; }
public NeutralMovement(MotionProxy motionproxy, object LockMotion, MovingSettings ms, LedsProxy ledsproxy, object LockLeds) { setting = ms; ALMotion = motionproxy; this.LockMotion = LockMotion; ALLeds = ledsproxy; this.LockLeds = LockLeds; mItems = setting.WhatToMove; }
public void Connect(string ip) { try { _motion = new MotionProxy(ip, 9559); // --------------- prepare limits -------------------------- // three floats as an ArrayList for each joint in the chain // min,max,maxNoLoadSpeedPerCycle ArrayList rightArmLimits = (ArrayList)_motion.getLimits("RArm"); ArrayList RSPitchLimits = (ArrayList)rightArmLimits[0]; _RSminPitch = (float)RSPitchLimits[0]; _RSmaxPitch = (float)RSPitchLimits[1]; ArrayList RSRollLimits = (ArrayList)rightArmLimits[1]; _RSminRoll = (float)RSRollLimits[0]; _RSmaxRoll = (float)RSRollLimits[1]; ArrayList REYawLimits = (ArrayList)rightArmLimits[2]; _REminYaw = (float)REYawLimits[0]; _REmaxYaw = (float)REYawLimits[1]; ArrayList RERollLimits = (ArrayList)rightArmLimits[3]; _REminRoll = (float)RERollLimits[0]; _REmaxRoll = (float)RERollLimits[1]; ArrayList leftArmLimits = (ArrayList)_motion.getLimits("LArm"); ArrayList LSPitchLimits = (ArrayList)leftArmLimits[0]; _LSminPitch = (float)LSPitchLimits[0]; _LSmaxPitch = (float)LSPitchLimits[1]; ArrayList LSRollLimits = (ArrayList)leftArmLimits[1]; _LSminRoll = (float)LSRollLimits[0]; _LSmaxRoll = (float)LSRollLimits[1]; ArrayList LEYawLimits = (ArrayList)leftArmLimits[2]; _LEminYaw = (float)LEYawLimits[0]; _LEmaxYaw = (float)LEYawLimits[1]; ArrayList LERollLimits = (ArrayList)leftArmLimits[3]; _LEminRoll = (float)LERollLimits[0]; _LEmaxRoll = (float)LERollLimits[1]; // give the joints some stiffness _motion.stiffnessInterpolation("RArm", 1.0f, 1.0f); _motion.stiffnessInterpolation("LArm", 1.0f, 1.0f); } catch (Exception e) { Console.Out.WriteLine("Elbow.Connect exception: " + e); } }
public NaoController(string ip) { try { this.ip = ip; proxy = new MotionProxy(ip, 9559); this.setStiffness(1.0f); } catch (Exception e) { Console.Out.WriteLine("HeadPanning.Connect exception: " + e); } }
public ToNao2(String IP, int port, Kinect kinect, int avg) { this.motioProxy = new MotionProxy(IP, port); this.memProxy = new MemoryProxy(IP, port); this.postureproxy = new RobotPostureProxy(IP, port); this.sensor = kinect; this.jointAngles = new Dictionary <string, float>(); this.avgPoint = new Dictionary <JointType, SkeletonPoint>(); this.avg = avg; this.iteration = 0; setUpjointAngleDictionary(); }
private bool Rotate(MotionProxy motionProxy, ref float actualRotation) { if (actualRotation > this.MaxRotation) { return(false); } motionProxy.moveInit(); motionProxy.moveTo(0f, 0f, this.RotationAmount); actualRotation += this.RotationAmount; return(true); }
public ToNao2(String IP, int port, Kinect kinect, int avg) { this.motioProxy = new MotionProxy(IP, port); this.memProxy = new MemoryProxy(IP, port); this.postureproxy = new RobotPostureProxy(IP, port); this.sensor = kinect; this.jointAngles = new Dictionary<string, float>(); this.avgPoint = new Dictionary<JointType, SkeletonPoint>(); this.avg = avg; this.iteration = 0; setUpjointAngleDictionary(); }
//Control LArm with all Joints public static void controlLArm(MotionProxy mp, float LSP, float LSR, float LER, float LEY, float LWY) { //Joint Controll //Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x) String[] names = { "LShoulderPitch", "LShoulderRoll", "LElbowRoll", "LElbowYaw", "LWristYaw" }; float[] newangles = { LSP, LSR, LER, LEY, LWY}; //Make sure floats are in range if (ShoulderPitch.ContainsValue(LSP) && LShoulderRoll.ContainsValue(LSR) && LElbowRoll.ContainsValue(LER) && ElbowYaw.ContainsValue(LEY)) { float fractionMaxSpeed = 0.2f; mp.setAngles(names, newangles, fractionMaxSpeed); } else return; }
public void Disconnect() { try { if (naoMotion != null) { // unsubscribe so the NAO knows we do not need data from the Motion anymore naoMotion.stiffnessInterpolation("Head", 0f, 0f); naoMotion.stiffnessInterpolation("LArm", 0f, 0f); naoMotion.stiffnessInterpolation("RArm", 0f, 0f); } } catch { } naoMotion = null; }
//PostureProxy ALMotion = null; /// <summary> /// Connects to the motion system in the NAO robot /// </summary> /// <param name="ip"> ip address of the robot </param> public void connect(string ip) { try { naoMotion = new MotionProxy(ip, 9559); // naoMotion = new ALProxy(); // give joints stiffness naoMotion.stiffnessInterpolation("Head", 0.6f, 0.6f); naoMotion.stiffnessInterpolation("Body", 0.6f, 0.6f); } catch (Exception e) { MessageBox.Show("Exception occurred, error log in C:\\NAOserver\\exception.txt"); System.IO.File.WriteAllText(@"C:\\NAOserver\\exception.txt", e.ToString()); // write exepctions to text file } }
/// <summary> /// Connects to the motion system in the NAO robot /// </summary> /// <param name="ip"> ip address of the robot </param> public void connect(string ip) { try { naoMotion = new MotionProxy(ip, 9559); // give joints stiffness naoMotion.stiffnessInterpolation("Head", 1.0f, 1.0f); naoMotion.stiffnessInterpolation("LArm", 1.0f, 1.0f); naoMotion.stiffnessInterpolation("RArm", 1.0f, 1.0f); } catch (Exception e) { MessageBox.Show("Exception occurred, error log in C:\\NAOcam\\exception.txt"); System.IO.File.WriteAllText(@"C:\\NAOcam\\exception.txt", e.ToString()); // write exepctions to text file } }
/// <summary> /// Connects to the motion system in the NAO robot /// </summary> /// <param name="ip"> ip address of the robot </param> /// public void connect(string ip) { try { naoMotion = new MotionProxy(ip, 9559); bmp = new BehaviorManagerProxy(ip, 9559); // give joints stiffness naoMotion.stiffnessInterpolation("Head", 1.0f, 1.0f); naoMotion.stiffnessInterpolation("LArm", 1.0f, 1.0f); naoMotion.stiffnessInterpolation("RArm", 1.0f, 1.0f); } catch (Exception e) { //MessageBox.Show("Exception occurred, error log in C:\\NAOcam\\exception.txt"); System.IO.File.WriteAllText(@"C:\\NAOcam\\exception.txt", e.ToString()); // write exepctions to text file } }
private int LookForMarks(MotionProxy motionProxy) { List <string> names = new List <string>(); List <float[]> times = new List <float[]>(); List <float[]> keys = new List <float[]>(); var time = 2f; MotionHelper.MoveHead(motionProxy, 0f, 0f, true); /* * names.Add("HeadYaw"); * times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.76000f, 5.04000f, 7.20000f }); * keys.Add(new[] { 0.00149f, -0.27925f, -0.34826f, -0.47124f, -0.52360f, -0.28690f, -0.00940f, -0.00940f }); */ /* // absolute * names.Add("HeadPitch"); * times.Add(new[] { 2f, 4f, 6f, 9f, 15f, 18f, 21f, 22f, 28f, 29f, 35f, 38f}); * keys.Add(new[] { 0f, 0.5f, 0f, 0f, 0f, 0f, 0f, 0.25f, 0.25f, 0.5f, 0.5f, 0f}); * * names.Add("HeadYaw"); * times.Add(new[] { 2f, 4f, 6f, 9f, 15f, 18f, 21f, 22f, 28f, 29f, 35f, 38f}); * keys.Add(new[] { 0f, 0f, 0f, -1f, 1f, 0f, -1f, -1f, 1f, 1f, -1f, 0f }); * * * var methodID = motionProxy.post.angleInterpolation(names, keys, times, true); */ // relative names.Add("HeadPitch"); times.Add(new[] { 2f, 4f, 9f, 19f, 21f, 26f, 28f, 38f, 43f }); keys.Add(new[] { 0.5f, 0f, 0f, 0f, 0.25f, 0.25f, 0.5f, 0.5f, 0f }); names.Add("HeadYaw"); times.Add(new[] { 2f, 4f, 9f, 19f, 21f, 26f, 28f, 38f, 43f }); keys.Add(new[] { 0f, 0f, -1f, 1f, 1f, -1f, -1f, 1f, 0f }); var methodID = motionProxy.post.angleInterpolation(names, keys, times, false); return(methodID); }
//Control LArm with all Joints public static void controlLArm(MotionProxy mp, float LSP, float LSR, float LER, float LEY, float LWY) { //Joint Controll //Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x) String[] names = { "LShoulderPitch", "LShoulderRoll", "LElbowRoll", "LElbowYaw", "LWristYaw" }; float[] newangles = { LSP, LSR, LER, LEY, LWY }; //Make sure floats are in range if (ShoulderPitch.ContainsValue(LSP) && LShoulderRoll.ContainsValue(LSR) && LElbowRoll.ContainsValue(LER) && ElbowYaw.ContainsValue(LEY)) { float fractionMaxSpeed = 0.2f; mp.setAngles(names, newangles, fractionMaxSpeed); } else { return; } }
/// <summary> /// Aligns the robot with the landmark /// </summary> /// <param name="motionProxy"></param> /// <param name="markInfo"></param> private void AlignWithMark(MotionProxy motionProxy, ArrayList markInfo) { // get mark shape infor var shapeInfo = LandMarkHelper.Instance.GetShapeInfo(markInfo); // get current yaw position var currentYawPosition = motionProxy.getAngles("HeadYaw", true); if (!currentYawPosition.IsNullOrEmpty() && shapeInfo[1] != null) { // move pitch to landmark MotionHelper.MoveHead(motionProxy, shapeInfo[2] as float?, null, false); // calculate robot rotation var rotation = currentYawPosition[0] + (float)shapeInfo[1]; motionProxy.moveTo(0f, 0f, rotation); // reset yaw MotionHelper.MoveHead(motionProxy, null, 0f, true); } }
//connect to the Nao robot private void ConnectToNao(string nao_ip_address, int nao_port) { bool success = true; try { TextToSpeechProxy = new TextToSpeechProxy(nao_ip_address, nao_port); BehaviorManagerProxy = new BehaviorManagerProxy(nao_ip_address, nao_port); LedsProxy = new LedsProxy(nao_ip_address, nao_port); VideoRecorderProxy = new VideoRecorderProxy(nao_ip_address, nao_port); MotionProxy = new MotionProxy(nao_ip_address, nao_port); AudioProxy = new AudioDeviceProxy(nao_ip_address, nao_port); } catch (Exception) { success = false; } ConnectButton.Dispatcher.BeginInvoke(DispatcherPriority.Normal, new UpdateInterfaceAfterConnectDelegate(UpdateUserInterfaceAfterConnect), success); }
/// <summary> /// Connects to the motion system in the NAO robot /// </summary> /// <param name="ip"> ip address of the robot </param> public void connect(string ip) { // Make sure the standard output directory exists if (!Directory.Exists("C:\\NAO Motion\\")) { Directory.CreateDirectory("C:\\NAO Motion\\"); } try { naoMotion = new MotionProxy(ip, 9559); // give joints stiffness naoMotion.stiffnessInterpolation("Head", 1.0f, 1.0f); naoMotion.stiffnessInterpolation("LArm", 1.0f, 1.0f); naoMotion.stiffnessInterpolation("RArm", 1.0f, 1.0f);; } catch (Exception e) { File.WriteAllText(@"C:\\NAO Motion\\exception.txt", e.ToString()); // Write exepctions to text file } }
public NaoController(string ip) { try { lastUpdate = null; proxy = new MotionProxy(ip, 9559); float stiffness = 1.0f; float time = 1.0f; /* proxy.stiffnessInterpolation("Head", stiffness, time); proxy.stiffnessInterpolation("LShoulderPitch", stiffness, time); proxy.stiffnessInterpolation("RShoulderPitch", stiffness, time); proxy.stiffnessInterpolation("LShoulderRoll", stiffness, time); proxy.stiffnessInterpolation("RShoulderRoll", stiffness, time); proxy.stiffnessInterpolation("LElbowYaw", stiffness, time); proxy.stiffnessInterpolation("RElbowYaw", stiffness, time); proxy.stiffnessInterpolation("LElbowRoll", stiffness, time); proxy.stiffnessInterpolation("RElbowRoll", stiffness, time); proxy.stiffnessInterpolation("RHipYawPitch", stiffness, time); proxy.stiffnessInterpolation("LHipYawPitch", stiffness, time); proxy.stiffnessInterpolation("LHipPitch", stiffness, time); proxy.stiffnessInterpolation("RHipPitch", stiffness, time); proxy.stiffnessInterpolation("LKneePitch", stiffness, time); proxy.stiffnessInterpolation("RKneePitch", stiffness, time); proxy.stiffnessInterpolation("LAnklePitch", stiffness, time); proxy.stiffnessInterpolation("RAnklePitch", stiffness, time); proxy.stiffnessInterpolation("LHipRoll", stiffness, time); proxy.stiffnessInterpolation("RHipRoll", stiffness, time); proxy.stiffnessInterpolation("LAnkleRoll", stiffness, time); proxy.stiffnessInterpolation("RAnkleRoll", stiffness, time); * */ } catch (Exception e) { Console.Out.WriteLine("HeadPanning.Connect exception: " + e); } }
public NaoController(string ip, MainController main) { try { this.main = main; this.ip = ip; prevAng = PI; proxy = new MotionProxy(ip, 9559); if (this.ip != "127.0.0.1") { tts = new TextToSpeechProxy(this.ip, 9559); } else { tts = null; } //this.setStiffness(1.0f); } catch (Exception e) { Console.Out.WriteLine("Connect exception: " + e); } }
public tracker(string ip, int port) { this.ip = ip; this.port = port; motion = new MotionProxy(ip, port); RobotPostureProxy pos = new RobotPostureProxy(ip, port); ArrayList stiff = new ArrayList(); stiff.Add(1.0F); stiff.Add(1.0F); ArrayList joint = new ArrayList(); joint.Add("Head"); motion.setStiffnesses(joint, stiff); pos.goToPosture("SitRelax", 0.6F); track = new FaceTrackerProxy(ip, port); track.setWholeBodyOn(false); track.startTracker(); Thread.Sleep(30000); track.stopTracker(); stiff.Clear(); stiff.Add(0.0F); stiff.Add(0.0F); motion.setStiffnesses(joint, stiff); }
private bool LookDown(MotionProxy motionProxy) { // get initial head pitch position var initialHeadPitchPosition = motionProxy.getAngles("HeadPitch", true); MotionHelper.MoveHead(motionProxy, this.HeadPitchStepAmount, null, false); // get current pitch position var currentHeadPitchPosition = motionProxy.getAngles("HeadPitch", true); // return true if the current position changed if (!initialHeadPitchPosition.IsNullOrEmpty() && !currentHeadPitchPosition.IsNullOrEmpty() && initialHeadPitchPosition[0] != currentHeadPitchPosition[0]) { return(true); } else { // reset pitch position MotionHelper.MoveHead(motionProxy, 0f, null, true); return(false); } }
private void init() { try { rp = new RobotPostureProxy(ip, 9559); rp.goToPosture("StandZero", 1); } catch (Exception) { MessageBox.Show("IP Adresse von Nao nicht erreichbar, prüfen Sie die Konfiguration!", "Fehler"); Application.Exit(); throw new Exception("Cannot connect to Proxy. Check IP-Configuration!"); } try { List <String> joints = new List <string> { "LArm", "RArm" }; //mp = new MotionProxy(ip, port); mpL = new MotionProxy(ip, port); mpR = new MotionProxy(ip, port); //mp.setStiffnesses(joints, 0.6f); //List<float> angles = mp.getAngles(new String[] { "RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw" } ,true); } catch (Exception) { throw new Exception("Cannot connect to Proxy"); } RArm = new RArm(mpR); LArm = new LArm(mpL); }
/// <summary> /// !!!Tested only with simulator!!! /// /// Conclusion: /// when two proxy instances in two threads operates different joints, /// they are executed in the same time. /// /// when two proxy instances in two threads operates the same joints at the same time, /// they are scheduled to be executed one by one. /// /// SOAP error will occur if two threads operates /// the same instance of MotionProxy at the same time! /// </summary> public void TestMotionProxies() { MotionProxy mp1 = new MotionProxy(this.IP, Port); MotionProxy mp2 = new MotionProxy(this.IP, Port); List<string> jn1 = new List<string>(){"RShoulderPitch", "RShoulderRoll"}; List<float> jv1 = new List<float>(){0.0f, -1.0f}; List<float> jv3 = new List<float>() { -0.5f, 0.25f }; List<float> t1 = new List<float>() { 1.0f, 1.0f }; List<string> jn2 = new List<string>() { "RHipPitch", "RHipRoll" }; List<float> jv2 = new List<float>() { -0.5f, -0.5f }; List<float> t2 = new List<float>() { 1.0f, 1.0f }; BackgroundWorker bgw1 = new BackgroundWorker(); BackgroundWorker bgw2 = new BackgroundWorker(); bgw1.DoWork += delegate { mp1.angleInterpolation(jn1, jv1, t1, true); }; bgw2.DoWork += delegate { // No problem so far; executed simultaneously mp2.angleInterpolation(jn2, jv2, t2, true); // No problem so far; scheduled one by one; order is not definite //mp2.angleInterpolation(jn1, jv3, t2, true); // SOAP error. sometimes caused at bgw1, sometimes bgw2 // some times no errors occurred, probably because // the two threads were not operating at the same time coincidently //mp1.angleInterpolation(jn2, jv2, t2, true); // SOAP error. sometimes caused at bgw1, sometimes bgw2 // some times no errors occurred, probably because // the two threads were not operating at the same time coincidently //mp1.angleInterpolation(jn1, jv3, t2, true); }; bgw1.RunWorkerAsync(); bgw2.RunWorkerAsync(); }
///<summary> /// Creates proxes to be used by this class only. /// </summary> private void CreateMyProxies() { motion = Proxies.GetProxy <MotionProxy>(); battery = Proxies.GetProxy <BatteryProxy>(); memory = Proxies.GetProxy <MemoryProxy>(); }
void DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e) { if (frameCount % 3 == 0) // increase the number after the mod to increase speed. in its current setup it skips 2 out of 3 frames and only processes the 3rd. { DepthImageFrame imageFrame = e.OpenDepthImageFrame(); if (imageFrame != null) // makes sure a frame exists. { short[] pixelData = new short[imageFrame.PixelDataLength]; // Creates an array the size of the image frame pixel length imageFrame.CopyPixelDataTo(pixelData); // copies pixel data to the pixel data array. int temp = 0; // initalizes depth. int i = 0; // intializes the count. for (int y = 0; y < 480; y += pointDen)// iterates through each row. { for (int x = 0; x < 640; x += pointDen) //iterates through each column in the row. { temp = ((ushort)pixelData[x + y * 640])>> 4; ((TranslateTransform3D)points[i].Transform).OffsetZ = temp; // sets the triangles z offset. if (temp<400 && x >= 200 && x <= 400) { points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Red)); // throws a read warning. rotate = -.2f; speed = .0f; REDALERT = true; } else if (temp <500 && x >= 200 && x<= 400 && !REDALERT) { points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Yellow)); // sets it to be yellow when it is in a cetain range. objectDetected = true; av += x; count++; // 3 triagles that are yellow, what x value? x value is on left side, rotate right. } else { points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Blue)); // paints everyhting else blue. } i++; } } if (objectDetected) { av = av / count; av = av - 320; if (av >= 0 && count > 5) { rotate = -.2f; speed = .5f; } else if ( count > 5) { rotate = .2f; speed = .5f; } } if(pathChecked == true) { if (mp == null) { mp = new MotionProxy(ipBox.Text, 9559); } if (mp != null) { mp.setWalkTargetVelocity(speed, 0, rotate, .5f); } } else if (pathChecked == false) { if (mp == null) { mp = new MotionProxy(ipBox.Text, 9559); } if (mp != null) { mp.stopMove(); } } } } speed = .6f; rotate = 0; av = 0; count = 0; objectDetected = false; REDALERT = false; pathChecked = false; frameCount++; }
public GrabWorker() { motion = Grabber.Instance.Motion; posture = Grabber.Instance.Posture; }
/// <summary> /// Discards the proxies used by this class (intended to be called when they have been disposed). /// </summary> /// <param name="ip">The IP of the Nao disconnected from.</param> /// <param name="port">The port number of the Nao disconnected from.</param> private void ResetProxies(string ip, int port) { motion = null; posture = null; }
/// <summary> /// Fetches new proxies to be used by this class. /// </summary> /// <param name="ip">The IP of the Nao connected to.</param> /// <param name="port">The port number the Nao connected to.</param> private void BuildProxies(string ip, int port) { motion = Proxies.GetProxy <MotionProxy>(); posture = Proxies.GetProxy <RobotPostureProxy>(); }
/// <summary> /// Makes the robot look left and right /// </summary> /// <param name="motionProxy"></param> /// <returns></returns> private bool LookAround(MotionProxy motionProxy, LookDirection startLookDirection, ref LookDirection currentLookDirection) { // get initial head yaw position var initialHeadYawPositionArray = motionProxy.getAngles("HeadYaw", true); if (initialHeadYawPositionArray.IsNullOrEmpty()) { // should not happen return(false); } var initialHeadYawPosition = initialHeadYawPositionArray[0]; float maxHeadYaw, step; float marginOfError = 0.01f; var maxReached = false; var directionChanged = currentLookDirection != startLookDirection; if (currentLookDirection == LookDirection.Left) { maxHeadYaw = this.MaxHeadYaw; step = this.HeadYawStepAmount; // check if it's the last movement and ajust the step if it is if (step + initialHeadYawPosition >= maxHeadYaw) { step = maxHeadYaw - initialHeadYawPosition; } // if it reached the maximum value we need to change the direction and update the step if (step - marginOfError <= 0) { maxReached = true; step = -this.HeadYawStepAmount; } } else { // if it's the right direction we need to have negative values for max and step angles maxHeadYaw = -this.MaxHeadYaw; step = -this.HeadYawStepAmount; // check if it's the last movement and ajust the step if it is if (step + initialHeadYawPosition <= maxHeadYaw) { step = maxHeadYaw - initialHeadYawPosition; } // if it reached the maximum value we need to change the direction and update the step if (step + marginOfError >= 0) { maxReached = true; step = this.HeadYawStepAmount; } } if (maxReached && directionChanged) { // no more movement allowed // reset head yaw position to 0 MotionHelper.MoveHead(motionProxy, null, 0f, true); return(false); } else { MotionHelper.MoveHead(motionProxy, null, step, maxReached); if (maxReached) { currentLookDirection = currentLookDirection == LookDirection.Left ? LookDirection.Right : LookDirection.Left; } return(true); } }
///<summary> /// Creates proxes to be used by this class only. /// </summary> private void CreateMyProxies() { motion = Proxies.GetProxy<MotionProxy>(); battery = Proxies.GetProxy<BatteryProxy>(); memory = Proxies.GetProxy<MemoryProxy>(); }
/// <summary> /// Forces this classes' proxy properties to refresh. /// </summary> private void ResetProxies(string ip, int port) { Motion = null; }
public Nao_Winkel(MotionProxy motion) { // TODO: Complete member initialization this.motion = motion; }
// Chaque client à son propre "main" public void Thread() { //on demande au client de s'authentifier sw.WriteLine("NICK!"); sw.Flush(); string nickString = null; try { nickString = sr.ReadLine(); } catch(Exception e) { Fermerconnexion(); return; } string[] commande = null; //on décortique la commande commande = GetCommande(nickString); //Le protocole dit : on doit envoyer NICK!LeNick - si ce n'est pas le cas on envoie le message d'erreur conséquent if (!commande[0].Equals("NICK") || commande[1] == null ) { //le client n'a pas fourni son nom if (commande[1] == null) { commande[1] = "Mauvaise authentification: NICK!Authentifiant attendu"; } //on se garde une trace sur la console serveur Console.WriteLine(commande[1]); //on envoie l'erreur au client Erreur(commande[1]); // on ferme la connexion Fermerconnexion(); // on sort de la méthode - tuer le thread. return; } if (!Regex.IsMatch(commande[1], @"^[a-zA-Z0-9_]+$")) { //on se garde une trace sur la console serveur Console.WriteLine(commande[1] + " ne contient pas uniquement des caractère alphanumérique et des soulignés"); //on envoie l'erreur au client Erreur("votre nom contient des caractère proscrit : uniquement A-Z a-z 0-9 _ sont authorisés"); // on ferme la connexion Fermerconnexion(); // on sort de la méthode - tuer le thread. return; } //Si le nom est déjà dans la liste, erreur foreach (Client client in listeClient) { if (client.nick.ToUpper().Equals(commande[1].ToUpper())) { Erreur("Ce nom est déjà présent"); Fermerconnexion(); return; } } //enregistrer le nom dans l'objet du client. this.nick = commande[1]; //on se garde une trace Console.WriteLine(this.nick + " vient de se connecter"); //on ajoute le client à la liste AjouterClient(this); //on repond au client avec HELO!. Ecrire("HELO!" + nick); //On doit diffuser que le client vient de se connecter DiffuserCommande(this, "JOIN"); //tant que le client est actif (l'état va changer avec la commande QUIT!) bool active = true; //tant que le socket est connecté. while (ClientConnecte() && active) { //bloquant - jusqu'à ce qu'une ligne arrive ou encore une déconnexion violente string message = null; try{ message = sr.ReadLine(); } catch (Exception e) { Console.WriteLine("message null, on déconnecte ce socket : "+e.Message); } //lorsque le socket se fait débrancher, sr.ReadLine() va débloquer et message va être à null if (message == null) { //on se garde une trace dans la console pour indiquer que le client vient de se déconnecter. Console.WriteLine(this.nick + " vient de se déconnecter"); // on demande à etre retirer de la liste; RetirerClient(this); //On doit diffuser que le client vient de se déconnecter. Diffuser(this, "QUIT", "Socket terminé"); //on ferme les tream Fermerconnexion(); //on sors de la méthode ce qui va tuer le thread. fermeture propre. return; } Console.WriteLine(" message explicite >> " + message); //maintenant que les erreurs de deconnexions ont été géré. //on décortique la commande recu. commande = GetCommande(message); switch (commande[0]) { //message d'erreur case "MSGE": { Erreur(commande[1]); break; } //message à tous case "MSGA": { if (commande[1] != null) { Diffuser(this, "MSGA", commande[1]); } else { Erreur("Le message ne peut pas être vide"); } break; } //message privé case "MSGP": { commande[1] = (commande[1] == null) ? "" : commande[1]; string[] destinataireMessage = AnalyseSousCommande(commande[1]); if (destinataireMessage[0].Length == 0) { Erreur("Aucun destinataire spéficié"); } else if (destinataireMessage[1].Length == 0) { Erreur("Aucun message spécifié"); } else { Console.WriteLine("Tentative de message privé à envoyer à: " + destinataireMessage[0]); bool envoye = false; foreach (Client client in listeClient) { if (client.nick.ToUpper().Equals(destinataireMessage[0].ToUpper())) { Ecrire("SUCC!" + commande[0]); client.Ecrire(commande[0] + "!" + this.nick + "!" + destinataireMessage[1]); envoye = true; } } if (!envoye) { this.Erreur("aucun destinataire trouvé"); } } break; } /* * Le protocol dit que le serveur doit répondre avec * LIST!nom1,nom2,nom3,nom4.....nomN * vous devez implémenter cette méthode * */ case "LIST": { StringBuilder sb = new StringBuilder(); sb.Append("LIST!"); bool append = false; foreach (Client client in listeClient) { sb.Append(client.nick + ","); append = true; } //enleve la dernière virgule; if (append) { sb.Length -= 1; } Ecrire(sb.ToString()); break; } case "QUIT": { Ecrire("SUCC!" + commande[0]); if (commande[1] == null) { Diffuser(this, commande[0], "bye bye !"); } else { Diffuser(this, commande[0], commande[1]); } active = false; break; } case "CONN": { if (naoTts == null || naoMotion == null || naoPosture == null) { Console.WriteLine("Connection à NAO"); //naoTts = new TextToSpeechProxy("192.168.2.134", 9559); naoTts = new TextToSpeechProxy("127.0.0.1", 9559); Console.WriteLine("TextToSpeechProxy connecté"); naoTts.setLanguage("FRENCH"); naoTts.setVolume(0.3f); //naoMotion = new MotionProxy("192.168.2.134", 9559); naoMotion = new MotionProxy("127.0.0.1", 9559); Console.WriteLine("MotionProxy connecté"); //naoPosture = new RobotPostureProxy("192.168.2.134", 9559); naoPosture = new RobotPostureProxy("127.0.0.1", 9559); Console.WriteLine("RobotPostureProxy connecté"); Console.WriteLine("CONN terminé"); } else Console.WriteLine("Déjà connecté"); break; } case "INIT": { if (naoTts != null && naoMotion != null && naoPosture != null) { naoMotion.setStiffnesses("Body", 1f); if (naoPosture.goToPosture("StandZero", 1f)) naoTts.say("Initialisation terminée"); Console.WriteLine("Initialisation terminée"); } else Console.WriteLine("CONN nécessaire avant INIT"); break; } case "TALK": { if (commande.Length > 1) { naoTts.say(commande[1]); Console.WriteLine("NAO TextToSpeechProxy : {0}", commande[1]); } break; } case "STIF": { if(commande.Length > 1) { try { naoMotion.setStiffnesses("Body", float.Parse(commande[1])); } catch(Exception e) { Diffuser(this, "MSGA", "La commande STIF requiert un parametre valide"); } } else { Diffuser(this, "MSGA", "La commande STIF requiert un parametre"); } break; } case "POST": { if(commande.Length == 2) { try { naoPosture.goToPosture(commande[1], 1f); } catch(Exception) { Diffuser(this, "MSGA", "Argument du POST invalide"); } } else { Diffuser(this, "MSGA", "POST requiert un argument"); } break; } case "MOVE": { if(commande[1] != null) { string[] args = commande[1].Split('!'); args = args.Select(x => x.Replace(".", ",")).ToArray(); if(args.Length > 1) { string joint = args[0]; float angle = 0; float.TryParse(args[1], out angle); float vitesse = 0.1f; try { naoMotion.setAngles(joint, angle, vitesse); } catch(Exception e) { Console.WriteLine(e); } } } break; } case "TEST": { if (commande.Length > 1) { string[] args = commande[1].Split('!'); args = args.Select(x => x.Replace(".", ",")).ToArray(); if (args.Length > 6) { string chainName = args[0]; int space = 0; float[] position = { float.Parse(args[1]), float.Parse(args[2]), float.Parse(args[3]), float.Parse(args[4]), float.Parse(args[5]), float.Parse(args[6]), }; int axisMask = 63; naoMotion.setPosition(chainName, space, position.ToList<float>(), 0.4f, axisMask); } } break; } case "RGHT": { if (commande.Length > 1) { string[] args = commande[1].Split('!'); args = args.Select(x => x.Replace(".", ",")).ToArray(); if (args.Length > 2) { float[] position = { float.Parse(args[0]), float.Parse(args[1]), float.Parse(args[2]), 0f, 0f, 0f }; float[] currPosition = naoMotion.getPosition("RArm",0,true).ToArray(); float[] deltaPosition = new float[6]; for(int i = 0; i < 6; i++) { deltaPosition[i] = (position[i] - currPosition[i]) / 10; } Diffuser(this, "MSGA", message); naoMotion.changePosition("RArm", 0, position.ToList<float>(), 0.8f, 63); } } break; } default: { Erreur("Commande non implémenté sur le serveur"); break; } } } Console.WriteLine(this.nick + " vient de se deconnecter"); // on se retire soi-même de la liste; RetirerClient(this); //on ferme les socket et les streams Fermerconnexion(); //le thread va mourrir... sniff sniff }
public Angles(MotionProxy mp, String[] joints) { currAngles = mp.getAngles(joints, true); }
public GrabWorker() { motion = Grabber.Instance.Motion; posture = Grabber.Instance.Posture; }
public SitDownExecuter(MotionProxy motionProxy, RobotPoseProxy robotPoseProxy) { this.motionProxy = motionProxy; this.robotPoseProxy = robotPoseProxy; }
/// <summary> /// Forces this classes' proxy properties to refresh. /// </summary> private void ResetProxies(string ip, int port) { Motion = null; }
private void Button_Click_1(object sender, RoutedEventArgs e) { //Initialize the motion proxy using the text in the ipBox MotionProxy mp = new MotionProxy(ipBox.Text, 9559); //Initialize the text to speech proxy using the text in the ipBox ttsp = new TextToSpeechProxy(ipBox.Text, 9559); //Initialize the robots posture proxy using the text in the ipBox RobotPostureProxy rpp = new RobotPostureProxy(ipBox.Text, 9559); //Wakes the robot up, tells the robot to stand and say hello mp.wakeUp(); rpp.goToPosture("StandInit", .5f); ttsp.say("Hola"); //Sets up lighting of the camera with a white color and directs it in the appropriate direction DirectionalLight DirLight1 = new DirectionalLight(); DirLight1.Color = Colors.White; DirLight1.Direction = new Vector3D(1, 1, 1); //Setting up the camera so that the picture we see is in a good position PerspectiveCamera Camera1 = new PerspectiveCamera(); Camera1.FarPlaneDistance = 16000; Camera1.NearPlaneDistance = 10; //Zoomed in with a 45 degree field of view Camera1.FieldOfView = 45; //50 mm away from the point cloud and centered in the middle Camera1.Position = new Point3D(320, 240, -50); //Flips the camera so that we see everything right side up Camera1.LookDirection = new Vector3D(0, 0, 1); Camera1.UpDirection = new Vector3D(0, -1, 0); //Creates a new model 3D group to hold the sets of 3D models Model3DGroup modelGroup = new Model3DGroup(); //builds the 3D model int i = 0; for (int y = 0; y < 480; y += pointDen) { for (int x = 0; x < 640; x += pointDen) { points[i] = Triangle(x, y, pointDen); points[i].Transform = new TranslateTransform3D(0, 0, 0); modelGroup.Children.Add(points[i]); i++; } } //Adds the directional light to the model group modelGroup.Children.Add(DirLight1); ModelVisual3D modelsVisual = new ModelVisual3D(); modelsVisual.Content = modelGroup; Viewport3D myViewport = new Viewport3D(); myViewport.IsHitTestVisible = false; myViewport.Camera = Camera1; myViewport.Children.Add(modelsVisual); canvas1.Children.Add(myViewport); myViewport.Height = canvas1.Height; myViewport.Width = canvas1.Width; Canvas.SetTop(myViewport, 0); Canvas.SetLeft(myViewport, 0); sensor = KinectSensor.KinectSensors[0]; sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30); sensor.DepthFrameReady += DepthFrameReady; sensor.Start(); }
private void Button_Click_1(object sender, RoutedEventArgs e) { //Initialize the motion proxy using the text in the ipBox MotionProxy mp = new MotionProxy(ipBox.Text, 9559); //Initialize the text to speech proxy using the text in the ipBox ttsp = new TextToSpeechProxy(ipBox.Text, 9559); //Initialize the robots posture proxy using the text in the ipBox RobotPostureProxy rpp = new RobotPostureProxy(ipBox.Text, 9559); //Wakes the robot up, tells the robot to stand and say hello mp.wakeUp(); rpp.goToPosture("StandInit", .5f); ttsp.say("Hola"); //Sets up lighting of the camera with a white color and directs it in the appropriate direction DirectionalLight DirLight1 = new DirectionalLight(); DirLight1.Color = Colors.White; DirLight1.Direction = new Vector3D(1, 1, 1); //Setting up the camera so that the picture we see is in a good position PerspectiveCamera Camera1 = new PerspectiveCamera(); Camera1.FarPlaneDistance = 16000; Camera1.NearPlaneDistance = 10; //Zoomed in with a 45 degree field of view Camera1.FieldOfView = 45; //50 mm away from the point cloud and centered in the middle Camera1.Position = new Point3D(320, 240, -50); //Flips the camera so that we see everything right side up Camera1.LookDirection = new Vector3D(0, 0, 1); Camera1.UpDirection = new Vector3D(0, -1, 0); //Creates a new model 3D group to hold the sets of 3D models Model3DGroup modelGroup = new Model3DGroup(); //builds the 3D model int i = 0; for (int y = 0; y < 480; y += pointDen) { for (int x = 0; x < 640; x += pointDen) { points[i] = Triangle(x, y, pointDen); points[i].Transform = new TranslateTransform3D(0, 0, 0); modelGroup.Children.Add(points[i]); i++; } } //Adds the directional light to the model group modelGroup.Children.Add(DirLight1); ModelVisual3D modelsVisual = new ModelVisual3D(); modelsVisual.Content = modelGroup; Viewport3D myViewport = new Viewport3D(); myViewport.IsHitTestVisible = false; myViewport.Camera = Camera1; myViewport.Children.Add(modelsVisual); canvas1.Children.Add(myViewport); myViewport.Height = canvas1.Height; myViewport.Width = canvas1.Width; Canvas.SetTop(myViewport, 0); Canvas.SetLeft(myViewport, 0); sensor = KinectSensor.KinectSensors[0]; sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30); sensor.DepthFrameReady += DepthFrameReady; sensor.Start(); }
void DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e) { if (frameCount % 3 == 0) // increase the number after the mod to increase speed. in its current setup it skips 2 out of 3 frames and only processes the 3rd. { DepthImageFrame imageFrame = e.OpenDepthImageFrame(); if (imageFrame != null) // makes sure a frame exists. { short[] pixelData = new short[imageFrame.PixelDataLength]; // Creates an array the size of the image frame pixel length imageFrame.CopyPixelDataTo(pixelData); // copies pixel data to the pixel data array. int temp = 0; // initalizes depth. int i = 0; // intializes the count. for (int y = 0; y < 480; y += pointDen) // iterates through each row. { for (int x = 0; x < 640; x += pointDen) //iterates through each column in the row. { temp = ((ushort)pixelData[x + y * 640]) >> 4; ((TranslateTransform3D)points[i].Transform).OffsetZ = temp; // sets the triangles z offset. if (temp < 400 && x >= 200 && x <= 400) { points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Red)); // throws a read warning. rotate = -.2f; speed = .0f; REDALERT = true; } else if (temp < 500 && x >= 200 && x <= 400 && !REDALERT) { points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Yellow)); // sets it to be yellow when it is in a cetain range. objectDetected = true; av += x; count++; // 3 triagles that are yellow, what x value? x value is on left side, rotate right. } else { points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Blue)); // paints everyhting else blue. } i++; } } if (objectDetected) { av = av / count; av = av - 320; if (av >= 0 && count > 5) { rotate = -.2f; speed = .5f; } else if (count > 5) { rotate = .2f; speed = .5f; } } if (pathChecked == true) { if (mp == null) { mp = new MotionProxy(ipBox.Text, 9559); } if (mp != null) { mp.setWalkTargetVelocity(speed, 0, rotate, .5f); } } else if (pathChecked == false) { if (mp == null) { mp = new MotionProxy(ipBox.Text, 9559); } if (mp != null) { mp.stopMove(); } } } } speed = .6f; rotate = 0; av = 0; count = 0; objectDetected = false; REDALERT = false; pathChecked = false; frameCount++; }