128 lines
3.4 KiB
C++
128 lines
3.4 KiB
C++
#include "MecanumVehicleAdapter.h"
|
|
|
|
#include "Sockets.h"
|
|
#include "Networking.h"
|
|
#include "Misc/Timespan.h"
|
|
#include "SocketSubsystem.h"
|
|
|
|
AMecanumVehicleAdapter::AMecanumVehicleAdapter(const FObjectInitializer& ObjectInitializer)
|
|
{
|
|
PrimaryActorTick.bCanEverTick = true;
|
|
}
|
|
|
|
void AMecanumVehicleAdapter::BeginPlay()
|
|
{
|
|
Super::BeginPlay();
|
|
|
|
SocketSubsystem = ISocketSubsystem::Get(PLATFORM_SOCKETSUBSYSTEM);
|
|
|
|
UDPSocket = SocketSubsystem->CreateSocket(NAME_DGram, TEXT("Raspberry Socket"), FNetworkProtocolTypes::IPv4);
|
|
|
|
UDPBuffer.SetNumUninitialized(1024);
|
|
|
|
PingTime = 1024.0;
|
|
|
|
for (int32 Index = 0; Index < MAX_uint8; ++Index)
|
|
{
|
|
UDPRecvTime[Index] = FDateTime(0);
|
|
}
|
|
|
|
UpdataSocket(TEXT("192.168.1.1"));
|
|
}
|
|
|
|
void AMecanumVehicleAdapter::UpdataCompensation(const TArray<double>& Value)
|
|
{
|
|
if (Value.Num() != 8) return;
|
|
|
|
SpeedCompensationMax[0] = Value[0];
|
|
SpeedCompensationMax[1] = Value[1];
|
|
SpeedCompensationMax[2] = Value[2];
|
|
SpeedCompensationMax[3] = Value[3];
|
|
|
|
SpeedCompensationMin[0] = Value[4];
|
|
SpeedCompensationMin[1] = Value[5];
|
|
SpeedCompensationMin[2] = Value[6];
|
|
SpeedCompensationMin[3] = Value[7];
|
|
|
|
}
|
|
|
|
void AMecanumVehicleAdapter::UpdataSocket(const FString& IP, const int32 Port)
|
|
{
|
|
int32 BytesNum = 0;
|
|
Addr = SocketSubsystem->CreateInternetAddr();
|
|
|
|
bool bIsValid;
|
|
Addr->SetIp(TEXT("192.168.1.1"), bIsValid);
|
|
Addr->SetIp(*IP, bIsValid);
|
|
Addr->SetPort(Port);
|
|
}
|
|
|
|
void AMecanumVehicleAdapter::UpdataInput(const FVector Value, const bool bIsBrake)
|
|
{
|
|
++UDPSequenceIndex;
|
|
|
|
UDPSequenceIndex %= MAX_uint8;
|
|
|
|
UDPRecvTime[UDPSequenceIndex] = FDateTime::UtcNow();
|
|
|
|
double SpeedLF = 0.0f;
|
|
double SpeedRF = 0.0f;
|
|
double SpeedLR = 0.0f;
|
|
double SpeedRR = 0.0f;
|
|
|
|
SpeedLF += Value.X;
|
|
SpeedRF += Value.X;
|
|
SpeedLR += Value.X;
|
|
SpeedRR += Value.X;
|
|
|
|
SpeedLF += Value.Y;
|
|
SpeedRF -= Value.Y;
|
|
SpeedLR -= Value.Y;
|
|
SpeedRR += Value.Y;
|
|
|
|
SpeedLF += Value.Z;
|
|
SpeedRF -= Value.Z;
|
|
SpeedLR += Value.Z;
|
|
SpeedRR -= Value.Z;
|
|
|
|
UDPBuffer[0] = UDPSequenceIndex;
|
|
UDPBuffer[1] = (int8)(FMath::Clamp(FMath::Sign(SpeedLF) * FMath::Lerp(SpeedCompensationMin[0], SpeedCompensationMax[0], FMath::Abs(SpeedLF)) * 128.0, MIN_int8, MAX_int8));
|
|
UDPBuffer[2] = (int8)(FMath::Clamp(FMath::Sign(SpeedRF) * FMath::Lerp(SpeedCompensationMin[1], SpeedCompensationMax[1], FMath::Abs(SpeedRF)) * 128.0, MIN_int8, MAX_int8));
|
|
UDPBuffer[3] = (int8)(FMath::Clamp(FMath::Sign(SpeedLR) * FMath::Lerp(SpeedCompensationMin[2], SpeedCompensationMax[2], FMath::Abs(SpeedLR)) * 128.0, MIN_int8, MAX_int8));
|
|
UDPBuffer[4] = (int8)(FMath::Clamp(FMath::Sign(SpeedRR) * FMath::Lerp(SpeedCompensationMin[3], SpeedCompensationMax[3], FMath::Abs(SpeedRR)) * 128.0, MIN_int8, MAX_int8));
|
|
UDPBuffer[5] = bIsBrake;
|
|
|
|
int32 BytesNum = 0;
|
|
UDPSocket->SendTo(UDPBuffer.GetData(), 6, BytesNum, *Addr);
|
|
}
|
|
|
|
void AMecanumVehicleAdapter::Tick(float DeltaTime)
|
|
{
|
|
Super::Tick(DeltaTime);
|
|
|
|
int32 BytesNum = 0;
|
|
TSharedRef<FInternetAddr> Source = SocketSubsystem->CreateInternetAddr();
|
|
|
|
uint32 PendingDataSize;
|
|
while (UDPSocket->HasPendingData(PendingDataSize))
|
|
{
|
|
UDPSocket->RecvFrom(UDPBuffer.GetData(), UDPBuffer.Num(), BytesNum, *Source);
|
|
|
|
if (BytesNum == 1)
|
|
{
|
|
uint8 Index = UDPBuffer[0];
|
|
PingTime = (FDateTime::UtcNow() - UDPRecvTime[Index]).GetTotalMilliseconds();
|
|
}
|
|
else continue;
|
|
|
|
UDPLastTime = FDateTime::UtcNow();
|
|
}
|
|
|
|
PingTime = FMath::Max(PingTime, (FDateTime::UtcNow() - UDPLastTime).GetTotalMilliseconds());
|
|
}
|
|
|
|
void AMecanumVehicleAdapter::EndPlay(const EEndPlayReason::Type EndPlayReason)
|
|
{
|
|
Super::EndPlay(EndPlayReason);
|
|
}
|