private void checkPlacementGesture(Body body)
{
HandState state = body.HandRightState;
if (isTrackingItem)
{
if (state.Equals(HandState.Open))
{
stageOne = true;
}
if (stageOne && state.Equals(HandState.Closed))
{
stageTwo = true;
}
if (stageTwo && state.Equals(HandState.Open))
{
stageThree = true;
}
}
if (stageThree && isItemPlaceable)
{
trackingEnabled = false;
}
}
public void OutputSound(HandState left, HandState right, float leftX, float leftY, float rightX, float rightY)
{
//if(left != lastLeftState)
//{
if (left.Equals(HandState.Closed))
{
if (!NoteOn)
{
outputDevice.SendNoteOn(Channel.Channel1, Pitch.C4, 80);
NoteOn = true;
}
try
{
outputDevice.SendPitchBend(Channel.Channel1, (int)scaleBetween(leftX, (float)-0.5, (float)0.5));
outputDevice.SendControlChange(Channel.Channel1, Midi.Control.Volume, (int)scaleBetween(leftY, (float)-0.3, (float)0.3, 30, 127));
}
catch (Exception e)
{
}
}
// else if (left.Equals(HandState.Open))
else
{
outputDevice.SendControlChange(Channel.Channel1, Midi.Control.AllNotesOff, 0);
NoteOn = false;
}
//}
}
private WinnerResult EvaluateResult(HandState playerHandState, HandState enemyHandState)
{
// The player loses if no hand is seen
if (playerHandState == HandState.UNKNOWN)
{
return(WinnerResult.ENEMY);
}
if (playerHandState.Equals(enemyHandState))
{
return(WinnerResult.TIE);
}
// Yes, this needs refactoring... :)
if (playerHandState == HandState.PAPER)
{
switch (enemyHandState)
{
case HandState.POINTER:
return(WinnerResult.ENEMY);
case HandState.ROCK:
return(WinnerResult.PLAYER);
}
}
if (playerHandState == HandState.POINTER)
{
switch (enemyHandState)
{
case HandState.PAPER:
return(WinnerResult.PLAYER);
case HandState.ROCK:
return(WinnerResult.ENEMY);
}
}
if (playerHandState == HandState.ROCK)
{
switch (enemyHandState)
{
case HandState.PAPER:
return(WinnerResult.ENEMY);
case HandState.POINTER:
return(WinnerResult.PLAYER);
}
}
return(0);
}
/// <summary>
/// 姿态检测并处理
/// </summary>
/// <param name="position">舵机动作数组</param>
public bool PoseDetect(int[] position, HandState hand)
{
int r = position[(int)servos.HipRight]; //右髋舵机值
int l = position[(int)servos.HipLeft]; //左髋舵机值
int t = position[(int)servos.ThighLeft]; //左大腿舵机值
int p = position[(int)servos.ThighRight]; //右大腿舵机值
string PWM;
if (HandState_p.Equals(""))//如果手部上次为空，即第一次进入
{
HandState_p = hand;
return(false); //返回
}
else if (HandState_p == HandState.Lasso && hand == HandState.Closed) //如果上次剪刀手，这次握拳，则停止前进
{
if (_IsWalk == true) //正在前行
{
if (_IsSquat == false) //未下蹲
{
_IsWalk = false;
Thread thread_send = new Thread(new ThreadStart(new Action(() =>
{
PWM = "p" + (char)pose.walk_stop + toPWM(position) + "\r\n";//停止前进
Console.WriteLine("PWM:" + PWM.Length);
PWM_Send(PWM);
PWM_Send(PWM);
Console.WriteLine(PWM);
Thread.Sleep(1000);
})));
thread_send.Start();
//LegSendComplete();
HandState_p = hand;
return(true);
}
}
}
else if ((HandState_p == HandState.Closed && hand == HandState.Lasso) || (HandState_p == HandState.Closed && hand == HandState.Lasso)) //如果上次打开，这次剪刀手
{ //或者上次握拳，这次剪刀手，一直前进
if (_IsWalk == false)
{
if (_IsSquat == false)//未下蹲
{
_IsWalk = true;
Thread thread_send = new Thread(new ThreadStart(new Action(() =>
{
PWM = "p" + (char)pose.walk_always + toPWM(position) + "\r\n";//一直前进
Console.WriteLine("PWM:" + PWM.Length);
PWM_Send(PWM);
Console.WriteLine(PWM);
Thread.Sleep(1000);
})));
thread_send.Start();
//LegSendComplete();
HandState_p = hand;
return(true);
}
}
}
#region 版本代码
// if (hand == HandState.Closed)//石头即停止前进(手势，石头剪刀布)
// {
// if (_IsWalk == false)
// // if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
// if (_IsSquat == false)//未下蹲
// {
// _IsWalk = true;
// Thread thread_send = new Thread(new ThreadStart(new Action(() =>
// {
// PWM = "p" + (char)pose.walk_always + toPWM(position) + "\r\n";//一直前进
// Console.WriteLine("PWM:" + PWM.Length);
// PWM_Send(PWM);
// Console.WriteLine(PWM);
// Thread.Sleep(1000);
// })));
// thread_send.Start();
// //LegSendComplete();
// return true;
// }
// }
// if (hand == HandState.Lasso)//剪刀即一直前进
// {
// if(_IsWalk == false)
//// if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
// if (_IsSquat == false)//未下蹲
// {
// _IsWalk = true;
// Thread thread_send = new Thread(new ThreadStart(new Action(() =>
// {
// PWM = "p" + (char)pose.walk_always + toPWM(position) + "\r\n";//一直前进
// Console.WriteLine("PWM:" + PWM.Length);
// PWM_Send(PWM);
// Console.WriteLine(PWM);
// Thread.Sleep(1000);
// })));
// thread_send.Start();
// //LegSendComplete();
// return true;
// }
// }
#endregion
if (p >= 110 && p < 150) //右脚在正常站立范围
{
if (_IsSquat == true) //只有当前状态为下蹲状态时才起立
{
_IsSquat = false;
LegStand(position);//起立
//Thread thread_send = new Thread(new ThreadStart(new Action(() =>
//{
// PWM = "p" + (char)pose.stand_up + toPWM(position) + "\r\n";//起立
// Console.WriteLine("PWM:" + PWM.Length);
// PWM_Send(PWM);
// Console.WriteLine(PWM);
// Thread.Sleep(1000);
//})));
//thread_send.Start();
//LegSendComplete();
return(true);
}
#region 版本代码
//else if (_IsLiftRight == true && _IsLiftLeft == false)//当处于右抬脚状态时
//{
// _IsLiftRight = false;
// Thread thread_send = new Thread(new ThreadStart(new Action(() =>
// {
// PWM = "p" + (int)pose.drop_right + toPWM(position) + "\r\n";//右放脚
// Console.WriteLine("PWM:" + PWM.Length);
// PWM_Send(PWM);
// Console.WriteLine(PWM);
// Thread.Sleep(1000);
// })));
// thread_send.Start();
// LegSendComplete();
// return true;
//}
#endregion
}
#region 版本代码
//if (p >= 150 && p < 170)//右抬脚
//{
// if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
// if (_IsSquat == false)//未下蹲
// {
// _IsLiftRight = true;
// Thread thread_send = new Thread(new ThreadStart(new Action(() =>
// {
// PWM = "p" + (int)pose.lift_right + toPWM(position) + "\r\n";//右抬脚
// Console.WriteLine("PWM:" + PWM.Length);
// PWM_Send(PWM);
// Console.WriteLine(PWM);
// Thread.Sleep(1000);
// })));
// thread_send.Start();
// LegSendComplete();
// return true;
// }
//}
//if (t > 110)
//{
// if (_IsSquat == false)//未下蹲
// if (_IsLiftLeft == true && _IsLiftRight == false)//当处于抬脚状态时
// {
// _IsLiftRight = false;
// Thread thread_send = new Thread(new ThreadStart(new Action(() =>
// {
// PWM = "p" + (int)pose.drop_left + toPWM(position) + "\r\n";//左放脚
// Console.WriteLine("PWM:" + PWM.Length);
// PWM_Send(PWM);
// Console.WriteLine(PWM);
// Thread.Sleep(1000);
// })));
// thread_send.Start();
// LegSendComplete();
// return true;
// }
//}
//if (t <= 110 && t > 50)
//{
// if (_IsSquat == false)//未下蹲
// if (_IsLiftLeft == false && _IsLiftRight == false)//未抬脚
// {
// _IsLiftLeft = true;
// Thread thread_send = new Thread(new ThreadStart(new Action(() =>
// {
// PWM = "p" + (int)pose.lift_left + toPWM(position) + "\r\n";//左抬脚
// Console.WriteLine("PWM:" + PWM.Length);
// PWM_Send(PWM);
// Console.WriteLine(PWM);
// Thread.Sleep(1000);
// })));
// thread_send.Start();
// LegSendComplete();
// return true;
// }
//}
#endregion
if (p >= 170 && p < 200) //前行
{
// if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
if (_IsSquat == false) //未下蹲
{
Thread thread_send = new Thread(new ThreadStart(new Action(() =>
{
PWM = "p" + (int)pose.walk_front + toPWM(position) + "\r\n";
Console.WriteLine("PWM:" + PWM.Length);
PWM_Send(PWM);
Console.WriteLine(PWM);
Thread.Sleep(1000);
})));
thread_send.Start();
//LegSendComplete();
return(true);
}
}
if (p >= 200 && p < 250)
{
// if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
if (_IsSquat == false) //只有当前状态为站立状态时才下蹲
{
LegSquat(position); //下蹲
_IsSquat = true;
//Thread thread_send = new Thread(new ThreadStart(new Action(() =>
//{
// PWM = "p" + (char)pose.squat + toPWM(position) + "\r\n";//起立
// Console.WriteLine("PWM:" + PWM.Length);
// PWM_Send(PWM);
// Console.WriteLine(PWM);
// Thread.Sleep(1000);
//})));
//thread_send.Start();
//LegSendComplete();
return(true);
}
}
if (l > Constants.left_shift_threshold && l < Constants.left_turn_threshold)//左行一步
{
// if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
if (_IsSquat == false) //未下蹲
{
Thread thread_send = new Thread(new ThreadStart(new Action(() =>
{
PWM = "p" + (int)pose.walk_left + toPWM(position) + "\r\n";
Console.WriteLine("PWM:" + PWM.Length);
PWM_Send(PWM);
Console.WriteLine(PWM);
Thread.Sleep(1000);
})));
thread_send.Start();
//LegSendComplete();
return(true);
}
}
if (l > Constants.left_turn_threshold)//左转
{
// if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
if (_IsSquat == false) //未下蹲
{
Thread thread_send = new Thread(new ThreadStart(new Action(() =>
{
PWM = "p" + (int)pose.turn_left + toPWM(position) + "\r\n";
Console.WriteLine("PWM:" + PWM.Length);
PWM_Send(PWM);
Console.WriteLine(PWM);
Thread.Sleep(1000);
})));
thread_send.Start();
//LegSendComplete();
return(true);
}
}
if (r < Constants.right_shift_threshold && r > Constants.right_turn_threshold)//右行一步
{
// if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
if (_IsSquat == false) //未下蹲
{
Thread thread_send = new Thread(new ThreadStart(new Action(() =>
{
PWM = "p" + (int)pose.walk_right + toPWM(position) + "\r\n";
Console.WriteLine("PWM:" + PWM.Length);
PWM_Send(PWM);
Console.WriteLine(PWM);
Thread.Sleep(1000);
})));
thread_send.Start();
//LegSendComplete();
return(true);
}
}
if (r < Constants.right_turn_threshold)//右转
{
// if (_IsLiftLeft == false && _IsLiftLeft == false)//未抬脚
if (_IsSquat == false) //未下蹲
{
Thread thread_send = new Thread(new ThreadStart(new Action(() =>
{
PWM = "p" + (char)pose.turn_right + toPWM(position) + "\r\n";
Console.WriteLine("PWM:" + PWM.Length);
PWM_Send(PWM);
PWM_Send(PWM);
Console.WriteLine(PWM);
Thread.Sleep(1000);
})));
thread_send.Start();
//LegSendComplete();
return(true);
}
}
LegSendComplete();
return(false);
}