/// <summary>
/// Converts SSL detection Robot to Robot Info. Normally to use the location of robot in functions.
/// </summary>
/// <param name="robot"> SSL Detection robot to be converted.</param>
/// <returns> returns the converted robot of type RobotInfo.</returns>
public IRobotInfo RobotToIRobotInfo(SSL_DetectionRobot robot)
{
IRobotInfo newRobot = new RobotParameters();
newRobot.X = robot.x;
newRobot.Y = robot.y;
newRobot.W = robot.orientation;
return newRobot;
//return new IRobotInfo(robot.x, robot.y, robot.orientation);
}
public IRobotInfo ComputeExclusive(IRobotInfo target, SSL_DetectionRobot current)
{
if (_repReference == null)
throw new NullReferenceException("The controller needs to read data through functions assigned to the delegates Read and Write. Assign them a method.");
IRobotInfo Reference = target;
SSL_DetectionRobot Feedback = current;
double pvx = (double)Math.Round((decimal)Feedback.x, 0, MidpointRounding.AwayFromZero);
double pvy = (double)Math.Round((decimal)Feedback.y, 0, MidpointRounding.AwayFromZero);
double pvw = (double)Math.Round((decimal)Feedback.orientation, 5, MidpointRounding.AwayFromZero);
float tempRobotAngle = Feedback.orientation;
double spx = (double)Math.Round((decimal)Reference.X, 0, MidpointRounding.AwayFromZero);
double spy = Reference.Y;
double spw = Reference.W;
#region 180 Degree Error Correction
if ((spw - pvw) < -Math.PI)
{ spw += (2 * Math.PI); }
else if ((spw - pvw) > Math.PI)
{ pvw += (2 * Math.PI); }
#endregion
pvx = Clamp(pvx, pvMinx, pvMaxx);
pvx = ScaleValue(pvx, pvMinx, pvMaxx, -1.0f, 1.0f);
pvy = Clamp(pvy, pvMiny, pvMaxy);
pvy = ScaleValue(pvy, pvMiny, pvMaxy, -1.0f, 1.0f);
pvw = Clamp(pvw, pvMinw, pvMaxw);
pvw = ScaleValue(pvw, pvMinw, pvMaxw, -1.0f, 1.0f);
spx = Clamp(spx, pvMinx, pvMaxx);
spx = ScaleValue(spx, pvMinx, pvMaxx, -1.0f, 1.0f);
spy = Clamp(spy, pvMiny, pvMaxy);
spy = ScaleValue(spy, pvMiny, pvMaxy, -1.0f, 1.0f);
spw = Clamp(spw, pvMinw, pvMaxw);
spw = ScaleValue(spw, pvMinw, pvMaxw, -1.0f, 1.0f);
double errx = spx - pvx;
double erry = spy - pvy;
double errw = spw - pvw;
double pTermx = errx * kpx;
double iTermx = 0.0f;
double dTermx = 0.0f;
double pTermy = erry * kpy;
double iTermy = 0.0f;
double dTermy = 0.0f;
double pTermw = errw * kpw;
double iTermw = 0.0f;
double dTermw = 0.0f;
double partialSumx = 0.0f;
DateTime nowTimex = DateTime.Now;
double partialSumy = 0.0f;
DateTime nowTimey = DateTime.Now;
double partialSumw = 0.0f;
DateTime nowTimew = DateTime.Now;
if (lastUpdatex != null)
{
double dTx = (nowTimex - lastUpdatex).TotalSeconds;
if (pvx >= pvMinx && pvx <= pvMaxx)
{
partialSumx = errSumx + dTx * errx;
iTermx = kix * partialSumx;
}
if (dTx != 0.0f)
dTermx = kdx * (pvx - lastPVx) / dTx;
}
if (lastUpdatey != null)
{
double dTy = (nowTimey - lastUpdatey).TotalSeconds;
if (pvy >= pvMiny && pvy <= pvMaxy)
{
partialSumy = errSumy + dTy * erry;
iTermy = kiy * partialSumy;
}
if (dTy != 0.0f)
dTermy = kdy * (pvy - lastPVy) / dTy;
}
if (lastUpdatew != null)
{
double dTw = (nowTimew - lastUpdatew).TotalSeconds;
if (pvw >= pvMinw && pvw <= pvMaxw)
{
partialSumw = errSumw + dTw * errw;
iTermw = kiw * partialSumw;
}
if (dTw != 0.0f)
dTermw = kdw * (pvw - lastPVw) / dTw;
}
lastUpdatex = nowTimex;
errSumx = partialSumx;
lastPVx = pvx;
lastUpdatey = nowTimey;
errSumy = partialSumy;
lastPVy = pvy;
lastUpdatew = nowTimew;
errSumw = partialSumw;
lastPVw = pvw;
double outRealx = pTermx + iTermx + dTermx;
double outRealy = pTermy + iTermy + dTermy;
double outRealw = pTermw + iTermw + dTermw;
#region Rotation Transform for angle Compensation
PointF temp = RotatePoint(new PointF((float)outRealx, (float)outRealy), new PointF(0, 0), -tempRobotAngle);
outRealx = temp.X;
outRealy = temp.Y;
#endregion
outRealx = Clamp(outRealx, -1.0f, 1.0f);
outRealx = ScaleValue(outRealx, -1.0f, 1.0f, outMinx, outMaxx);
outRealy = Clamp(outRealy, -1.0f, 1.0f);
outRealy = ScaleValue(outRealy, -1.0f, 1.0f, outMiny, outMaxy);
outRealw = Clamp(outRealw, -1.0f, 1.0f);
outRealw = ScaleValue(outRealw, -1.0f, 1.0f, outMinw, outMaxw);
//Write it out to the world
IRobotInfo roboparams = Reference;
roboparams.Id = _id;
roboparams.XVelocity = (float)outRealx;
roboparams.YVelocity = (float)outRealy;
roboparams.WVelocity = (float)outRealw;
return roboparams;
}
