// PID void Update() { if (!goal) { return; } // compute errors Vector3 positionError = goal.transform.position - transform.position; Vector3 derivativeError = (positionError - previousPositionError) / elapseTime; float angle = goal.transform.rotation.eulerAngles.y - transform.rotation.eulerAngles.y; // keep angle between -180 and 180 degree if (angle > 180) { angle -= 360.0f; } else if (angle < -180) { angle += 360.0f; } float orientationError = Mathf.Deg2Rad * angle; float derivativeOrientationError = (orientationError - previousOrientationError) / elapseTime; // compute PID Vector3 commandPosition = new Vector3(); commandPosition.x = Kp_pitchroll * positionError.x + Ki_pitchroll * integralPositionError.x + Kd_pitchroll * derivativeError.x; commandPosition.y = Kp_vertical * positionError.y + Ki_vertical * integralPositionError.y + Kd_vertical * derivativeError.y; commandPosition.z = Kp_pitchroll * positionError.z + Ki_pitchroll * integralPositionError.z + Kd_pitchroll * derivativeError.z; float commandOrientation = Kp_yaw * orientationError + Ki_yaw * integralOrientationError + Kd_yaw * derivativeOrientationError; // update PID parameters previousPositionError = positionError; integralPositionError += elapseTime * positionError; previousOrientationError = orientationError; integralOrientationError += elapseTime * orientationError; // clamp commands and integral error to prevent drone dammages commandPosition.x = Mathf.Clamp(commandPosition.x, -CommandClamp_pitchroll, CommandClamp_pitchroll); commandPosition.y = Mathf.Clamp(commandPosition.y, -CommandClamp_vertical, CommandClamp_vertical); commandPosition.z = Mathf.Clamp(commandPosition.z, -CommandClamp_pitchroll, CommandClamp_pitchroll); commandOrientation = Mathf.Clamp(commandOrientation, -CommandClamp_yaw, CommandClamp_yaw); integralPositionError.x = Mathf.Clamp(integralPositionError.x, -IntegralClamp_pitchroll, IntegralClamp_pitchroll); integralPositionError.y = Mathf.Clamp(integralPositionError.y, -IntegralClamp_vertical, IntegralClamp_vertical); integralPositionError.z = Mathf.Clamp(integralPositionError.z, -IntegralClamp_pitchroll, IntegralClamp_pitchroll); integralOrientationError = Mathf.Clamp(integralOrientationError, -IntegralClamp_yaw, IntegralClamp_yaw); // compute local command : from global Optitrack to local drone body Matrix4x4 localToWorld = Matrix4x4.Translate(transform.position) * Matrix4x4.Rotate(transform.rotation); Vector4 localPositionCommand = localToWorld.inverse * new Vector4(commandPosition.x, commandPosition.y, commandPosition.z, 0.0f); Vector3Int msgComm = new Vector3Int(Mathf.RoundToInt(localPositionCommand.x), Mathf.RoundToInt(localPositionCommand.y), Mathf.RoundToInt(localPositionCommand.z)); // Send command to raspberry pi : to make sure the order of execution doesnt matter (unity <-> raspberry pi) if (udpclient) { udpclient.SendMessage(msgComm.x.ToString() + " " + msgComm.y.ToString() + " " + msgComm.z.ToString() + " " + Mathf.RoundToInt(commandOrientation).ToString()); } else { udpclient = gameObject.GetComponent <UDPSend>(); if (udpclient) { Debug.Log("CONNECTED "); } } //Debug.Log(localPositionCommand.x.ToString() + " " + localPositionCommand.y.ToString() + " " + localPositionCommand.z.ToString() + " " + Mathf.RoundToInt(commandOrientation).ToString()); //Debug.Log(msgComm.x.ToString() + " " + msgComm.y.ToString() + " " + msgComm.z.ToString() + " " + Mathf.RoundToInt(commandOrientation).ToString()); }