/// <summary>
/// Changes the angle of the pendulums and calculates the corrections for the feedback controllers.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void ChangeAngle(object sender, ElapsedEventArgs e)
{
if (DateTime.Now.Subtract(dateTime).TotalSeconds > WaitTimeForCalculation)
{
if (!initializeFeedbackControllers)
{
pid = new PID(kp, ki, kd, maxOutput);
adrc = new ADRC_PD(r, c, b, hModifier, kp, kd, maxOutput);
this.BeginInvoke((Action)(() =>
{
label1.Text = "Correction State: True";
}));
initializeFeedbackControllers = true;
}
if (useADRC)
{
currentOutput = adrc.Calculate(SetPoint, currentAngle);
}
else
{
currentOutput = pid.Calculate(SetPoint, currentAngle);
}
fileWriter.WriteLine(counter + "," + currentOutput + "," + currentAngle);
counter++;
PreviousAngles.Add((float)currentAngle);
PreviousOutputs.Add((float)currentOutput);
if (counter > FourierTransform.FourierMemory)
{
PreviousAngles.RemoveAt(0);
PreviousOutputs.RemoveAt(0);
}
}
Random rand = new Random();
double noise = rand.NextDouble() * NoiseFactor * (rand.Next(0, 1) * 2 - 1);
double invertedPendulumAngle = 0;
invertedPendulumAngle = invertedPendulum.Calculate(-currentOutput);
currentAngle = (invertedPendulumAngle + noise);
}
/// <summary>
/// Changes the angle of the pendulums and calculates the corrections for the feedback controllers.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void ChangeAngle(object sender, ElapsedEventArgs e)
{
if (DateTime.Now.Subtract(dateTime).TotalSeconds > WaitTimeForCalculation)
{
if (!initializeFeedbackControllers)
{
pid = new PID(kp, ki, kd, maxOutput);
adrc = new ADRC_PD(r, c, b, hModifier, kp, kd, maxOutput);
initializeFeedbackControllers = true;
}
correctionState = true;
outputPID = pid.Calculate(SetPoint, anglePID);
outputADRC = adrc.Calculate(SetPoint, angleADRC);
pidFileWriter.WriteLine(pidCounter + "," + outputPID + "," + anglePID);
adrcFileWriter.WriteLine(adrcCounter + "," + outputADRC + "," + angleADRC);
pidCounter++;
adrcCounter++;
ADRCAngle.Add((float)angleADRC);
PIDAngle.Add((float)anglePID);
ADRCOutput.Add((float)outputADRC);
PIDOutput.Add((float)outputPID);
if (ADRCAngle.ToArray().Length > FourierTransform.FourierMemory)
{
ADRCAngle.RemoveAt(0);
PIDAngle.RemoveAt(0);
ADRCOutput.RemoveAt(0);
PIDOutput.RemoveAt(0);
}
}
Random rand = new Random();
double noise = rand.NextDouble() * NoiseFactor * (rand.Next(0, 1) * 2 - 1);
double tempAnglePID = (invertedPendulumPID.Step(-outputPID) * 180 / Math.PI + noise); // % 360;
double tempAngleADRC = (invertedPendulumADRC.Step(-outputADRC) * 180 / Math.PI + noise); // % 360;
anglePID = tempAnglePID;
angleADRC = tempAngleADRC;
}
void yawUpdate()
{
// input
float yaw = (transform.parent.localEulerAngles.z < 180f) ? transform.parent.localEulerAngles.z : transform.parent.localEulerAngles.z - 360;
// yaw PID
Vector3 force = -1 * transform.right * yawPid.Calculate(Time.deltaTime, yaw);
yawError = yawPid.error;
YawForce = force.magnitude;
// add force
_rigidBody.AddForceAtPosition(force, _thrusterCluster1WorldPosition);
_rigidBody.AddForceAtPosition(force, _thrusterCluster2WorldPosition);
// draw debug
Debug.DrawLine(_thrusterCluster1WorldPosition, _thrusterCluster1WorldPosition + (-1) * force, Color.yellow);
Debug.DrawLine(_thrusterCluster2WorldPosition, _thrusterCluster2WorldPosition + (-1) * force, Color.yellow);
}
void pitchUpdate()
{
// input
float pitch = (transform.parent.localEulerAngles.x < 180f) ? transform.parent.localEulerAngles.x : transform.parent.localEulerAngles.x - 360;
// picth PID
Vector3 force = transform.forward * pitchPid.Calculate(Time.deltaTime, pitch);
pitchError = pitchPid.error;
PitchForce = force.magnitude;
// add force
_rigidBody.AddForceAtPosition(force, _thrusterCluster1WorldPosition);
_rigidBody.AddForceAtPosition(force, _thrusterCluster2WorldPosition);
// draw debug
Debug.DrawLine(_thrusterCluster1WorldPosition, _thrusterCluster1WorldPosition + (-1) * force, Color.yellow);
Debug.DrawLine(_thrusterCluster2WorldPosition, _thrusterCluster2WorldPosition + (-1) * force, Color.yellow);
}
void rollUpdate()
{
// input
float roll = (transform.parent.localEulerAngles.y < 180f) ? transform.parent.localEulerAngles.y : transform.parent.localEulerAngles.y - 360;
// roll PID
Vector3 force = transform.forward * rollPid.Calculate(Time.deltaTime, roll);
rollError = rollPid.error;
RollForce = force.magnitude;
// add force
_rigidBody.AddForceAtPosition(-force, _thrusterCluster1WorldPosition);
_rigidBody.AddForceAtPosition(force, _thrusterCluster2WorldPosition);
// draw debug
Debug.DrawLine(_thrusterCluster1WorldPosition, _thrusterCluster1WorldPosition + (-1) * force, Color.yellow);
Debug.DrawLine(_thrusterCluster2WorldPosition, _thrusterCluster2WorldPosition + (-1) * force, Color.yellow);
}
void FixedUpdate()
{
if (StaticDataAccess.config == null)
{
Debug.LogWarning("StaticDataAccess.config not found");
return;
}
if (StaticDataAccess.config.input == null)
{
Debug.LogWarning("StaticDataAccess.config.input not found");
return;
}
if (StaticDataAccess.config.input.GetBtnReset())
{
transform.position = startPos;
transform.rotation = startRot;
rb.velocity = Vector3.zero;
rb.angularVelocity = Vector3.zero;
if (lipo != null)
{
lipo.ChargeTo(lipo.maxVoltage);
}
}
if (StaticDataAccess.config.input.GetBtnFlip())
{
RaycastHit rayHit;
if (Physics.Raycast(transform.position, Vector3.down, out rayHit, LayerMask.GetMask("Terrain")))
{
transform.position = rayHit.point + Vector3.up * 3.0f;
}
else
{
transform.position += Vector3.up * 5.0f;
}
transform.rotation = Quaternion.identity;
rb.velocity = Vector3.zero;
rb.angularVelocity = Vector3.zero;
}
float throttle = StaticDataAccess.config.input.GetAxisThrottle();
float roll = StaticDataAccess.config.input.GetAxisRoll();
float pitch = StaticDataAccess.config.input.GetAxisPitch();
float yaw = StaticDataAccess.config.input.GetAxisYaw();
throttle = Mathf.Clamp01(throttle + idleThrottle);
roll = rateProfile.ApplyRoll(roll) * Mathf.Deg2Rad;
pitch = rateProfile.ApplyPitch(pitch) * Mathf.Deg2Rad;
yaw = rateProfile.ApplyYaw(yaw) * Mathf.Deg2Rad;
pidRoll.ApplyProfile(pidProfile, PIDAxis.Roll);
pidPitch.ApplyProfile(pidProfile, PIDAxis.Pitch);
pidYaw.ApplyProfile(pidProfile, PIDAxis.Yaw);
Vector3 localRot = transform.InverseTransformVector(rb.angularVelocity);
float rollOut = pidRoll.Calculate(-localRot.z, roll);
float pitchOut = pidPitch.Calculate(localRot.x, pitch);
float yawOut = pidYaw.Calculate(localRot.y, yaw);
Vector3 force = Vector3.zero;
Vector3 torque = Vector3.zero;
if (lipo == null)
{
force = Vector3.up * throttle * thrust;
torque = new Vector3(pitch, yaw, -roll);
}
else
{
float thrCurrent = 0.0f;
float rotCurrent = 0.0f;
if (powertrain == null)
{
thrCurrent = Mathf.Abs(throttle * 200);
rotCurrent = new Vector3(pitch, yaw, -roll).magnitude * 20;
}
else
{
thrCurrent = powertrain.throttleCurrentCurve.Evaluate(throttle);
// might need to tweak the scale
rotCurrent = powertrain.throttleCurrentCurve.Evaluate(new Vector3(pitch, yaw, -roll).magnitude / 30.0f);
}
lipo.expectedCurrent = thrCurrent + rotCurrent;
if (lipo.expectedCurrent > 0)
{
float thrFraction = thrCurrent / lipo.expectedCurrent;
float rotFraction = rotCurrent / lipo.expectedCurrent;
float power = lipo.actualCurrent * lipo.totalVoltage;
float groundEffect = 0.0f;
if (groundEffectFactor > 0)
{
RaycastHit rayHit;
if (Physics.Raycast(transform.position, Vector3.down, out rayHit, groundEffectHeight, LayerMask.GetMask("Terrain")))
{
groundEffect = (groundEffectHeight - rayHit.distance) * groundEffectFactor / groundEffectHeight;
}
}
if (powertrain == null)
{
force = 0.017f * Vector3.up * power * thrFraction * (1 + groundEffect);
torque = 0.01f * new Vector3(pitchOut, yawOut, -rollOut) * power * rotFraction;
}
else
{
force = 0.025f * Vector3.up * power * thrFraction * (1 + groundEffect);
torque = 0.07f * new Vector3(pitchOut, yawOut, -rollOut) * power * rotFraction;
}
}
}
if (propwashFactor > 0)
{
Vector2 forw2d = new Vector2(transform.forward.x, transform.forward.z);
Vector2 vel2d = new Vector2(rb.velocity.x, rb.velocity.z);
float propwash = force.magnitude * propwashFactor * (1 - Vector2.Dot(forw2d.normalized, vel2d.normalized)) / (0.1f * vel2d.magnitude + 1.5f);
if (vel2d.magnitude < 2.0f)
{
// fixes too much propwash while hovering
propwash *= vel2d.magnitude + 0.05f;
}
quadMesh.localEulerAngles = new Vector3(
UnityEngine.Random.Range(-1.0f, 1.0f),
UnityEngine.Random.Range(-1.0f, 1.0f),
UnityEngine.Random.Range(-1.0f, 1.0f)
) * propwash;
}
else
{
quadMesh.localEulerAngles = Vector3.zero;
}
float aoaSine = Vector3.Dot(transform.forward, rb.velocity.normalized);
float drag = Mathf.Lerp(areaFront, areaTop, Mathf.Abs(aoaSine)) * rb.velocity.sqrMagnitude * Cd;
rb.drag = drag;
if (torque.magnitude < 0.1f)
{
rb.angularDrag = rotDrag;
}
else
{
rb.angularDrag = rotDrag / 5.0f / Mathf.Max(1.0f, torque.magnitude);
}
rb.AddRelativeForce(force);
rb.AddRelativeTorque(torque);
}
