void cProjectileEntity::HandlePhysics(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
{
if (m_IsInGround)
{
// Already-grounded projectiles don't move at all
return;
}
auto DtSec = std::chrono::duration_cast<std::chrono::duration<double>>(a_Dt);
const Vector3d DeltaSpeed = GetSpeed() * DtSec.count();
const Vector3d Pos = GetPosition();
const Vector3d NextPos = Pos + DeltaSpeed;
// Test for entity collisions:
cProjectileEntityCollisionCallback EntityCollisionCallback(this, Pos, NextPos);
a_Chunk.ForEachEntity(EntityCollisionCallback);
if (EntityCollisionCallback.HasHit())
{
// An entity was hit:
Vector3d HitPos = Pos + (NextPos - Pos) * EntityCollisionCallback.GetMinCoeff();
// DEBUG:
LOGD("Projectile %d has hit an entity %d (%s) at {%.02f, %.02f, %.02f} (coeff %.03f)",
m_UniqueID,
EntityCollisionCallback.GetHitEntity()->GetUniqueID(),
EntityCollisionCallback.GetHitEntity()->GetClass(),
HitPos.x, HitPos.y, HitPos.z,
EntityCollisionCallback.GetMinCoeff()
);
OnHitEntity(*(EntityCollisionCallback.GetHitEntity()), HitPos);
if (!IsTicking())
{
return; // We were destroyed by an override of OnHitEntity
}
}
// TODO: Test the entities in the neighboring chunks, too
// Trace the tick's worth of movement as a line:
cProjectileTracerCallback TracerCallback(this);
if (!cLineBlockTracer::Trace(*m_World, TracerCallback, Pos, NextPos))
{
// Something has been hit, abort all other processing
return;
}
// The tracer also checks the blocks for slowdown blocks - water and lava - and stores it for later in its SlowdownCoeff
// Update the position:
SetPosition(NextPos);
// Add slowdown and gravity effect to the speed:
Vector3d NewSpeed(GetSpeed());
NewSpeed.y += m_Gravity * DtSec.count();
NewSpeed -= NewSpeed * (m_AirDrag * 20.0f) * DtSec.count();
SetSpeed(NewSpeed);
SetYawFromSpeed();
SetPitchFromSpeed();
/*
LOGD("Projectile %d: pos {%.02f, %.02f, %.02f}, speed {%.02f, %.02f, %.02f}, rot {%.02f, %.02f}",
m_UniqueID,
GetPosX(), GetPosY(), GetPosZ(),
GetSpeedX(), GetSpeedY(), GetSpeedZ(),
GetYaw(), GetPitch()
);
*/
}
void cProjectileEntity::HandlePhysics(float a_Dt, cChunk & a_Chunk)
{
if (m_IsInGround)
{
// Already-grounded projectiles don't move at all
return;
}
Vector3d PerTickSpeed = GetSpeed() / 20;
Vector3d Pos = GetPosition();
// Trace the tick's worth of movement as a line:
Vector3d NextPos = Pos + PerTickSpeed;
cProjectileTracerCallback TracerCallback(this);
if (!cLineBlockTracer::Trace(*m_World, TracerCallback, Pos, NextPos))
{
// Something has been hit, abort all other processing
return;
}
// The tracer also checks the blocks for slowdown blocks - water and lava - and stores it for later in its SlowdownCoeff
// Test for entity collisions:
cProjectileEntityCollisionCallback EntityCollisionCallback(this, Pos, NextPos);
a_Chunk.ForEachEntity(EntityCollisionCallback);
if (EntityCollisionCallback.HasHit())
{
// An entity was hit:
Vector3d HitPos = Pos + (NextPos - Pos) * EntityCollisionCallback.GetMinCoeff();
// DEBUG:
LOGD("Projectile %d has hit an entity %d (%s) at {%.02f, %.02f, %.02f} (coeff %.03f)",
m_UniqueID,
EntityCollisionCallback.GetHitEntity()->GetUniqueID(),
EntityCollisionCallback.GetHitEntity()->GetClass(),
HitPos.x, HitPos.y, HitPos.z,
EntityCollisionCallback.GetMinCoeff()
);
OnHitEntity(*(EntityCollisionCallback.GetHitEntity()), HitPos);
}
// TODO: Test the entities in the neighboring chunks, too
// Update the position:
SetPosition(NextPos);
// Add slowdown and gravity effect to the speed:
Vector3d NewSpeed(GetSpeed());
NewSpeed.y += m_Gravity / 20;
NewSpeed *= TracerCallback.GetSlowdownCoeff();
SetSpeed(NewSpeed);
SetRotationFromSpeed();
SetPitchFromSpeed();
// DEBUG:
LOGD("Projectile %d: pos {%.02f, %.02f, %.02f}, speed {%.02f, %.02f, %.02f}, rot {%.02f, %.02f}",
m_UniqueID,
GetPosX(), GetPosY(), GetPosZ(),
GetSpeedX(), GetSpeedY(), GetSpeedZ(),
GetRotation(), GetPitch()
);
}
void tCougarIO::on_SPIDataProcessing_messageReceived(const tSPIDataProcessing::tMessage& msg)
{
Assert(tThread::CurrentThreadName() == "SonarMaster");
//DbgPrintf("MessageReceived = %d", msg.MessageID);
switch(msg.MessageID)
{
case tSPIDataProcessing::MSG_ID_COLUMN_INFO:
#ifdef USE_REPROGRAMMING_CODE
//if(m_Reprogramming == false)
#endif
{
//DbgPrintf("tCougarIO: m_Reprogramming = false");
tCSMSettingsXPtr xCSMSettings;
tCSMSoftwareInfoXPtr xCSMSoftwareInfo;
tCSMAnalogData csmAnalogData;
GetCSMData(msg.data, xCSMSettings, xCSMSoftwareInfo, csmAnalogData);
//~~~~~~ AC - Why would this happen? Need to make sure bogus messages are not sent!
if(xCSMSettings->LowerLimitFt() <= xCSMSettings->UpperLimitFt())
{
// This is normal when hiding the digital search with an upper limit...
// Ignore the problem if the lower limit is like 0.5ft.
if(xCSMSettings->LowerLimitFt() > 1)
{
//DbgPrintf("tCougarIO: LowerLimit(%f) <= UpperLimit(%f) !!!.....",xCSMSettings->LowerLimitFt(), xCSMSettings->UpperLimitFt() );
break;
}
}
//DbgPrintf("Settings received....");
//xCSMSettings->DEBUG_PrintSettings();
//DbgPrintf("SonarIO: SIGNAL csmSettingsReceived");
//emit csmSettingsReceived(xCSMSettings);
//DbgPrintf("SonarIO: SIGNAL DiagnosticsReceived");
emit SoftwareInfoReceived(xCSMSoftwareInfo);
//DbgPrintf("SonarIO: SIGNAL NewAnalogData");
// Always emit these, the SonarClient is responsible for the logic on whether to use them.
emit NewTemperature(csmAnalogData.m_TemperatureValue, csmAnalogData.m_ValidTemperature, SonarCommon::eWaterTemperatureInstance_1);
emit NewSpeed(csmAnalogData.m_SpeedValue, csmAnalogData.m_ValidSpeed);
emit NewTemperature(csmAnalogData.m_TemperatureValue, csmAnalogData.m_ValidTemperature, SonarCommon::eWaterTemperatureInstanceStructure);
// Get SonarColumnNumber
unsigned short SonarColumnNumber = 0;
SonarColumnNumber |= ((msg.data.at(CSM_SNR_COLUMN_NUM_INDEX + 0) & 0xFF) << 0);
SonarColumnNumber |= ((msg.data.at(CSM_SNR_COLUMN_NUM_INDEX + 1) & 0xFF) << 8);
if(m_SonarColumnNumber != (SonarColumnNumber - 1))
{
DbgPrintf("Lost Soundings = %d (%i,%i)", SonarColumnNumber - m_SonarColumnNumber, SonarColumnNumber, m_SonarColumnNumber);
}
m_SonarColumnNumber = SonarColumnNumber;
// Get the CSM Chart Data
tReceivedSonarInformation sonarInfo;
tReceivedStructureInformation structureInfo;
sonarInfo.m_ChartRangeCells.resize(CSM_CHART_DATA_SIZE);
memcpy(sonarInfo.m_ChartRangeCells.data(), (msg.data.data() + CSM_CHART_DATA_INDEX), CSM_CHART_DATA_SIZE);
Assert(xCSMSettings->UpperLimitFt() >= 0);
Assert(xCSMSettings->LowerLimitFt() > xCSMSettings->UpperLimitFt());
sonarInfo.m_DigitalRangeCells.resize(CSM_DIGITAL_DATA_SIZE);
memcpy(sonarInfo.m_DigitalRangeCells.data(), (msg.data.data() + CSM_DIGITAL_DATA_INDEX), CSM_DIGITAL_DATA_SIZE);
sonarInfo.m_NoiseRangeCells.resize(CSM_NOISE_WINDOW_SIZE);
memcpy(sonarInfo.m_NoiseRangeCells.data(), (msg.data.data() + CSM_NOISE_WINDOW_INDEX), CSM_NOISE_WINDOW_SIZE);
bool PrimaryFreq = xCSMSettings->IsPrimaryFrequency();
sonarInfo.m_UpperLimit = xCSMSettings->UpperLimitFt(PrimaryFreq);
sonarInfo.m_LowerLimit = xCSMSettings->LowerLimitFt(PrimaryFreq);
sonarInfo.m_DigitalLimit = xCSMSettings->DigitalLimitFt(PrimaryFreq);
sonarInfo.m_SonarFrequency = tSonarFrequencyAndChannel((eSonarFrequencyEnum)xCSMSettings->FrequencyIndex(PrimaryFreq), eSonarReceiver_High);
sonarInfo.m_SonarChannel = PrimaryFreq ? eSonarChannel_Primary : eSonarChannel_Secondary;
structureInfo.m_LeftRangecells.resize(CSM_SIDESCAN_DATA_SIZE);
memcpy(structureInfo.m_LeftRangecells.data(), (msg.data.data() + CSM_LEFT_DATA_INDEX), CSM_SIDESCAN_DATA_SIZE);
structureInfo.m_RightRangecells.resize(CSM_SIDESCAN_DATA_SIZE);
memcpy(structureInfo.m_RightRangecells.data(), (msg.data.data() + CSM_RIGHT_DATA_INDEX), CSM_SIDESCAN_DATA_SIZE);
structureInfo.m_DownRangecells.resize(CSM_DOWNSCAN_DATA_SIZE);
memcpy(structureInfo.m_DownRangecells.data(), (msg.data.data() + CSM_DOWN_DATA_INDEX), CSM_DOWNSCAN_DATA_SIZE);
structureInfo.m_DownRange = xCSMSettings->DownRangeFt();
structureInfo.m_SideRange = xCSMSettings->SideRangeFt();
structureInfo.m_StructureDownFrequency = tSonarFrequencyAndChannel((eSonarFrequencyEnum)xCSMSettings->StructureDownFrequency(), eSonarReceiver_High);
structureInfo.m_StructureSideFrequency = tSonarFrequencyAndChannel((eSonarFrequencyEnum)xCSMSettings->StructureSideFrequency(), eSonarReceiver_High);
structureInfo.m_XIDVoltage100x = xCSMSettings->StructureXIDVoltage100x();
//DbgPrintf("SonarIO: SIGNAL DataPacketReceived");
emit DataPacketReceived(xCSMSettings, sonarInfo, structureInfo);
if(xCSMSettings->NoCompensationDataMode() == true)
{
emit UnmodulatedDataReceived();
}
}
break;
#ifdef USE_REPROGRAMMING_CODE
case MSG_ID_REPROGRAM_MSG:
emit csmReprogramMessageBytesReceived(msg.data);
break;
#endif //USE_REPROGRAMMING_CODE
// We'll only receive this message when the CSM is configured as StopSonar... CSM needs to keep transmitting data
// because it's the master in the SPI bus
case tSPIDataProcessing::MSG_ID_CSM_DATA:
#ifdef USE_REPROGRAMMING_CODE
//if(m_Reprogramming == false)
#endif
{
tCSMSettingsXPtr xCSMSettings;
tCSMSoftwareInfoXPtr xCSMSoftwareInfo;
tCSMAnalogData csmAnalogData;
GetCSMData(msg.data, xCSMSettings, xCSMSoftwareInfo, csmAnalogData);
//~~~~~~ AC - Why would this happen? Need to make sure bogus messages are not sent!
if(xCSMSettings->LowerLimitFt() <= xCSMSettings->UpperLimitFt())
break;
emit csmSettingsReceived(xCSMSettings);
emit SoftwareInfoReceived(xCSMSoftwareInfo);
// Always emit these, the SonarClient is responsible for the logic on whether to use them.
emit NewTemperature(csmAnalogData.m_TemperatureValue, csmAnalogData.m_ValidTemperature, SonarCommon::eWaterTemperatureInstance_1);
emit NewSpeed(csmAnalogData.m_SpeedValue, csmAnalogData.m_ValidSpeed);
emit NewTemperature(csmAnalogData.m_TemperatureValue, csmAnalogData.m_ValidTemperature, SonarCommon::eWaterTemperatureInstanceStructure);
}
break;
default:
DbgPrintf("Sonar - invalid msg id: %i", msg.MessageID);
break;
}
}
void Drivetrain::Periodic(float moveLeftInput, float moveRightInput, bool enableMatchEncoders, bool STOP)
{
moveLeftInput = -moveLeftInput;
moveRightInput = -moveRightInput;
float leftInputSign = Sign(moveLeftInput);
float rightInputSign = Sign(moveRightInput);
//abs of inputs are within allowable tolerence and same sign
if (fabs(moveLeftInput - moveRightInput) <= allowableInputDifference && leftInputSign == rightInputSign)
{
// get difference in favor of left
float leftDiff = moveLeftInput - moveRightInput;
float leftDiffSign = Sign(leftDiff);
// when signs match.
if (leftInputSign == leftDiffSign)
{
moveRightInput += leftDiff;
}
// when they dont - trust us, the math works....
else
{
moveLeftInput -= leftDiff;
}
}
float newLeftSpeed = NewSpeed(leftMotorSetting, moveLeftInput);
float newRightSpeed = NewSpeed(rightMotorSetting, moveRightInput);
scaledLeft = ScaleDriving(newLeftSpeed);
scaledRight = ScaleDriving(newRightSpeed);
if ((leftInputSign == rightInputSign) && enableMatchEncoders)
{
int deltaLeft = leftCount - speedMatchLeftCounterStart;
int deltaRight = rightCount - speedMatchRightCounterStart;
//moving forward
if (leftInputSign > 0)
{
if (deltaLeft > deltaRight)
{
scaledLeft -= (deltaLeft - deltaRight) * ENCODER_FOLLOW_SCALE;
}
else
{
scaledRight -= (deltaRight - deltaLeft) * ENCODER_FOLLOW_SCALE;
}
}
//backward or stopped
else
{
//left is ahead of right!!!
if (deltaLeft < deltaRight)
{
scaledLeft -= (deltaLeft - deltaRight) * ENCODER_FOLLOW_SCALE;
}
else
{
scaledRight -= (deltaRight - deltaLeft) * ENCODER_FOLLOW_SCALE;
}
}
}
if (STOP)
{
newLeftSpeed = 0.0;
newRightSpeed = 0.0;
scaledLeft = 0.0;
scaledRight = 0.0;
}
RobotDrive::TankDrive(scaledLeft, scaledRight);
leftMotorSetting = newLeftSpeed;
rightMotorSetting = newRightSpeed;
DeadReckoner::Update();
}
