Agrarsense
PIDDrone.cpp
Go to the documentation of this file.
1// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
2//
3// This work is licensed under the terms of the MIT license.
4// For a copy, see <https://opensource.org/licenses/MIT>.
5
9
10#include "Camera/CameraComponent.h"
11#include "Field/FieldSystemNodes.h"
12#include "Algo/Reverse.h"
13
14#if WITH_EDITOR
15#include "DrawDebugHelpers.h"
16#endif
17
19{
20 PrimaryActorTick.bCanEverTick = true;
21 InteractableName = NSLOCTEXT("Agrarsense", "DroneInteractableName", "Drone");
22}
23
25{
26 Super::BeginPlay();
27
28 World = GetWorld();
29
30 mesh = Cast<USkeletalMeshComponent>(GetComponentByClass(USkeletalMeshComponent::StaticClass()));
31 desiredPositionMesh = Cast<UStaticMeshComponent>(GetComponentByClass(UStaticMeshComponent::StaticClass()));
32
33 StartingPosition = GetActorLocation();
34}
35
36void APIDDrone::EndPlay(const EEndPlayReason::Type EndPlayReason)
37{
38 Super::EndPlay(EndPlayReason);
39}
40
41void APIDDrone::Tick(float DeltaTime)
42{
43 Super::Tick(DeltaTime);
44
45 if (!IsVehicleInGarage())
46 {
47 AutoPilot(DeltaTime);
48 }
49}
50
52{
53 // TODO: Check if in garage
54 // include drone rotation to point
55
56 // Don't try to fly if no waypoints
57 if (DroneParameters.Points.Num() == 0 && IsRoaming())
58 {
60 }
61
62 FVector waypoint = GetCurrentWaypointTarget();
63 FVector currentlocation = mesh->GetRelativeTransform().GetLocation();
64
66 {
67 // Doesn't work
68 desiredPositionMesh->SetWorldLocation(waypoint);
69 }
70#if WITH_EDITOR
71 else
72 {
73 UE_LOG(LogTemp, Warning, TEXT("Desired location mesh not found"));
74 //DrawDebugLine(World, currentlocation, waypoint, FColor::Red, false, 0.0f, 0.0f, 5.0f);
75 }
76#endif
77
78 if (FVector2f::Distance(FVector2f(waypoint.X, waypoint.Y), FVector2f(currentlocation.X, currentlocation.Y)) < 1000)
79 {
80 waypointReached = true;
81 if (passedWaypoints != DroneParameters.Points.Num() - 1)
82 {
83#if WITH_EDITOR
84 UE_LOG(LogTemp, Warning, TEXT("Changing waypoint from %i to %i"), passedWaypoints, passedWaypoints + 1);
85#endif
86
88 }
89 else
90 {
92 {
95 }
96 else
97 {
99 {
102 break;
105 passedWaypoints = 0;
106 break;
108 passedWaypoints = 0;
109 break;
111 Algo::Reverse(DroneParameters.Points);
112 passedWaypoints = 0;
113 break;
114 default:
115 break;
116 }
117 }
118 }
119 }
120}
121
122void APIDDrone::MoveDroneToPosition(const FTransform Transform)
123{
124 if (DroneParameters.Points.Num() > 0)
125 {
126 DroneParameters.Points.Empty();
128 }
129 else
130 {
132 }
133}
134
136{
137 // UE_LOG(LogTemp, Warning, TEXT(" % s"), *DroneParameters.Points[passedWaypoints].GetLocation().ToString());
139 {
140 return FVector(0, 0, 0);
141 }
142
143 return DroneParameters.Points[passedWaypoints].GetLocation();
144}
145
146void APIDDrone::AssignRoamingPoints(const TArray<FTransform> Points)
147{
148 // Empty current waypoints if any
150
151 for (const FTransform& point : Points)
152 {
153 // Assing new waypoints to the array
154 WayPoints.Add(point.GetLocation());
155 }
156}
157
158void APIDDrone::SetDroneRotation(USkeletalMeshComponent* target, FRotator rotator)
159{
160 if (!target)
161 {
162 return;
163 }
164
165 FRotator rotation = FRotator(
166 FMath::Clamp(rotator.Pitch, -2, 2),
167 FMath::Clamp(rotator.Yaw, -2, 2),
168 FMath::Clamp(rotator.Roll, -2, 2));
169
170 FRotator currentRotation = target->GetRelativeRotation();
171
172 FRotator rotationDifference = currentRotation - rotator;
173
174 if (FMath::Abs(rotator.Pitch) > 0 && FMath::Abs(currentRotation.Pitch) < FMath::Abs(rotationDifference.Pitch))
175 {
176 target->AddRelativeRotation(FRotator(rotation.Pitch, 0, 0), false, nullptr, ETeleportType::TeleportPhysics);
177 }
178 if (FMath::Abs(rotator.Yaw) > 0 && FMath::Abs(currentRotation.Yaw) < FMath::Abs(rotationDifference.Yaw))
179 {
180 target->AddRelativeRotation(FRotator(0, rotation.Yaw, 0), false, nullptr, ETeleportType::TeleportPhysics);
181 }
182 if (FMath::Abs(rotator.Roll) > 0 && FMath::Abs(currentRotation.Roll) < FMath::Abs(rotationDifference.Roll))
183 {
184 target->AddRelativeRotation(FRotator(0, 0, rotation.Roll), false, nullptr, ETeleportType::TeleportPhysics);
185 }
186
187 if (rotator.Pitch == 0 || rotator.Yaw == 0 || rotator.Roll == 0)
188 {
189 target->AddRelativeRotation(FRotator(-FMath::Clamp(rotationDifference.Pitch, -2, 2), -FMath::Clamp(rotationDifference.Yaw, -2, 2), -FMath::Clamp(rotationDifference.Roll, -2, 2)), false, nullptr, ETeleportType::TeleportPhysics);
190 }
191}
192
193TArray<FTransform> APIDDrone::GenerateRoamingPoints(float radius, int32 roamingPoints)
194{
195 TArray<FTransform> generatedRoamingPoints;
196 generatedRoamingPoints.Reserve(roamingPoints);
197
198 FVector currentPosition = this->GetTransform().GetLocation();
199
200 FVector min = currentPosition - FVector(radius / 2, radius / 2, 0);
201 FVector max = currentPosition + FVector(radius / 2, radius / 2, 0);
202
203 for (int32 i = 0; i < roamingPoints; i++)
204 {
205 FTransform randomPoint;
206 randomPoint.SetLocation(FVector(FMath::RandRange(min.X, max.X), FMath::RandRange(min.Y, max.Y), 5000));
207
208 generatedRoamingPoints.Add(randomPoint);
209#if WITH_EDITOR
210 UE_LOG(LogTemp, Warning, TEXT("Waypoint %i: (%s)"), i, *randomPoint.GetLocation().ToString());
211#endif
212 }
213
214 return generatedRoamingPoints;
215}
216
217void APIDDrone::AutoPilot(float DeltaTime)
218{
220 {
221 return;
222 }
223
225}
bool waypointReached
Definition: PIDDrone.h:195
TArray< FVector > WayPoints
Definition: PIDDrone.h:199
void AssignRoamingPoints(const TArray< FTransform > Points)
Definition: PIDDrone.cpp:146
void SetDroneRotation(USkeletalMeshComponent *target, FRotator rotator)
Definition: PIDDrone.cpp:158
FVector GetCurrentWaypointTarget()
Definition: PIDDrone.cpp:135
void MoveDroneToPosition(const FTransform Transform)
Override all drone roaming points and continue towards this position.
Definition: PIDDrone.cpp:122
UWorld * World
Definition: PIDDrone.h:188
int32 passedWaypoints
Definition: PIDDrone.h:204
FVector StartingPosition
Definition: PIDDrone.h:190
virtual void BeginPlay() override
Definition: PIDDrone.cpp:24
void ClearWaypoints()
Definition: PIDDrone.h:93
UStaticMeshComponent * desiredPositionMesh
Definition: PIDDrone.h:193
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
Definition: PIDDrone.cpp:36
void AutoPilot(float DeltaTime)
Definition: PIDDrone.cpp:217
FDroneParameters DroneParameters
Definition: PIDDrone.h:182
bool IsRoaming() const
Definition: PIDDrone.h:162
USkeletalMeshComponent * mesh
Definition: PIDDrone.h:192
void FlySetFlightpath()
Called in tick function for drone roaming through points.
Definition: PIDDrone.cpp:51
TArray< FTransform > GenerateRoamingPoints(float radius, int32 roamingPoints)
Generates a roadming points array for the drone in radius.
Definition: PIDDrone.cpp:193
virtual void Tick(float DeltaTime) override
Definition: PIDDrone.cpp:41
FText InteractableName
Definition: Vehicle.h:257
bool IsVehicleInGarage() const
Definition: Vehicle.h:123
EDroneEndAction DroneEndAction
EDroneAction DroneAction
TArray< FTransform > Points