void CFootBotDistanceScannerRotZOnlySensor::UpdateNotRotating() {
/* Short range [0] */
CRadians cAngle = m_cLastDistScanRotation;
cAngle.SignedNormalize();
Real fReading = CalculateReadingForRay(m_cShortRangeRays0[0], SHORT_RANGE_MIN_DISTANCE);
m_tShortReadingsMap[cAngle] = fReading;
m_tReadingsMap[cAngle] = fReading;
/* Long range [1] */
cAngle += CRadians::PI_OVER_TWO;
cAngle.SignedNormalize();
fReading = CalculateReadingForRay(m_cLongRangeRays1[0], LONG_RANGE_MIN_DISTANCE);
m_tLongReadingsMap[cAngle] = fReading;
m_tReadingsMap[cAngle] = fReading;
/* Short range [2] */
cAngle += CRadians::PI_OVER_TWO;
cAngle.SignedNormalize();
fReading = CalculateReadingForRay(m_cShortRangeRays2[0], SHORT_RANGE_MIN_DISTANCE);
m_tShortReadingsMap[cAngle] = fReading;
m_tReadingsMap[cAngle] = fReading;
/* Long range [3] */
cAngle += CRadians::PI_OVER_TWO;
cAngle.SignedNormalize();
fReading = CalculateReadingForRay(m_cLongRangeRays3[0], LONG_RANGE_MIN_DISTANCE);
m_tLongReadingsMap[cAngle] = fReading;
m_tReadingsMap[cAngle] = fReading;
}
void CFootBotDistanceScannerRotZOnlySensor::UpdateRotating() {
CRadians cInterSensorSpan = (m_pcDistScanEntity->GetRotation() - m_cLastDistScanRotation).UnsignedNormalize() / 6.0f;
CRadians cStartAngle = m_cLastDistScanRotation;
/* Short range [0] */
CRadians cAngle = cStartAngle;
cAngle.SignedNormalize();
Real fReading = CalculateReadingForRay(m_cShortRangeRays0[0], SHORT_RANGE_MIN_DISTANCE);
m_tShortReadingsMap[cAngle] = fReading;
m_tReadingsMap[cAngle] = fReading;
ADD_READINGS(m_cShortRangeRays0, m_tShortReadingsMap, SHORT_RANGE_MIN_DISTANCE);
/* Short range [2] */
cAngle = cStartAngle + CRadians::PI;
cAngle.SignedNormalize();
fReading = CalculateReadingForRay(m_cShortRangeRays2[0], SHORT_RANGE_MIN_DISTANCE);
m_tShortReadingsMap[cAngle] = fReading;
m_tReadingsMap[cAngle] = fReading;
ADD_READINGS(m_cShortRangeRays2, m_tShortReadingsMap, SHORT_RANGE_MIN_DISTANCE);
/* Long range [1] */
cAngle = cStartAngle + CRadians::PI_OVER_TWO;
cAngle.SignedNormalize();
fReading = CalculateReadingForRay(m_cLongRangeRays1[0], LONG_RANGE_MIN_DISTANCE);
m_tLongReadingsMap[cAngle] = fReading;
m_tReadingsMap[cAngle] = fReading;
ADD_READINGS(m_cLongRangeRays1, m_tLongReadingsMap, LONG_RANGE_MIN_DISTANCE);
/* Long range [3] */
cAngle = cStartAngle + CRadians::PI_OVER_TWO + CRadians::PI;
cAngle.SignedNormalize();
fReading = CalculateReadingForRay(m_cLongRangeRays3[0], LONG_RANGE_MIN_DISTANCE);
m_tLongReadingsMap[cAngle] = fReading;
m_tReadingsMap[cAngle] = fReading;
ADD_READINGS(m_cLongRangeRays3, m_tLongReadingsMap, LONG_RANGE_MIN_DISTANCE);
}
void Agent::updateVelocity() {
CRadians delta = desideredAngle.SignedNormalize();
Real targetAngularSpeed = (1.0 / rotationTau) * delta.GetValue() * 0.5 * axisLength;
Real targetLinearSpeed = 0;
if (targetAngularSpeed > maxRotationSpeed) {
targetAngularSpeed = maxRotationSpeed;
} else if (targetAngularSpeed < -maxRotationSpeed) {
targetAngularSpeed = -maxRotationSpeed;
} else {
if (linearSpeedIsContinuos) {
targetLinearSpeed = desideredSpeed * (1 - fabs(targetAngularSpeed) / maxRotationSpeed);
} else {
targetLinearSpeed = desideredSpeed;
}
}
#ifdef ANGULAR_SPEED_DOMINATE
if (fabs(targetLinearSpeed) + fabs(targetAngularSpeed) > maxSpeed) {
if (targetLinearSpeed < 0) {
targetLinearSpeed = -maxSpeed + fabs(targetAngularSpeed);
} else {
targetLinearSpeed = maxSpeed - fabs(targetAngularSpeed);
}
}
#endif
leftWheelTargetSpeed = targetLinearSpeed - targetAngularSpeed;
rightWheelTargetSpeed = targetLinearSpeed + targetAngularSpeed;
//printf("%p TV L:%.3f, R:%.3f\n",this,leftWheelTargetSpeed,rightWheelTargetSpeed);
}
void CGroundSensorEquippedEntity::AddSensorRing(const CVector2& c_center,
Real f_radius,
const CRadians& c_start_angle,
ESensorType e_type,
UInt32 un_num_sensors,
const SAnchor& s_anchor) {
CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
CRadians cAngle;
CVector2 cOffset;
for(UInt32 i = 0; i < un_num_sensors; ++i) {
cAngle = c_start_angle + i * cSensorSpacing;
cAngle.SignedNormalize();
cOffset.Set(f_radius, 0.0f);
cOffset.Rotate(cAngle);
cOffset += c_center;
AddSensor(cOffset, e_type, s_anchor);
}
}
void CRandomWalkBehavior::Action(Real &fLeftWheelSpeed, Real &fRightWheelSpeed)
{
if (m_fChangeDirectionProbability >= m_sSensoryData.m_pcRNG->Uniform(CRange<Real>(0.0, 1.0)))
{
Real fSpeed;
CRadians angle = m_sSensoryData.m_pcRNG->Uniform(CRange<CRadians>(-CRadians::PI, CRadians::PI));
//fSpeed = angle.GetAbsoluteValue() * m_sRobotData.HALF_INTERWHEEL_DISTANCE / m_sRobotData.seconds_per_iterations;
fSpeed = angle.GetAbsoluteValue() * m_sRobotData.HALF_INTERWHEEL_DISTANCE;
fSpeed = fSpeed * 100.0; // converting to cm/s - as used in SetLinearVelocity
fSpeed = Min<Real>(fSpeed, m_sRobotData.MaxSpeed);
//std::cout << "fSpeed " << fSpeed << std::endl;
if(angle.GetValue() > 0.0f) //turn right
{
//std::cout << " turn right " << angle.GetValue() << std::endl;
fLeftWheelSpeed = fSpeed;
fRightWheelSpeed = -fSpeed;
}
else
{
//std::cout << " turn left " << angle.GetValue() << std::endl;
fLeftWheelSpeed = -fSpeed;
fRightWheelSpeed = fSpeed;
}
}
else
{
//std::cout << " move straight " << std::endl;
fLeftWheelSpeed = m_sRobotData.MaxSpeed;
fRightWheelSpeed = m_sRobotData.MaxSpeed;
}
//std::cout << "RavdLS: " << fLeftWheelSpeed << " RS: " << fRightWheelSpeed << std::endl;
}
void CProximitySensorEquippedEntity::AddSensorRing(const CVector3& c_center,
Real f_radius,
const CRadians& c_start_angle,
Real f_range,
UInt32 un_num_sensors,
const SAnchor& s_anchor) {
CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
CRadians cAngle;
CVector3 cOff, cDir;
for(UInt32 i = 0; i < un_num_sensors; ++i) {
cAngle = c_start_angle + i * cSensorSpacing;
cAngle.SignedNormalize();
cOff.Set(f_radius, 0.0f, 0.0f);
cOff.RotateZ(cAngle);
cOff += c_center;
cDir.Set(f_range, 0.0f, 0.0f);
cDir.RotateZ(cAngle);
AddSensor(cOff, cDir, f_range, s_anchor);
}
}
bool CCI_TriLaserSensor::SetAngle(CRadians angle, int clockwise){
float dif = 2; // tolerance
double targetAngle = angle.GetValue() * CRadians::RADIANS_TO_DEGREES;
double currentAngle = m_Sensor->GetAngle().GetValue() * CRadians::RADIANS_TO_DEGREES;
m_Sensor->SetRotationDirection(clockwise);
std::cout <<"c: " << currentAngle << ", t: " << targetAngle << std::endl;
if(currentAngle - dif < targetAngle && targetAngle < currentAngle + dif){
m_Actuator->SetRPM(0);
return false;
} else {
m_Actuator->SetRPM(5);
return true;
}
}
void CEPuckLightSensor::Update() {
/* Here we assume that the e-puck is rotated only wrt to the Z axis */
/* Erase readings */
for(size_t i = 0; i < m_tReadings.size(); ++i) {
m_tReadings[i].Value = 0.0f;
}
/* Get e-puck position */
const CVector3& cEPuckPosition = GetEntity().GetEmbodiedEntity().GetPosition();
/* Get e-puck orientation */
CRadians cTmp1, cTmp2, cOrientationZ;
GetEntity().GetEmbodiedEntity().GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2);
/* Buffer for calculating the light--e-puck distance */
CVector3 cLightDistance;
/* Buffer for the angle of the sensor wrt to the e-puck */
CRadians cLightAngle;
/* Initialize the occlusion check ray start to the baseline of the e-puck */
CRay cOcclusionCheckRay;
cOcclusionCheckRay.SetStart(cEPuckPosition);
/* Buffer to store the intersection data */
CSpace::SEntityIntersectionItem<CEmbodiedEntity> sIntersectionData;
/* Ignore the sensing ropuck when checking for occlusions */
TEmbodiedEntitySet tIgnoreEntities;
tIgnoreEntities.insert(&GetEntity().GetEmbodiedEntity());
/*
* 1. go through the list of light entities in the scene
* 2. check if a light is occluded
* 3. if it isn't, distribute the reading across the sensors
* NOTE: the readings are additive
* 4. go through the sensors and clamp their values
*/
try{
CSpace::TAnyEntityMap& tEntityMap = m_cSpace.GetEntitiesByType("light_entity");
for(CSpace::TAnyEntityMap::iterator it = tEntityMap.begin();
it != tEntityMap.end();
++it) {
/* Get a reference to the light */
CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second));
/* Consider the light only if it has non zero intensity */
if(cLight.GetIntensity() > 0.0f) {
/* Get the light position */
const CVector3& cLightPosition = cLight.GetPosition();
/* Set the ray end */
cOcclusionCheckRay.SetEnd(cLightPosition);
/* Check occlusion between the e-puck and the light */
if(! m_cSpace.GetClosestEmbodiedEntityIntersectedByRay(sIntersectionData,
cOcclusionCheckRay,
tIgnoreEntities)) {
/* The light is not occluded */
if(m_bShowRays) GetEntity().GetControllableEntity().AddCheckedRay(false, cOcclusionCheckRay);
/* Get the distance between the light and the e-puck */
cOcclusionCheckRay.ToVector(cLightDistance);
/* Linearly scale the distance with the light intensity
The greater the intensity, the smaller the distance */
cLightDistance /= cLight.GetIntensity();
/* Get the angle wrt to e-puck rotation */
cLightAngle = cLightDistance.GetZAngle();
cLightAngle -= cOrientationZ;
/* Transform it into counter-clockwise rotation */
cLightAngle.Negate().UnsignedNormalize();
/* Find reading corresponding to the sensor */
SInt16 nMin = 0;
for(SInt16 i = 1; i < NUM_READINGS; ++i){
if((cLightAngle - m_tReadings[i].Angle).GetAbsoluteValue() < (cLightAngle - m_tReadings[nMin].Angle).GetAbsoluteValue())
nMin = i;
}
/* Set the actual readings */
Real fReading = cLightDistance.Length();
m_tReadings[Modulo((SInt16)(nMin-1), NUM_READINGS)].Value += ComputeReading(fReading * Cos(cLightAngle - m_tReadings[Modulo(nMin-1, NUM_READINGS)].Angle));
m_tReadings[ nMin ].Value += ComputeReading(fReading);
m_tReadings[Modulo((SInt16)(nMin+1), NUM_READINGS)].Value += ComputeReading(fReading * Cos(cLightAngle - m_tReadings[Modulo(nMin+1, NUM_READINGS)].Angle));
}
else {
/* The ray is occluded */
if(m_bShowRays) {
GetEntity().GetControllableEntity().AddCheckedRay(true, cOcclusionCheckRay);
GetEntity().GetControllableEntity().AddIntersectionPoint(cOcclusionCheckRay, sIntersectionData.TOnRay);
}
}
}
}
}
catch(argos::CARGoSException& e){
}
/* Now go through the sensors, add noise and clamp their values if above 1024 or under 1024 */
for(size_t i = 0; i < m_tReadings.size(); ++i) {
if(m_fNoiseLevel>0.0f)
AddNoise(i);
if(m_tReadings[i].Value > 1024.0f)
m_tReadings[i].Value = 1024.0f;
if(m_tReadings[i].Value < 0.0f)
m_tReadings[i].Value = 0.0f;
}
}
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
const std::string& str_key,
const CRadians& c_angle) {
return TablePut(t_table, str_key, c_angle.GetValue());
}
buzzvm_state CBuzzController::Register(const std::string& str_key,
const CRadians& c_angle) {
return Register(str_key, c_angle.GetValue());
}
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
SInt32 n_idx,
const CRadians& c_angle) {
return TablePut(t_table, n_idx, c_angle.GetValue());
}
