void FixedUpdate()
{
Vector3 pose = kinematics.Pose;
if (FollowTarget)
{
Position = Target.position;
TargetAngle = KinematicsUtils.FixAngle(Target.rotation.eulerAngles.y * Mathf.Deg2Rad);
dx = Position.z - pose.x;
dy = -Position.x - pose.y;
}
else
{
dx = Position.x - pose.x;
dy = Position.y - pose.y;
}
p = Mathf.Sqrt(dx * dx + dy * dy);
if (p > MaxDistance)
{
dth = KinematicsUtils.FixAngle(TargetAngle - pose.z);
gamma = Mathf.Atan2(dy, dx);
alpha = KinematicsUtils.FixAngle(gamma - pose.z);
kinematics.LinearVelocity = Mathf.Min(Krho * p, MaxVelocity);
kinematics.AngularVelocity = Kp * alpha + Ki * i_alpha + Kd * (alpha - alpha_1);
alpha_1 = alpha;
i_alpha += alpha;
}
else
{
alpha_1 = 0;
i_alpha = 0;
kinematics.LinearVelocity = 0.0f;
kinematics.AngularVelocity = 0.0f;
}
}
void FixedUpdate ()
{
if(Trace != trace)
{
trace = Trace;
PoseTrace.positionCount = 0;
OdometryTrace.positionCount = 0;
lastPosePoint = Pose;
lastOdometryPoint = Odometry;
}
if(Input.GetKeyUp(KeyCode.R))
{
if(Input.GetKey(KeyCode.LeftShift))
Reset();
else
Reset(Pose);
}
if(changePose)
{
rigidbody.velocity = new Vector3(0, 0, 0);
rigidbody.angularVelocity = new Vector3(0, 0, 0);
rigidbodyOdometry.velocity = new Vector3(0, 0, 0);
rigidbodyOdometry.angularVelocity = new Vector3(0, 0, 0);
if(!reset)
{
transform.position = new Vector3(-nextPose.y, transform.position.y, nextPose.x);
OdometryTransform.position = new Vector3(-nextPose.y, transform.position.y, nextPose.x);
}
else
{
transform.position = new Vector3(-nextPose.y, 0.86f, nextPose.x);
OdometryTransform.position = new Vector3(-nextPose.y, 0.86f, nextPose.x);
}
Vector3 e = transform.rotation.eulerAngles;
if(!reset)
{
transform.localEulerAngles = new Vector3(e.x, -nextPose.z * Mathf.Rad2Deg, e.z);
OdometryTransform.localEulerAngles = new Vector3(e.x, -nextPose.z * Mathf.Rad2Deg, e.z);
}
else
{
transform.localEulerAngles = new Vector3(0, -nextPose.z * Mathf.Rad2Deg, 0);
OdometryTransform.localEulerAngles = new Vector3(0, -nextPose.z * Mathf.Rad2Deg, 0);
}
PoseTrace.positionCount = 0;
OdometryTrace.positionCount = 0;
lastPosePoint = Pose;
lastOdometryPoint = Odometry;
changePose = false;
reset = false;
return;
}
if (LowLevelControl)
{
LeftVelocity = LeftWheel.AngularSpeed * LeftRadius;
RightVelocity = RightWheel.AngularSpeed * RightRadius;
LinearVelocity = 0.5f * (LeftVelocity + RightVelocity);
AngularVelocity = (RightVelocity - LeftVelocity) / (WheelsDistance);
}
else
{
LeftVelocity = LinearVelocity - ((WheelsDistance * 0.5f) * AngularVelocity);
RightVelocity = LinearVelocity + ((WheelsDistance * 0.5f) * AngularVelocity);
LeftWheel.AngularSpeed = LeftVelocity / LeftRadius;
RightWheel.AngularSpeed = RightVelocity / RightRadius;
}
DS = Time.deltaTime * LinearVelocity;
DTH = Time.deltaTime * AngularVelocity;
DSX = DS * Mathf.Cos(DTH * 0.5f);
EDSX = DSX;
DSY = DS * Mathf.Sin(DTH * 0.5f);
EDSY = DSY;
EDTH = DTH;
if(MotionStd.magnitude > 0)
{
if(Mathf.Abs(DS) > 0)
{
EDSX += (float)KinematicsUtils.randn(0, MotionStd.x);
EDSY += (float)KinematicsUtils.randn(0, MotionStd.x);
}
if(Mathf.Abs(DTH) > 0)
{
EDTH += (float)KinematicsUtils.randn(0, MotionStd.y);
}
}
transform.Translate(EDSY,
0,
EDSX,
Space.Self);
transform.Rotate(Vector3.down,
EDTH * Mathf.Rad2Deg);
if(MotionStd.magnitude > 0)
{
OdometryTransform.Translate(DSY,
0,
DSX,
Space.Self);
OdometryTransform.Rotate(Vector3.down,
DTH * Mathf.Rad2Deg);
}
else
{
OdometryTransform.position = transform.position;
OdometryTransform.rotation = transform.rotation;
}
currentPose.Set(transform.position.z, -transform.position.x, -KinematicsUtils.FixAngle(transform.rotation.eulerAngles.y * Mathf.Deg2Rad));
odometryPose.Set(OdometryTransform.position.z, -OdometryTransform.position.x, -KinematicsUtils.FixAngle(OdometryTransform.rotation.eulerAngles.y * Mathf.Deg2Rad));
currentGPS.Set(currentPose.x + (float)KinematicsUtils.randn(0, GPSStd.x), currentPose.y + (float)KinematicsUtils.randn(0, GPSStd.y), currentPose.z + (float)KinematicsUtils.randn(0, GPSStd.z));
if(trace)
{
if((lastOdometryPoint-Odometry).magnitude > 0.2f)
{
OdometryTrace.positionCount = OdometryTrace.positionCount + 1;
OdometryTrace.SetPosition(OdometryTrace.positionCount - 1, new Vector3(OdometryTransform.position.x, OdometryTransform.position.y - 0.8f, OdometryTransform.position.z));
lastOdometryPoint = Odometry;
}
if((lastPosePoint-Pose).magnitude > 0.2f)
{
PoseTrace.positionCount = PoseTrace.positionCount + 1;
PoseTrace.SetPosition(PoseTrace.positionCount - 1, new Vector3(transform.position.x, transform.position.y - 0.8f, transform.position.z));
lastPosePoint = Pose;
}
}
}
