//圆轨迹跟随
public void FollowCircle(ShipData boat, HUST_1_Demo.Form1.TargetCircle circle)
{
double Err_phi = 0.0d;
// double ROBOTphi_r = 0.0d;//相对参考向的航向角或航迹角
double k = 3.5d;
double Radius = circle.Radius; //目标圆半径，将单位转为毫米
double Center_X = circle.x; //圆心坐标
double Center_Y = circle.y;
double Ye = (Math.Sqrt((boat.Fter_pos_X - Center_X) * (boat.Fter_pos_X - Center_X) + (boat.Fter_pos_Y - Center_Y) * (boat.Fter_pos_Y - Center_Y)) - Radius);
float Robot_xy = (float)(Math.Atan2(boat.Fter_pos_Y - Center_Y, boat.Fter_pos_X - Center_X) / Math.PI * 180); //航行器相对于原点的极坐标点
double Dir_R = Robot_xy - 90; //圆切线角 得出航行器和制导角的参考0向，即极坐标的x轴，两者角度都是相对该轴的角度值
if (Dir_R > 180)
{
Dir_R = Dir_R - 360;
}
else if (Dir_R < -180)
{
Dir_R = Dir_R + 360;
}
double errorRobot_Pos = boat.Ctrl_Phi - Robot_xy;
#region 获取当前制导角
float Ref_phi = (float)(-Math.Atan(Ye / k) / Math.PI * 180); //制导角（角度制°）
if ((errorRobot_Pos > 0 && errorRobot_Pos < 180) || (errorRobot_Pos < -180)) //根据船向与顺逆边界的关系，选取制导角对称与否
{
if (Ref_phi < 0)
{
Ref_phi = -180 - Ref_phi;
}
else
{
Ref_phi = 180 - Ref_phi;
}
HUST_1_Demo.Form1.cirDir = 2;//逆时针
}
#endregion
Err_phi = Ref_phi - (boat.Ctrl_Phi - Dir_R); //实际航向减去制导角的偏差
if (Err_phi > 180) //偏差角大于180°时减去360°得到负值，表示航向左偏于制导角；偏差小于180°时表示航向右偏于制导角。
{
Err_phi = Err_phi - 360;
}
else if (Err_phi < -180)
{
Err_phi = Err_phi + 360;
}
if (Math.Abs(Ye) < 0.8)
{
boat.Err_phi_In += Err_phi;
}
int R = (int)(boat.Kp * Err_phi + boat.Ki * boat.Err_phi_In);
// R = (int)(boat.Kp * Err_phi);
if (R > 32)
{
R = 32;
}
else if (R < -32)
{
R = -32;
}
R = R + 32;
this.command[3] = (byte)R;
}
/// <summary>
///
/// </summary>
/// <param name="port"></param>
/// <param name="boat"></param>
/// <param name="line"></param>
public void Closed_Control_Circle(SerialPort port, ShipData boat, HUST_1_Demo.Form1.TargetCircle circle)
{
double Err_phi = 0.0d;
double ROBOTphi_r = 0.0d;//相对参考向的航向角或航迹角
double k = 3.5d;
double Radius = circle.Radius; //目标圆半径，将单位转为毫米
double Center_X = circle.x; //圆心坐标
double Center_Y = circle.y;
double Robot_xy = 0.0d;
int Dir_flag = 0;//正逆时针标志
double Ye = (Math.Sqrt((boat.pos_X - Center_X) * (boat.pos_X - Center_X) + (boat.pos_Y - Center_Y) * (boat.pos_Y - Center_Y)) - Radius);
Robot_xy = Math.Atan2(boat.pos_Y - Center_Y, boat.pos_X - Center_X) / Math.PI * 180; //航行器相对于原点的极坐标点
double limit_ang = 0.0d; //边界，通过圆心与航行器坐标点的直径的另一半角度
#region 跟随方向的判断和 跟随标志的确定
if (Robot_xy > 0)
{
limit_ang = Robot_xy - 180;//航行器坐标点角度大于零，则判断界限角小于0，故-180，在航行器方向在界限（坐标角通过圆心的直线）右侧顺时针跟随，反之逆时针
if ((boat.phi < limit_ang) || (boat.phi > Robot_xy))
{
Dir_flag = 1;
}
}
else if (Robot_xy < 0)
{
limit_ang = Robot_xy + 180;//航行器坐标点小于零，则判断界限大于零，故+180，在界限左侧则逆时针跟随，反之顺时针
if ((boat.phi > Robot_xy) && (boat.phi < limit_ang))
{
Dir_flag = 1;
}
}
else
{
if (boat.phi > 0)
{
Dir_flag = 1;
}
}
Dir_flag = 1;
#endregion
#region 获取当前制导角
double Ref_phi = -Math.Atan(Ye / k) / Math.PI * 180; //制导角（角度制°）
if (Dir_flag == 1) //若标志为1，则要处理关于Y轴对称，确定是顺时针旋转还是逆时针旋转
{
if (Ref_phi > 0)
{
Ref_phi = 180 - Ref_phi;
}
else if (Ref_phi < 0)
{
Ref_phi = -180 - Ref_phi;
}
else
{
Ref_phi = 180;
}
}
#endregion
#region 计算参考向，以及航行器方向相对参考向的角度，以得到控制error
double Dir_R = Robot_xy - 90;//得出航行器和制导角的参考0向，即极坐标的x轴，两者角度都是相对该轴的角度值
ROBOTphi_r = boat.Control_Phi - Dir_R;
if (ROBOTphi_r > 180)
{
ROBOTphi_r = ROBOTphi_r - 360;//使得航行器相对于参考方向角度范围总在正负180之间
}
if (ROBOTphi_r < -180)
{
ROBOTphi_r = ROBOTphi_r + 360;
}
if (Ref_phi * ROBOTphi_r < -90 * 90)//处理正负180度附近的偏差值，如期望角和当前角分别是170和-170，则偏差为360-|170|-|-170|=20，而不用170+170=340， -90*90是阈值
{
Err_phi = 360 - Math.Abs(Ref_phi) - Math.Abs(ROBOTphi_r);
if (Ref_phi > 0)
{
Err_phi = -Err_phi; //若期望角为正，而实际角为负，则此时偏差值要取反
}
}
else
{
Err_phi = Ref_phi - ROBOTphi_r; //阈值内取正常偏差，当Y偏差为零时，参考角度REFphi始终为零，但是ROBOTphi_r不为零，故可以一直绕圆走。
}
#endregion
if (Math.Abs(Ye) < 0.8)
{
boat.Err_phi_In += Err_phi;
}
int R = (int)(boat.Kp * Err_phi + boat.Ki * boat.Err_phi_In);
// R = (int)(boat.Kp * Err_phi);
if (R > 32)
{
R = 32;
}
else if (R < -32)
{
R = -32;
}
R = R + 32;
command[3] = (byte)R;
Send_Command(port);
}
