//Initialization
private void Start()
{
SceneTransi.Instance.Transi(false, null);
currentStatus = e_status.Default;
DontDestroyOnLoad(this);
Connect();
}
/// <summary>
/// To enable and proceed the calculation of moving toward a target
/// </summary>
/// <param name="x">The x of the target</param>
/// <param name="y">The y of the target</param>
/// <param name="theta">The bearings of the target</param>
public static void NewTarget(Single x, Single y, Single theta, Single range)
{
target.X = x;
target.Y = y;
target.Theta = theta;
target_range = range;
status = e_status.HasTask;
StopVehicle = false;
}
private static void MainLoop()
{
Single[] k = new Single[2] { 0.85f, 0.4f };
Single diff_dist;
Single diff_angle;
Single tmpSingle1, tmpSingle2;
Single Vcross;
double deviation, deviation_old, d_deviation;
double a, b;
double tmpDouble1, tmpDouble2;
int tmpInt;
s_position Pose_old;
s_position Vt;
s_position Vr;
short back_count = 0;
short lock_count = 0;
short hit_count = 0;
short mark_count = 0;
status = e_status.None;
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
deviation = 0;
deviation_old = 0;
while (true)
{
hpcounter1.Start();
#region Check status
if (Vehicle.Bumper == 0xFF)
{
hit_count++;
back_count = 20;
}
ob.save_sensor_reading(Vehicle.sonic);
//if (ob.HasObstacle) status = status | e_status.HasObstacle;
//else status = status & e_status.NoObstacle;
#endregion
#region Mode 1 : Force stop
if (StopVehicle) goto Wait;
#endregion
#region Mode 2 : Move forward
if(PureMove)
{
if(back_count>0)
{
back_count--;
hit_count = 0;
speed = -30;
turn = 0;
}
else if(ob.HasObstacle)
{
ob.avoid(turn,0);
speed = (short)ob.OutSpeed;
turn = (short)ob.OutTurn;
}
else
{
speed = 60;
turn = 0;
}
if (ControlEvent != null) ControlEvent(null, EventArgs.Empty);
goto Wait;
}
#endregion
#region Mode 3 : Move to target
if (status == e_status.HasTask)
{
#region Get a new task, need to initial
hit_count = 0;
back_count = 0;
MakeTurn(target.Theta);
MakeTurn(target.Theta);
ForwardOnly(1);
status = status | e_status.Initialized;
#endregion
}
if ((status & e_status.Moving) >0)
{
#region diff_dist, diff_angle and check arrival
// calculate distance difference and check if arrived to the target
diff_dist = (Single)Math.Sqrt((target.X - Pose.X) * (target.X - Pose.X) + (target.Y - Pose.Y) * (target.Y - Pose.Y));
if (diff_dist < target_range) status = status | e_status.Arrived;
// calculate angle difference
diff_angle = (Single)(Math.Atan2((target.Y - Pose.Y), (target.X - Pose.X)) * 180f / 3.14f) - Pose.Theta;
if (diff_angle > 180) diff_angle = diff_angle - 360;
else if (diff_angle < -180) diff_angle = diff_angle + 360;
#endregion
if ((status & e_status.Arrived)>0)
{
speed = 0;
turn = 0;
status = status & e_status.NoMoving;
}
else if (back_count > 0)
{
back_count--;
if (hit_count == 1)
{
if (back_count == 0)
{
hit_count++;
}
else
{
speed = -35;
turn = 0;
}
}
else
{
if (back_count == 1)
{
if (diff_angle >= 0) MakeTurn(Pose.Theta + 90);
else MakeTurn(Pose.Theta - 90);
}
else if (back_count == 0)
{
ForwardOnly(12);
hit_count = 0;
}
else
{
speed = -35;
turn = 0;
}
}
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
}
else if (ob.HasObstacle)
{
lock_count = 50;
ob.avoid(turn,diff_angle);
speed = (short)ob.OutSpeed;
turn = (short)ob.OutTurn;
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
}
else
{
if (lock_count > 0) lock_count--;
#region determine the speed of the vehicle (for reference)
if (diff_dist < 150) // if pretty close to the target
{
speed = (short)(40 + diff_dist * (max_speed - 40) / 150f);
if(speed<15) speed = 15;
max_turn = 100;
}
else // ordinary situation
{
speed = max_speed;
}
#endregion
if (lock_count == 0 && diff_dist < 150 && (diff_angle > 30 || diff_angle < -30))
{
#region if need to calibrate the bearings
Thread.Sleep(100);
MakeTurn((Single)(Math.Atan2((target.Y - Pose.Y), (target.X - Pose.X)) * 180f / 3.14f));
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
#endregion
}
else
{
#region determine the turn of the vehicle
tmpSingle1 = (Single)Math.Sqrt((Pose.X - Pose_old.X) * (Pose.X - Pose_old.X) + (Pose.Y - Pose_old.Y) * (Pose.Y - Pose_old.Y));
if (tmpSingle1 < 5)
{
// distance is too short, keep going but reduce turn angle
tmpSingle2 = turn * 0.8f;
turn = (short)tmpSingle2;
}
else
{
// calculate the cross of the two vectors
Vr.X = Pose.X - Pose_old.X;
Vr.Y = Pose.Y - Pose_old.Y;
Vt.X = target.X - Pose.X;
Vt.Y = target.Y - Pose.Y;
Vcross = Vr.X * Vt.Y - Vr.Y * Vt.X;
// calculate deviation between the vehicle and the given path
if ((target.X - Pose_old.X) > -1 && (target.X - Pose_old.X) < 1)
{
a = 0;
b = target.Y;
}
else
{
a = (target.Y - Pose_old.Y) / (target.X - Pose_old.X);
b = (target.Y * Pose_old.X - Pose_old.Y * target.X) / (Pose_old.X - target.X);
}
deviation = Math.Abs(a * Pose.X - Pose.Y + b) / Math.Sqrt(a * a + 1);
if (deviation > 20) deviation = 20;
else if (deviation < -20) deviation = -20;
// calculate compensation
d_deviation = deviation - deviation_old;
tmpDouble2 = deviation * k[0] + d_deviation * k[1];
if (Vcross < 0) tmpDouble2 = tmpDouble2 * -1;
//if (Vcross < 0 && tmpDouble2 > 5) turn = -50;
//else if (Vcross > 0 && tmpDouble2 > 5) turn = 50;
//else turn = 0;
// calculate turn
turn = (short)(turn + tmpDouble2);
if (turn > max_turn) turn = (short)max_turn;
else if (turn < max_turn * -1) turn = (short)(max_turn * -1);
OutStr = a.ToString("f2") + " , " + b.ToString("f2") + " , " + deviation.ToString("f2") + " , " + tmpDouble2.ToString("f2") + " , " + turn.ToString();
deviation_old = deviation;
if (mark_count >= 3)
{
mark_count = 0;
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
}
else mark_count++;
}
#endregion
}
}
if (ControlEvent != null) ControlEvent(null, EventArgs.Empty);
goto Wait;
}
#endregion
Wait:
hpcounter1.Stop();
tmpInt = (int)(hpcounter1.Duration * 1000f);
if (tmpInt < 90)
{
Thread.Sleep(90 - tmpInt);
}
}
}
//Loop
void FixedUpdate()
{
foreach (Message m in messages)
{
#region Join/Left Room
if (m.Type == "PlayerJoined")
{
if (!teamAttributed)
{
team = (e_teams)m.GetInt(0);
teamAttributed = true;
}
if (m.GetInt(0) == 2)
{
if (currentStatus == e_status.Default)
{
GetComponent <SelectionInputs>().StartSelection(team);
currentStatus = e_status.InSelection;
}
}
}
else if (m.Type == "PlayerLeft")
{
if (currentStatus == e_status.InSelection)
{
DisconnectMe();
}
else if (currentStatus == e_status.InGame)
{
gameplayManager.GameEnd();
currentStatus = e_status.GameEnded;
}
}
else if (m.Type == "FullRoom")
{
ListRoomsJoin(roomInfos.ToArray());
}
#endregion
#region Initialization Checks
else if (m.Type == "TeamValidated")
{
for (int i = 0; i < 6; i++)
{
heroes[i] = m.GetInt((uint)i);
}
SceneTransi.ToExecute exe = LoadGameplay;
SceneTransi.Instance.Transi(true, exe);
}
else if (m.Type == "SceneLoaded")
{
gameplayManager = FindObjectOfType <GameplayManager>();
gameplayManager.Init(this, team, heroes);
gameplayManager.OnEnnemyTurn += EnnemyTurn;
SceneTransi.Instance.Transi(false, null);
Destroy(GetComponent <SelectionInputs>());
currentStatus = e_status.InGame;
}
else if (m.Type == "PlacementValidated")
{
gameplayManager.BoardManager.PlacementValidated();
}
else if (m.Type == "GameStart")
{
gameplayManager.GameStart();
}
#endregion
#region In Game
else if (m.Type == "MoveHeroPiece")
{
int index = -1;
index = m.GetInt(0);
Vector2Int desti = new Vector2Int(m.GetInt(1), m.GetInt(2));
bool useMove = m.GetBoolean(3);
gameplayManager.BoardManager.MoveHeroPiece(index, desti, useMove);
}
else if (m.Type == "ModifyStatHeroPiece")
{
int index = -1;
index = m.GetInt(0);
e_stats key = (e_stats)m.GetInt(1);
int value = m.GetInt(2);
int duration = m.GetInt(3);
int tick = m.GetInt(4);
gameplayManager.BoardManager.ModifyStatHeroPiece(index, key, value, duration, tick);
}
else if (m.Type == "YourTurn")
{
gameplayManager.YourTurn();
}
else if (m.Type == "Eliminated")
{
gameplayManager.Eliminated((e_teams)m.GetInt(0));
if (m.GetInt(1) <= 1)
{
gameplayManager.GameEnd();
currentStatus = e_status.GameEnded;
}
}
#endregion
}
messages.Clear();
}
/// <summary>
/// To enable and proceed the calculation of moving toward a target
/// </summary>
/// <param name="x">The x of the target</param>
/// <param name="y">The y of the target</param>
/// <param name="theta">The bearings of the target</param>
public static bool NewTask(struct_PointF[] task,int numbers)
{
if (numbers >= 1)
{
target = new struct_PointF[numbers + 1];
for (int i = 0; i <= numbers; i++)
{
target[i] = task[i];
}
//Array.Copy(task, target, numbers);
TargetTotal = numbers;
TargetNow = 1;
status = e_status.HasTask;
PauseVehicle = false;
if (TargetTotal == 1) range = tg_range;
else range = wp_range;
return true;
}
else return false;
}
public static void Abort()
{
speed = 0;
turn = 0;
PauseVehicle = true;
status = e_status.Finish;
}
private static void MainLoop()
{
Single[] k = new Single[2] { 0.8f, 1f };
Single diff_dist;
Single diff_angle;
Single target_angle;
Single robot_angle;
Single tmpSingle1, tmpSingle2;
Single total_dist=1;
double Vcross, Vcross_old, d_Vcross;
double Scross;
double[] Vdot=new double[3];
double deviation, deviation_old, d_deviation;
double d_origin_path, d_current_path, d_gain;
double tmpDouble1;
double a, b;
int tmpInt;
struct_PointF Pose_old;
struct_PointF Vt;
struct_PointF Vr;
short back_count = 0;
short lock_count = 0;
short hit_count = 0;
status = e_status.None;
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
deviation = 0;
deviation_old = 0;
Vcross_old = 0;
while (true)
{
OutStr = "";
hpcounter1.Start();
#region Check status
if (Vehicle.Bumper == 0xFF)
{
hit_count++;
back_count = 20;
}
ob.save_sensor_reading(Vehicle.sonic);
//if (ob.HasObstacle) status = status | e_status.HasObstacle;
//else status = status & e_status.NoObstacle;
#endregion
#region Mode 1 : Force stop
if (PauseVehicle) goto Wait;
#endregion
#region Mode 2 : Move forward
if(PureMove)
{
status = e_status.Moving;
if(back_count>0)
{
back_count--;
hit_count = 0;
speed = -30;
turn = 0;
}
else if(ob.Escaped)
{
speed = 80;
turn = 0;
}
else if(ob.HasObstacle)
{
ob.avoid(turn);
speed = (short)ob.OutSpeed;
turn = (short)ob.OutTurn;
}
else
{
speed = 80;
turn = 0;
}
if (ControlEvent != null) ControlEvent(null, EventArgs.Empty);
goto Wait;
}
#endregion
#region Mode 3 : Move to target
if (status == e_status.HasTask)
{
#region Get a new task, need to initial
hit_count = 0;
back_count = 0;
total_dist=(Single)Math.Sqrt((target[TargetNow].X - target[TargetNow-1].X) * (target[TargetNow].X - target[TargetNow-1].X) + (target[TargetNow].Y - target[TargetNow-1].Y) * (target[TargetNow].Y - target[TargetNow-1].Y));
status = status | e_status.Initialized;
MakeTurn(target[TargetNow].Theta);
MakeTurn(target[TargetNow].Theta);
ForwardOnly(1);
#endregion
}
if ((status & e_status.Moving) >0)
{
#region diff_dist, diff_angle and check arrival
// calculate distance difference
diff_dist = (Single)Math.Sqrt((target[TargetNow].X - Pose.X) * (target[TargetNow].X - Pose.X) + (target[TargetNow].Y - Pose.Y) * (target[TargetNow].Y - Pose.Y));
// calculate angle difference
target_angle = (Single)(Math.Atan2((target[TargetNow].Y - Pose.Y), (target[TargetNow].X - Pose.X)) * 180f / 3.14f);
robot_angle = (Single)(Math.Atan2((Pose.Y - Pose_old.Y), (Pose.X - Pose_old.X)) * 180f / 3.14f);
diff_angle = target_angle - Pose.Theta;
if (diff_angle > 180) diff_angle = diff_angle - 360;
else if (diff_angle < -180) diff_angle = diff_angle + 360;
// check if arrived to the target
if (diff_dist < range)
{
if (TargetNow == TargetTotal) status = status | e_status.Arrived;
else
{
deviation = Dot2Line(target[TargetNow].X, target[TargetNow].Y, target[TargetNow + 1].X, target[TargetNow + 1].Y, Pose.X, Pose.Y);
if (deviation < 30) status = status | e_status.Arrived;
}
}
#endregion
if ((status & e_status.Arrived)>0)
{
#region actions while arrived
if (TargetNow == TargetTotal)
{
speed = 0;
turn = 0;
status = e_status.Finish;
}
else
{
status = e_status.Moving;
TargetNow++;
if (TargetNow == TargetTotal) range = tg_range;
else range = wp_range;
total_dist=(Single)Math.Sqrt((target[TargetNow].X - target[TargetNow-1].X) * (target[TargetNow].X - target[TargetNow-1].X) + (target[TargetNow].Y - target[TargetNow-1].Y) * (target[TargetNow].Y - target[TargetNow-1].Y));
MakeTurn2(target[TargetNow].Theta);
}
#endregion
}
else if (back_count > 0)
{
#region perform moving backward because has hit something
back_count--;
if (hit_count == 1)
{
if (back_count == 0)
{
hit_count++;
}
else
{
speed = -40;
turn = 0;
}
}
else
{
if (back_count == 1)
{
if (diff_angle >= 0) MakeTurn(Pose.Theta + 45);
else MakeTurn(Pose.Theta - 45);
}
else if (back_count == 0)
{
ForwardOnly(8);
hit_count = 0;
}
else
{
speed = -35;
turn = 0;
}
}
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
#endregion
}
else if(ob.Escaped)
{
speed = (short)(0.5 * max_speed);
turn = 0;
}
else if (ob.HasObstacle)
{
lock_count = 50;
ob.avoid(turn);
speed = (short)ob.OutSpeed;
turn = (short)ob.OutTurn;
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
OutStr = "avoiding";
}
else if(ob.InCorridor)
{
ob.KeepStraight(turn);
turn = (short)ob.OutTurn;
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
OutStr = ob.OutStr+","+turn.ToString();
}
else
{
if (lock_count > 0) lock_count--;
#region determine the speed of the vehicle (for reference)
if (diff_dist < 200) // if pretty close to the target
{
speed = (short)(40 + diff_dist * (max_speed - 40) / 200f);
if(speed < 15) speed = 15;
max_turn = 90;
if (TargetNow == TargetTotal)
{
if (lock_count == 0 && (diff_angle > 30 || diff_angle < -30))
{
Thread.Sleep(100);
MakeTurn((Single)(Math.Atan2((target[TargetTotal].Y - Pose.Y), (target[TargetTotal].X - Pose.X)) * 180f / 3.14f));
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
}
}
}
else // ordinary situation
{
speed = max_speed;
max_turn = 35;
}
#endregion
#region determine the turn of the vehicle
tmpSingle1 = (Single)Math.Sqrt((Pose.X - Pose_old.X) * (Pose.X - Pose_old.X) + (Pose.Y - Pose_old.Y) * (Pose.Y - Pose_old.Y));
if (tmpSingle1 < 5)
{
// distance is too short, keep going but reduce turn angle
tmpSingle2 = turn * 0.8f;
turn = (short)tmpSingle2;
}
else
{
// calculate deviation between the vehicle and the given path
// calculate the cross of the two vectors
Vr.X = Pose.X - target[TargetNow - 1].X;
Vr.Y = Pose.Y - target[TargetNow - 1].Y;
Vt.X = target[TargetNow].X - target[TargetNow - 1].X;
Vt.Y = target[TargetNow].Y - target[TargetNow - 1].Y;
Vcross = Vr.X * Vt.Y - Vr.Y * Vt.X;
if ((target[TargetNow].X - target[TargetNow - 1].X) > -5 && (target[TargetNow].X - target[TargetNow-1].X) < 5)
{
deviation = Math.Abs(Pose.X - target[TargetNow].X);
}
else
{
a = (target[TargetNow].Y - target[TargetNow - 1].Y) / (target[TargetNow-1].X - target[TargetNow].X);
deviation = Math.Abs(a * (Pose.X - target[TargetNow - 1].X) + (Pose.Y - target[TargetNow - 1].Y)) / Math.Sqrt(a * a + 1);
}
if (Vcross < 0) deviation = 0f - deviation;
d_deviation = deviation - deviation_old;
//OutStr = Pose.X.ToString()+","+ target[TargetNow - 1].X.ToString()+","+ deviation.ToString("f1");
d_origin_path = deviation * k[0] + d_deviation * k[1];
deviation_old = deviation;
// calculate deviation between the vehicle and the current path
// calculate the cross of the two vectors
Vr.X = Pose.X - Pose_old.X;
Vr.Y = Pose.Y - Pose_old.Y;
Vt.X = target[TargetNow].X - Pose.X;
Vt.Y = target[TargetNow].Y - Pose.Y;
Scross = Math.Sqrt(Vr.X * Vr.X + Vr.Y * Vr.Y) * Math.Sqrt(Vt.X * Vt.X + Vt.Y * Vt.Y);
Vcross = Vr.X * Vt.Y - Vr.Y * Vt.X;
Vcross = 15f*Vcross / Scross;
//OutStr = OutStr + "," + Vcross.ToString("f1");
d_Vcross = Vcross - Vcross_old;
d_current_path = Vcross * k[0] + d_Vcross * k[1];
Vcross_old = Vcross;
for (int i = Vdot.Length - 1; i >= 1; i--)
{
Vdot[i] = Vdot[i - 1];
}
Vdot[0] = Vr.X * Vt.X + Vr.Y * Vt.Y;
// combine 2 deviations
d_gain = 0.1 * diff_dist / total_dist;
//OutStr = d_origin_path.ToString("f1") + "," + d_current_path.ToString("f1") ;
//d_gain = 0.3;
tmpDouble1 = d_gain * d_origin_path + (1 - d_gain) * d_current_path;
if (Vdot[0] < 0 && Vdot[1] < 0)
{
Vdot[0] = 0;
MakeTurn2(diff_angle + Pose.Theta);
}
else
{
// calculate turn
//turn = (short)(tmpDouble1 + turn);
turn = (short)tmpDouble1;
if (turn > max_turn) turn = (short)max_turn;
else if (turn < max_turn * -1) turn = (short)(max_turn * -1);
if (turn > 0) turn = (short)(turn + 5);
else if (turn < 0) turn = (short)(turn - 5);
}
Pose_old.X = Pose.X;
Pose_old.Y = Pose.Y;
}
#endregion
}
if (ControlEvent != null) ControlEvent(null, EventArgs.Empty);
goto Wait;
}
#endregion
Wait:
hpcounter1.Stop();
tmpInt = (int)(hpcounter1.Duration * 1000f);
if (tmpInt < 90)
{
Thread.Sleep(90 - tmpInt);
}
}
}
