/// <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; }