void Stabilize()
{
//Получаем текущий поворот квадрика по X и Z
float pitchCurrent = NormalizeAngle180(transform.eulerAngles.x);
float rollCurrent = NormalizeAngle180(transform.eulerAngles.z);
//Получаем ошибку (разницу между текущим и желаемым)
float pitchError = pitchCurrent - pitch;
float rollError = rollCurrent - roll;
//Получаем необходимую коррекцию
float pitchCorrection = PIDPitch.Calculate(pitchError);
float rollCorrection = PIDRoll.Calculate(rollError);
//Считаем силу для винтов
float propellerForceFL = throttle + pitchCorrection + rollCorrection;
float propellerForceFR = throttle + pitchCorrection - rollCorrection;
float propellerForceBL = throttle - pitchCorrection + rollCorrection;
float propellerForceBR = throttle - pitchCorrection - rollCorrection;
//Применяем силу к винтам (не больше макс.)
AddForceToPropeller(PropellerFL, Mathf.Clamp(propellerForceFL, 0, maxPropellerForce));
AddForceToPropeller(PropellerFR, Mathf.Clamp(propellerForceFR, 0, maxPropellerForce));
AddForceToPropeller(PropellerBL, Mathf.Clamp(propellerForceBL, 0, maxPropellerForce));
AddForceToPropeller(PropellerBR, Mathf.Clamp(propellerForceBR, 0, maxPropellerForce));
//Получаем текущее вращение по Y
float yawCurrent = Quadcopter.angularVelocity.y;
//Считаем ошибку
float yawError = yawCurrent - yaw;
//Получаем коррекцию
float yawCorrection = PIDYaw.Calculate(yawError);
/*
* //Считаем крутящий момент для винтов
* float propellerTorqueFL = -yawCorrection;// throttle - yawCorrection;
* float propellerTorqueFR = -yawCorrection;// -throttle - yawCorrection;
* float propellerTorqueBL = -yawCorrection;// -throttle - yawCorrection;
* float propellerTorqueBR = -yawCorrection;// throttle - yawCorrection;
*
* //Применяем крутящий момент к винтам
* AddTorqueToPropeller(PropellerFL, Mathf.Clamp(propellerTorqueFL, -maxTorque, maxTorque));
* AddTorqueToPropeller(PropellerFR, Mathf.Clamp(propellerTorqueFR, -maxTorque, maxTorque));
* AddTorqueToPropeller(PropellerBL, Mathf.Clamp(propellerTorqueBL, -maxTorque, maxTorque));
* AddTorqueToPropeller(PropellerBR, Mathf.Clamp(propellerTorqueBR, -maxTorque, maxTorque));
*/
//Добавляем вращение к квадрику
Quadcopter.AddTorque(transform.up * Mathf.Clamp(-yawCorrection, -maxTorque, maxTorque));
}
private void FixedUpdate()
{
if (autoHeight)
{
heightController.SetPoint = height;
power = (float)heightController.Calculate(core.position.y);
power = Mathf.Clamp(power, 0, 50);
}
fl.AddForce(Vector3.up * power);
fr.AddForce(Vector3.up * power);
bl.AddForce(Vector3.up * power);
br.AddForce(Vector3.up * power);
}
private void DetermineAdjustValue()
{
float solar_angle = Vector3.SignedAngle(resting_pos.forward, calculated_solar_dir, transform.right);
current_offset_angle = Vector3.SignedAngle(resting_pos.forward, transform.forward, transform.right);
desired_position = solar_angle;
if (!satController.disablePID && !satController.disable)
{
float e = solar_angle - current_offset_angle;
var u = pid.Calculate(e);
adjust_value = u;
}
else
{
adjust_value = (solar_angle - current_offset_angle) * rotationSpeed;
}
}
private double calculateSteering(VehicleState currentState, int target)
{
var targetPosition = path[target].State.Position;
var followingPointPosition = target < path.Count - 1
? path[target + 1].State.Position
: targetPosition + (targetPosition - path[target - 1].State.Position); // extend the last segment in the same direction
var targetDirection = followingPointPosition - targetPosition;
var directionToTarget = targetPosition - currentState.Position;
var leftOrRight = Math.Sign(targetDirection.Cross(directionToTarget));
var distance = Distance.Between(targetPosition, currentState.Position);
crossTrackError.OnNext(leftOrRight * distance);
var steering = -steeringController.Calculate(target: 0, leftOrRight * distance);
Console.WriteLine($"cte={leftOrRight * distance}, steering={steering}");
return(Math.Clamp(steering, -1, 1));
}
private void FixedUpdate()
{
rb.AddForce(Vector3.up * (float)pidController.Calculate(rb.position.y));
}
