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(); }