/**
* Run this Test
*/
void PWMBoardTest::spin()
{
double rate = 5.0d;
ros::Rate loop_rate(rate); //herz;
double pwmSpeed = 0.0d;
//Increment of each speed step
double stepSize = 0.25;
//Scale factor for setting speed at a lower rate
//then the loop
double modStepRate = 0.2;
//For determining how many times the
//loop has "looped"
double cntRate = 0.0d;
//Positive incrementing first
if (maxPWM > 0)
{
//First, climb to max value
bool toMax = true;
//Set initial speed
setSpeed(0, 0);
while (ros::ok())
{
//Set speed to pwm speed
setSpeed(pwmSpeed, pwmSpeed);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
//Check for errors
check_EM_STOP();
//Check if it is time for setting new speed
if (double_equals(cntRate, rate))
{
//Check if max is not reached
if (!(double_equals(pwmSpeed, maxPWM)) && (toMax)){
pwmSpeed += stepSize;
printf("Stepping up Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM);
}
//Max is reached
else
{
//Dont go to max value anymore
toMax = false;
//Check if 0 is not reached
if (!double_equals(pwmSpeed, 0.0))
{
setSpeed(pwmSpeed,pwmSpeed);
pwmSpeed -= stepSize;
printf("Stepping down Speed from here: %f %f \n", pwmSpeed, 0.0);
}
//0 is reached, done
else
{
printf("Done \n");
break;
}
}
//Reset countrate
cntRate = 0.0d;
std::cout << "cntrate " << cntRate << std::endl;
}
//Now it not the time for changing speed
else
{
//Increment times loop has looped
cntRate += modStepRate;
std::cout << "cntrate " << cntRate << std::endl;
}
loop_rate.sleep();
}
}
//Negative incrementing(decrementing) first
else
{
//First go to low value
bool toMin = true;
//Set initial speed
setSpeed(0, 0);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
while (ros::ok())
{
//Set speed to pwm speed
setSpeed(pwmSpeed, pwmSpeed);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
//Check for errors
check_EM_STOP();
//Check if it is time for setting new speed
if (double_equals(cntRate, rate))
{
//Check if min is not reached
if (!(double_equals(pwmSpeed, maxPWM)) && toMin){
pwmSpeed -= stepSize;
printf("Stepping down Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM);
}
//Min is reached
else
{
//Go only up now
toMin = false;
//Check if 0 is not reached
if (!double_equals(pwmSpeed, 0.0))
{
setSpeed(pwmSpeed,pwmSpeed);
pwmSpeed += stepSize;
printf("Stepping up Speed from here: %f %f \n", pwmSpeed, 0.0);
}
//0 is reached, done
else
{
printf("Done \n");
break;
}
}
//Reset countrate
cntRate = 0.0d;
std::cout << "cntrate " << cntRate << std::endl;
}
//Now it not the time for changing speed
else
{
//Increment times loop has looped
cntRate += modStepRate;
std::cout << "cntrate " << cntRate << std::endl;
}
loop_rate.sleep();
}
}
setSpeed(0, 0);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
}
void MyRobot::run()
{
double ir1_val = 0.0, ir14_val = 0.0;
double ir3_val = 0.0, ir12_val = 0.0;
double compass_angle;
while (step(_time_step) != -1) {
// Read the sensors
ir1_val = _distance_sensor[0]->getValue();
ir14_val = _distance_sensor[1]->getValue();
ir3_val = _distance_sensor[2]->getValue();
ir12_val = _distance_sensor[3]->getValue();
const double *compass_val = _my_compass->getValues();
compass_angle = convert_bearing_to_degrees(compass_val);
cout << "ds1: " << ir1_val << " ds14:" << ir14_val << endl;
cout << "ds3: " << ir3_val << " ds12:" << ir12_val << endl;
//Condition to choose between compass and sensor motion mode
if ((ir1_val < DISTANCE_LIMIT) && (ir14_val < DISTANCE_LIMIT)&& (ir12_val < DISTANCE_LIMIT)&& (ir3_val < DISTANCE_LIMIT)){
//Compass mode
if ( compass_angle < (DESIRED_ANGLE - 2)) {
_mode = TURN_RIGHT;
}
else{if(compass_angle > (DESIRED_ANGLE + 2)) {
_mode = TURN_LEFT;
}
else {
_mode = FORWARD;
}
}}else{
//Sensor mode
//Condition to stop and go backwards
if (((ir1_val > 800) || (ir14_val > 800)||(ir3_val > 800) || (ir12_val > 800)) && ((ir1_val != 0) || (ir14_val != 0))){
_mode = BACKWARDS;
cout << "Backwards." << endl;}
else{
//Condition to turn using sensors
if ((ir12_val > DISTANCE_LIMIT-50)|| (ir14_val > DISTANCE_LIMIT)) {
_mode = TURN_LEFT;
cout << "Turning left." << endl;
}
else {
if ((ir3_val > DISTANCE_LIMIT-50)||(ir1_val > DISTANCE_LIMIT)) {
_mode = TURN_RIGHT;
cout << "Turning right." << endl;
}
else {
_mode = FORWARD;
cout << "Moving forward." << endl;
}}}
}
// Send actuators commands according to the mode
switch (_mode){
case STOP:
_left_speed = 0;
_right_speed = 0;
break;
case FORWARD:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
break;
case TURN_LEFT:
_left_speed = MAX_SPEED/3.0;
_right_speed = MAX_SPEED;
break;
case TURN_RIGHT:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED/3.0;
break;
case BACKWARDS:
_left_speed = -MAX_SPEED;
_right_speed = -MAX_SPEED;
break;
default:
break;
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
/* Return Values:
0 = OK
1 = too manny pulse lengths
2 = abort
3 = too long
*/
unsigned char captureIR(struct ir_raw_* capture) {
unsigned long hightime, lowtime;
unsigned long T0count;
unsigned short highpulse, lowpulse;
unsigned long T1count;
unsigned long timeout;
unsigned char status;
unsigned char wide;
while(ANYKEY);
setSpeed(SPEED_60);
disableIRQ();
T0MCR = 0x00;
FIOSET0 |= (1<<4);
FIOCLR0 |= (1<<12);
PINSEL1 |= (1<<27);
T0CCR = (1<<9) | (1<<10) | (1<<11);
capture->widetable[0].count = 0;
capture->widetable[1].count = 0;
capture->count = 0;
status = 0;
T0TC = 0;
while (T0TC < 5000);
T0IR = 0xff;
while (!T0IR && !ANYKEY);
if(!ANYKEY) {
T0TC = 0;
T0IR = 0xff;
T0count = waitIrHi(&highpulse,&hightime,0);
capture->pulsetime = (hightime * 1000) / highpulse;
capture->data[0] = (decodewide(highpulse, &capture->widetable[1]) & 0x0F)<<4;
while (!(T0IR));
T0IR = 0xff;
T1count= T0CR3;
while ((capture->count < max_capture_data-1) && !status) {
lowtime = T1count-T0count;
T0count = waitIrHi(&highpulse,&hightime,T1count);
lowpulse = ((lowtime *1000)+(capture->pulsetime / 2)) / capture->pulsetime;
wide = decodewide(lowpulse, &capture->widetable[1]);
if(wide & 0xf0)
status = 2;
capture->data[capture->count] |= wide;
wide = decodewide(highpulse, &capture->widetable[1]);
if(wide & 0xf0)
status = 2;
capture->count++;
capture->data[capture->count] = wide<<4;
timeout = T0count + 500000;
while (!(T0IR) && !status) {
if (T0TC > timeout)
status =1;
}
T0IR = 0xff;
T1count= T0CR3;
}
}
else
status = 3;
if(capture->count == max_capture_data-1)
status = 4;
T0TC = 0;
T0MCR = 0x03;
FIOSET0 |= (1<<12);
T0CCR = 0;
T0IR = 0xff;
enableIRQ();
setBacklight(BL_AUTO);
setSpeed(SPEED_30);
return status - 1;
}
FirstPersonGravityCamera::FirstPersonGravityCamera() : FirstPersonCamera(glm::vec3(10,20,10), glm::vec3(0,0,0)){
setSpeed(20.0f);
}
/**
* Loads the craft from a YAML file.
* @param node YAML node.
* @param mod Mod for the saved game.
* @param save Pointer to the saved game.
*/
void Craft::load(const YAML::Node &node, const Mod *mod, SavedGame *save)
{
MovingTarget::load(node);
_id = node["id"].as<int>(_id);
_fuel = node["fuel"].as<int>(_fuel);
_damage = node["damage"].as<int>(_damage);
size_t j = 0;
for (YAML::const_iterator i = node["weapons"].begin(); i != node["weapons"].end(); ++i)
{
if (_rules->getWeapons() > j)
{
std::string type = (*i)["type"].as<std::string>();
if (type != "0" && mod->getCraftWeapon(type))
{
CraftWeapon *w = new CraftWeapon(mod->getCraftWeapon(type), 0);
w->load(*i);
_weapons[j] = w;
}
else
{
_weapons[j] = 0;
if (type != "0")
{
Log(LOG_ERROR) << "Failed to load craft weapon " << type;
}
}
j++;
}
}
_items->load(node["items"]);
// Some old saves have bad items, better get rid of them to avoid further bugs
for (std::map<std::string, int>::iterator i = _items->getContents()->begin(); i != _items->getContents()->end();)
{
if (mod->getItem(i->first) == 0)
{
Log(LOG_ERROR) << "Failed to load item " << i->first;
_items->getContents()->erase(i++);
}
else
{
++i;
}
}
for (YAML::const_iterator i = node["vehicles"].begin(); i != node["vehicles"].end(); ++i)
{
std::string type = (*i)["type"].as<std::string>();
if (mod->getItem(type))
{
Vehicle *v = new Vehicle(mod->getItem(type), 0, 4);
v->load(*i);
_vehicles.push_back(v);
}
else
{
Log(LOG_ERROR) << "Failed to load item " << type;
}
}
_status = node["status"].as<std::string>(_status);
_lowFuel = node["lowFuel"].as<bool>(_lowFuel);
_mission = node["mission"].as<bool>(_mission);
_interceptionOrder = node["interceptionOrder"].as<int>(_interceptionOrder);
if (const YAML::Node &dest = node["dest"])
{
std::string type = dest["type"].as<std::string>();
int id = dest["id"].as<int>();
if (type == "STR_BASE")
{
returnToBase();
}
else if (type == "STR_UFO")
{
for (std::vector<Ufo*>::iterator i = save->getUfos()->begin(); i != save->getUfos()->end(); ++i)
{
if ((*i)->getId() == id)
{
setDestination(*i);
break;
}
}
}
else if (type == "STR_WAYPOINT")
{
for (std::vector<Waypoint*>::iterator i = save->getWaypoints()->begin(); i != save->getWaypoints()->end(); ++i)
{
if ((*i)->getId() == id)
{
setDestination(*i);
break;
}
}
}
else if (type == "STR_ALIEN_BASE")
{
for (std::vector<AlienBase*>::iterator i = save->getAlienBases()->begin(); i != save->getAlienBases()->end(); ++i)
{
if ((*i)->getId() == id)
{
setDestination(*i);
break;
}
}
}
else
{
// Backwards compatibility
if (type == "STR_ALIEN_TERROR")
type = "STR_TERROR_SITE";
for (std::vector<MissionSite*>::iterator i = save->getMissionSites()->begin(); i != save->getMissionSites()->end(); ++i)
{
if ((*i)->getId() == id && (*i)->getDeployment()->getMarkerName() == type)
{
setDestination(*i);
break;
}
}
}
}
_takeoff = node["takeoff"].as<int>(_takeoff);
_inBattlescape = node["inBattlescape"].as<bool>(_inBattlescape);
if (_inBattlescape)
setSpeed(0);
}
bool Robot::processButton(int whichWay)
{
toggle(26);
switch (whichWay) {
case 'F': // forward
#ifdef DEBUG
Serial.println("Forward");
#endif
if (m_wheelLeft > m_wheelRight)
m_wheelRight = m_wheelLeft;
else if (m_wheelLeft < m_wheelRight)
m_wheelLeft = m_wheelRight;
else {
m_wheelLeft = m_wheelLeft + 32;
m_wheelRight = m_wheelRight + 32;
}
break;
case 'R': // right
#ifdef DEBUG
Serial.println("Right");
#endif
m_wheelLeft = m_wheelLeft + 16;
m_wheelRight = m_wheelRight - 16;
break;
case 'L': // left
#ifdef DEBUG
Serial.println("Left");
#endif
m_wheelLeft = m_wheelLeft - 16;
m_wheelRight = m_wheelRight + 16;
break;
case 'B': // reverse
#ifdef DEBUG
Serial.println("Reverse");
#endif
if(m_wheelLeft < m_wheelRight)
m_wheelRight = m_wheelLeft;
else if (m_wheelLeft > m_wheelRight)
m_wheelLeft = m_wheelRight;
else {
m_wheelLeft = m_wheelLeft - 32;
m_wheelRight = m_wheelRight - 32;
}
break;
case 'S': // stop
#ifdef DEBUG
Serial.println("Stop");
#endif
m_wheelLeft = 0;
m_wheelRight = 0;
break;
default: // unknown request
return false;
}
if (m_wheelLeft > 128) m_wheelLeft = 128;
if (m_wheelLeft < -128) m_wheelLeft = -128;
if (m_wheelRight > 128) m_wheelRight = 128;
if (m_wheelRight < -128) m_wheelRight = -128;
setSpeed(m_wheelLeft, m_wheelRight);
return true;
}
void LocalPlayer::applyControl(float dtime)
{
// Clear stuff
swimming_up = false;
// Random constants
f32 walk_acceleration = 4.0 * BS;
f32 walkspeed_max = 4.0 * BS;
setPitch(control.pitch);
setYaw(control.yaw);
v3f move_direction = v3f(0,0,1);
move_direction.rotateXZBy(getYaw());
v3f speed = v3f(0,0,0);
bool fly_allowed = m_gamedef->checkLocalPrivilege("fly");
bool fast_allowed = m_gamedef->checkLocalPrivilege("fast");
bool free_move = fly_allowed && g_settings->getBool("free_move");
bool fast_move = fast_allowed && g_settings->getBool("fast_move");
bool continuous_forward = g_settings->getBool("continuous_forward");
if(free_move || is_climbing)
{
v3f speed = getSpeed();
speed.Y = 0;
setSpeed(speed);
}
// Whether superspeed mode is used or not
bool superspeed = false;
// If free movement and fast movement, always move fast
if(free_move && fast_move)
superspeed = true;
// Auxiliary button 1 (E)
if(control.aux1)
{
if(free_move)
{
// In free movement mode, aux1 descends
v3f speed = getSpeed();
if(fast_move)
speed.Y = -20*BS;
else
speed.Y = -walkspeed_max;
setSpeed(speed);
}
else if(is_climbing)
{
v3f speed = getSpeed();
speed.Y = -3*BS;
setSpeed(speed);
}
else
{
// If not free movement but fast is allowed, aux1 is
// "Turbo button"
if(fast_move)
superspeed = true;
}
}
if(continuous_forward)
speed += move_direction;
if(control.up)
{
if(continuous_forward)
superspeed = true;
else
speed += move_direction;
}
if(control.down)
{
speed -= move_direction;
}
if(control.left)
{
speed += move_direction.crossProduct(v3f(0,1,0));
}
if(control.right)
{
speed += move_direction.crossProduct(v3f(0,-1,0));
}
if(control.jump)
{
if(free_move)
{
v3f speed = getSpeed();
if(fast_move)
speed.Y = 20*BS;
else
speed.Y = walkspeed_max;
setSpeed(speed);
}
else if(touching_ground)
{
/*
NOTE: The d value in move() affects jump height by
raising the height at which the jump speed is kept
at its starting value
*/
v3f speed = getSpeed();
if(speed.Y >= -0.5*BS)
{
speed.Y = 6.5*BS;
setSpeed(speed);
MtEvent *e = new SimpleTriggerEvent("PlayerJump");
m_gamedef->event()->put(e);
}
}
// Use the oscillating value for getting out of water
// (so that the player doesn't fly on the surface)
else if(in_water)
{
v3f speed = getSpeed();
speed.Y = 1.5*BS;
setSpeed(speed);
swimming_up = true;
}
else if(is_climbing)
{
v3f speed = getSpeed();
speed.Y = 3*BS;
setSpeed(speed);
}
}
// The speed of the player (Y is ignored)
if(superspeed)
speed = speed.normalize() * walkspeed_max * 5.0;
else if(control.sneak)
speed = speed.normalize() * walkspeed_max / 3.0;
else
speed = speed.normalize() * walkspeed_max;
f32 inc = walk_acceleration * BS * dtime;
// Faster acceleration if fast and free movement
if(free_move && fast_move)
inc = walk_acceleration * BS * dtime * 10;
// Accelerate to target speed with maximum increment
accelerate(speed, inc);
}
bool CCParticleExplosion::initWithTotalParticles(unsigned int numberOfParticles)
{
if( CCParticleSystemQuad::initWithTotalParticles(numberOfParticles) )
{
// duration
m_fDuration = 0.1f;
setEmitterMode(kCCParticleModeGravity);
// Gravity Mode: gravity
setGravity(ccp(0,0));
// Gravity Mode: speed of particles
setSpeed(70);
setSpeedVar(40);
// Gravity Mode: radial
setRadialAccel(0);
setRadialAccelVar(0);
// Gravity Mode: tangential
setTangentialAccel(0);
setTangentialAccelVar(0);
// angle
m_fAngle = 90;
m_fAngleVar = 360;
// emitter position
CCSize winSize = CCDirector::sharedDirector()->getWinSize();
this->setPosition(ccp(winSize.width/2, winSize.height/2));
setPosVar(CCPointZero);
// life of particles
m_fLife = 5.0f;
m_fLifeVar = 2;
// size, in pixels
m_fStartSize = 15.0f;
m_fStartSizeVar = 10.0f;
m_fEndSize = kCCParticleStartSizeEqualToEndSize;
// emits per second
m_fEmissionRate = m_uTotalParticles/m_fDuration;
// color of particles
m_tStartColor.r = 0.7f;
m_tStartColor.g = 0.1f;
m_tStartColor.b = 0.2f;
m_tStartColor.a = 1.0f;
m_tStartColorVar.r = 0.5f;
m_tStartColorVar.g = 0.5f;
m_tStartColorVar.b = 0.5f;
m_tStartColorVar.a = 0.0f;
m_tEndColor.r = 0.5f;
m_tEndColor.g = 0.5f;
m_tEndColor.b = 0.5f;
m_tEndColor.a = 0.0f;
m_tEndColorVar.r = 0.5f;
m_tEndColorVar.g = 0.5f;
m_tEndColorVar.b = 0.5f;
m_tEndColorVar.a = 0.0f;
CCTexture2D* pTexture = getDefaultTexture();
if (pTexture != NULL)
{
setTexture(pTexture);
}
// additive
this->setBlendAdditive(false);
return true;
}
return false;
}
bool CCParticleSmoke::initWithTotalParticles(unsigned int numberOfParticles)
{
if( CCParticleSystemQuad::initWithTotalParticles(numberOfParticles) )
{
// duration
m_fDuration = kCCParticleDurationInfinity;
// Emitter mode: Gravity Mode
setEmitterMode(kCCParticleModeGravity);
// Gravity Mode: gravity
setGravity(ccp(0,0));
// Gravity Mode: radial acceleration
setRadialAccel(0);
setRadialAccelVar(0);
// Gravity Mode: speed of particles
setSpeed(25);
setSpeedVar(10);
// angle
m_fAngle = 90;
m_fAngleVar = 5;
// emitter position
CCSize winSize = CCDirector::sharedDirector()->getWinSize();
this->setPosition(ccp(winSize.width/2, 0));
setPosVar(ccp(20, 0));
// life of particles
m_fLife = 4;
m_fLifeVar = 1;
// size, in pixels
m_fStartSize = 60.0f;
m_fStartSizeVar = 10.0f;
m_fEndSize = kCCParticleStartSizeEqualToEndSize;
// emits per frame
m_fEmissionRate = m_uTotalParticles/m_fLife;
// color of particles
m_tStartColor.r = 0.8f;
m_tStartColor.g = 0.8f;
m_tStartColor.b = 0.8f;
m_tStartColor.a = 1.0f;
m_tStartColorVar.r = 0.02f;
m_tStartColorVar.g = 0.02f;
m_tStartColorVar.b = 0.02f;
m_tStartColorVar.a = 0.0f;
m_tEndColor.r = 0.0f;
m_tEndColor.g = 0.0f;
m_tEndColor.b = 0.0f;
m_tEndColor.a = 1.0f;
m_tEndColorVar.r = 0.0f;
m_tEndColorVar.g = 0.0f;
m_tEndColorVar.b = 0.0f;
m_tEndColorVar.a = 0.0f;
CCTexture2D* pTexture = getDefaultTexture();
if (pTexture != NULL)
{
setTexture(pTexture);
}
// additive
this->setBlendAdditive(false);
return true;
}
return false;
}
void LocalPlayer::applyControl(float dtime, ClientEnvironment *env)
{
// Clear stuff
swimming_vertical = false;
setPitch(control.pitch);
setYaw(control.yaw);
// Nullify speed and don't run positioning code if the player is attached
if(isAttached)
{
setSpeed(v3f(0,0,0));
return;
}
v3f move_direction = v3f(0,0,1);
move_direction.rotateXZBy(getYaw());
v3f speedH = v3f(0,0,0); // Horizontal (X, Z)
v3f speedV = v3f(0,0,0); // Vertical (Y)
bool fly_allowed = m_gamedef->checkLocalPrivilege("fly");
bool fast_allowed = m_gamedef->checkLocalPrivilege("fast");
free_move = fly_allowed && g_settings->getBool("free_move");
bool fast_move = fast_allowed && g_settings->getBool("fast_move");
// When aux1_descends is enabled the fast key is used to go down, so fast isn't possible
bool fast_climb = fast_move && control.aux1 && !g_settings->getBool("aux1_descends");
bool continuous_forward = g_settings->getBool("continuous_forward");
bool fast_pressed = false;
bool always_fly_fast = g_settings->getBool("always_fly_fast");
// Whether superspeed mode is used or not
superspeed = false;
if (always_fly_fast && free_move && fast_move)
superspeed = true;
// Old descend control
if(g_settings->getBool("aux1_descends"))
{
// If free movement and fast movement, always move fast
if(free_move && fast_move)
superspeed = true;
// Auxiliary button 1 (E)
if(control.aux1)
{
if(free_move)
{
// In free movement mode, aux1 descends
if(fast_move)
speedV.Y = -movement_speed_fast;
else
speedV.Y = -movement_speed_walk;
}
else if(in_liquid || in_liquid_stable)
{
speedV.Y = -movement_speed_walk;
swimming_vertical = true;
}
else if(is_climbing)
{
speedV.Y = -movement_speed_climb;
}
else
{
// If not free movement but fast is allowed, aux1 is
// "Turbo button"
if(fast_allowed)
superspeed = true;
}
}
}
// New minecraft-like descend control
else
{
// Auxiliary button 1 (E)
if(control.aux1)
{
if(!is_climbing)
{
// aux1 is "Turbo button"
if(fast_allowed)
superspeed = true;
}
if(fast_allowed)
fast_pressed = true;
}
if(control.sneak)
{
if(free_move)
{
// In free movement mode, sneak descends
if (fast_move && (control.aux1 || always_fly_fast))
speedV.Y = -movement_speed_fast;
else
speedV.Y = -movement_speed_walk;
}
else if(in_liquid || in_liquid_stable)
{
if(fast_climb)
speedV.Y = -movement_speed_fast;
else
speedV.Y = -movement_speed_walk;
swimming_vertical = true;
}
else if(is_climbing)
{
if(fast_climb)
speedV.Y = -movement_speed_fast;
else
speedV.Y = -movement_speed_climb;
}
}
}
if (continuous_forward)
speedH += move_direction;
if (control.up) {
if (continuous_forward) {
if (fast_move)
superspeed = true;
} else {
speedH += move_direction;
}
}
if(control.down)
{
speedH -= move_direction;
}
if(control.left)
{
speedH += move_direction.crossProduct(v3f(0,1,0));
}
if(control.right)
{
speedH += move_direction.crossProduct(v3f(0,-1,0));
}
if(control.jump)
{
if (free_move) {
if (g_settings->getBool("aux1_descends") || always_fly_fast) {
if (fast_move)
speedV.Y = movement_speed_fast;
else
speedV.Y = movement_speed_walk;
} else {
if(fast_move && control.aux1)
speedV.Y = movement_speed_fast;
else
speedV.Y = movement_speed_walk;
}
}
else if(m_can_jump)
{
/*
NOTE: The d value in move() affects jump height by
raising the height at which the jump speed is kept
at its starting value
*/
v3f speedJ = getSpeed();
if(speedJ.Y >= -0.5 * BS)
{
speedJ.Y = movement_speed_jump * physics_override_jump;
setSpeed(speedJ);
MtEvent *e = new SimpleTriggerEvent("PlayerJump");
m_gamedef->event()->put(e);
}
}
else if(in_liquid)
{
if(fast_climb)
speedV.Y = movement_speed_fast;
else
speedV.Y = movement_speed_walk;
swimming_vertical = true;
}
else if(is_climbing)
{
if(fast_climb)
speedV.Y = movement_speed_fast;
else
speedV.Y = movement_speed_climb;
}
}
// The speed of the player (Y is ignored)
if(superspeed || (is_climbing && fast_climb) || ((in_liquid || in_liquid_stable) && fast_climb) || fast_pressed)
speedH = speedH.normalize() * movement_speed_fast;
else if(control.sneak && !free_move && !in_liquid && !in_liquid_stable)
speedH = speedH.normalize() * movement_speed_crouch;
else
speedH = speedH.normalize() * movement_speed_walk;
// Acceleration increase
f32 incH = 0; // Horizontal (X, Z)
f32 incV = 0; // Vertical (Y)
if((!touching_ground && !free_move && !is_climbing && !in_liquid) || (!free_move && m_can_jump && control.jump))
{
// Jumping and falling
if(superspeed || (fast_move && control.aux1))
incH = movement_acceleration_fast * BS * dtime;
else
incH = movement_acceleration_air * BS * dtime;
incV = 0; // No vertical acceleration in air
// better air control when falling fast
float speed = m_speed.getLength();
if (!superspeed && speed > movement_speed_fast && (control.down || control.up || control.left || control.right)) {
v3f rotate = move_direction * (speed / (movement_fall_aerodynamics * BS));
if(control.up) rotate = rotate.crossProduct(v3f(0,1,0));
if(control.down) rotate = rotate.crossProduct(v3f(0,-1,0));
if(control.left) rotate *=-1;
m_speed.rotateYZBy(rotate.X);
m_speed.rotateXZBy(rotate.Y);
m_speed.rotateXYBy(rotate.Z);
m_speed = m_speed.normalize() * speed * (1-speed*0.00001); // 0.998
if (m_speed.Y)
return;
}
}
else if (superspeed || (is_climbing && fast_climb) || ((in_liquid || in_liquid_stable) && fast_climb))
incH = incV = movement_acceleration_fast * BS * dtime;
else
incH = incV = movement_acceleration_default * BS * dtime;
// Accelerate to target speed with maximum increment
INodeDefManager *nodemgr = m_gamedef->ndef();
Map *map = &env->getMap();
v3s16 p = floatToInt(getPosition() - v3f(0,BS/2,0), BS);
float slippery = 0;
try {
slippery = itemgroup_get(nodemgr->get(map->getNode(p)).groups, "slippery");
}
catch (...) {}
accelerateHorizontal(speedH * physics_override_speed, incH * physics_override_speed, slippery);
accelerateVertical(speedV * physics_override_speed, incV * physics_override_speed);
}
void LocalPlayer::move(f32 dtime, Environment *env, f32 pos_max_d,
std::vector<CollisionInfo> *collision_info)
{
Map *map = &env->getMap();
INodeDefManager *nodemgr = m_gamedef->ndef();
v3f position = getPosition();
//v3f old_speed = m_speed;
// Copy parent position if local player is attached
if(isAttached)
{
setPosition(overridePosition);
m_sneak_node_exists = false;
return;
}
// Skip collision detection if noclip mode is used
bool fly_allowed = m_gamedef->checkLocalPrivilege("fly");
bool noclip = m_gamedef->checkLocalPrivilege("noclip") &&
g_settings->getBool("noclip");
bool free_move = noclip && fly_allowed && g_settings->getBool("free_move");
if (free_move) {
position += m_speed * dtime;
setPosition(position);
m_sneak_node_exists = false;
return;
}
/*
Collision detection
*/
bool is_valid_position;
MapNode node;
v3s16 pp;
/*
Check if player is in liquid (the oscillating value)
*/
// If in liquid, the threshold of coming out is at higher y
if (in_liquid)
{
// If not in liquid, the threshold of going in is at lower y
pp = floatToInt(position + v3f(0,BS*(in_liquid ? 0.1 : 0.5),0), BS);
node = map->getNodeNoEx(pp, &is_valid_position);
if (is_valid_position) {
auto f = nodemgr->get(node.getContent());
in_liquid = f.isLiquid();
liquid_viscosity = f.liquid_viscosity;
if (f.param_type_2 == CPT2_LEVELED) {
float level = node.getLevel(nodemgr);
float maxlevel = node.getMaxLevel(nodemgr);
if (level && maxlevel && level < maxlevel)
liquid_viscosity /= maxlevel / level;
}
} else {
in_liquid = false;
}
}
// If not in liquid, the threshold of going in is at lower y
else
{
pp = floatToInt(position + v3f(0,BS*0.5,0), BS);
node = map->getNodeNoEx(pp, &is_valid_position);
if (is_valid_position) {
in_liquid = nodemgr->get(node.getContent()).isLiquid();
liquid_viscosity = nodemgr->get(node.getContent()).liquid_viscosity;
} else {
in_liquid = false;
}
}
/*
Check if player is in liquid (the stable value)
*/
pp = floatToInt(position + v3f(0,0,0), BS);
node = map->getNodeNoEx(pp, &is_valid_position);
if (is_valid_position) {
in_liquid_stable = nodemgr->get(node.getContent()).isLiquid();
} else {
in_liquid_stable = false;
}
/*
Check if player is climbing
*/
pp = floatToInt(position + v3f(0,0.5*BS,0), BS);
v3s16 pp2 = floatToInt(position + v3f(0,-0.2*BS,0), BS);
node = map->getNodeNoEx(pp, &is_valid_position);
bool is_valid_position2;
MapNode node2 = map->getNodeNoEx(pp2, &is_valid_position2);
if (!(is_valid_position && is_valid_position2)) {
is_climbing = false;
} else {
bool can_climbing = (nodemgr->get(node.getContent()).climbable
|| nodemgr->get(node2.getContent()).climbable) && !free_move;
if (m_speed.Y >= -PLAYER_FALL_TOLERANCE_SPEED)
is_climbing = can_climbing;
else if (can_climbing)
m_speed.Y += 0.3*BS;
}
/*
Collision uncertainty radius
Make it a bit larger than the maximum distance of movement
*/
//f32 d = pos_max_d * 1.1;
// A fairly large value in here makes moving smoother
//f32 d = 0.15*BS;
// This should always apply, otherwise there are glitches
//sanity_check(d > pos_max_d);
// Maximum distance over border for sneaking
f32 sneak_max = BS*0.4;
/*
If sneaking, keep in range from the last walked node and don't
fall off from it
*/
if (control.sneak && m_sneak_node_exists &&
!(fly_allowed && g_settings->getBool("free_move")) && !in_liquid &&
physics_override_sneak && !got_teleported) {
f32 maxd = 0.5 * BS + sneak_max;
v3f lwn_f = intToFloat(m_sneak_node, BS);
auto old_pos = position;
position.X = rangelim(position.X, lwn_f.X-maxd, lwn_f.X+maxd);
position.Z = rangelim(position.Z, lwn_f.Z-maxd, lwn_f.Z+maxd);
if (old_pos != position) {
m_speed.X = rangelim(m_speed.X, -movement_speed_climb, movement_speed_climb);
m_speed.Z = rangelim(m_speed.Z, -movement_speed_climb, movement_speed_climb);
}
if (!is_climbing) {
// Move up if necessary
f32 new_y = (lwn_f.Y - 0.5 * BS) + m_sneak_node_bb_ymax;
if (position.Y < new_y && m_speed.Y >= -PLAYER_FALL_TOLERANCE_SPEED)
position.Y = new_y;
/*
Collision seems broken, since player is sinking when
sneaking over the edges of current sneaking_node.
TODO (when fixed): Set Y-speed only to 0 when position.Y < new_y.
*/
if (m_speed.Y < -PLAYER_FALL_TOLERANCE_SPEED)
m_speed.Y += 0.3*BS;
else
if (m_speed.Y < 0)
m_speed.Y = 0;
}
}
if (got_teleported)
got_teleported = false;
// this shouldn't be hardcoded but transmitted from server
float player_stepheight = touching_ground ? (BS*0.6) : (BS*0.2);
static const auto autojump = g_settings->getBool("autojump");
if (control.aux1 || autojump) {
player_stepheight += (0.6 * BS);
}
v3f accel_f = v3f(0,0,0);
collisionMoveResult result = collisionMoveSimple(env, m_gamedef,
pos_max_d, m_collisionbox, player_stepheight, dtime,
&position, &m_speed, accel_f);
/*
If the player's feet touch the topside of any node, this is
set to true.
Player is allowed to jump when this is true.
*/
bool touching_ground_was = touching_ground;
touching_ground = result.touching_ground;
//bool standing_on_unloaded = result.standing_on_unloaded;
/*
Check the nodes under the player to see from which node the
player is sneaking from, if any. If the node from under
the player has been removed, the player falls.
*/
f32 position_y_mod = 0.05 * BS;
if (m_sneak_node_bb_ymax > 0)
position_y_mod = m_sneak_node_bb_ymax - position_y_mod;
v3s16 current_node = floatToInt(position - v3f(0, position_y_mod, 0), BS);
if (m_sneak_node_exists &&
nodemgr->get(map->getNodeNoEx(m_old_node_below)).name == "air" &&
m_old_node_below_type != "air") {
// Old node appears to have been removed; that is,
// it wasn't air before but now it is
m_need_to_get_new_sneak_node = false;
m_sneak_node_exists = false;
} else if (nodemgr->get(map->getNodeNoEx(current_node)).name != "air") {
// We are on something, so make sure to recalculate the sneak
// node.
m_need_to_get_new_sneak_node = true;
}
if (m_need_to_get_new_sneak_node && physics_override_sneak) {
m_sneak_node_bb_ymax = 0;
v3s16 pos_i_bottom = floatToInt(position - v3f(0, position_y_mod, 0), BS);
v2f player_p2df(position.X, position.Z);
f32 min_distance_f = 100000.0 * BS;
// If already seeking from some node, compare to it.
/*if(m_sneak_node_exists)
{
v3f sneaknode_pf = intToFloat(m_sneak_node, BS);
v2f sneaknode_p2df(sneaknode_pf.X, sneaknode_pf.Z);
f32 d_horiz_f = player_p2df.getDistanceFrom(sneaknode_p2df);
f32 d_vert_f = fabs(sneaknode_pf.Y + BS*0.5 - position.Y);
// Ignore if player is not on the same level (likely dropped)
if(d_vert_f < 0.15*BS)
min_distance_f = d_horiz_f;
}*/
v3s16 new_sneak_node = m_sneak_node;
for(s16 x=-1; x<=1; x++)
for(s16 z=-1; z<=1; z++)
{
v3s16 p = pos_i_bottom + v3s16(x,0,z);
v3f pf = intToFloat(p, BS);
v2f node_p2df(pf.X, pf.Z);
f32 distance_f = player_p2df.getDistanceFrom(node_p2df);
f32 max_axis_distance_f = MYMAX(
fabs(player_p2df.X-node_p2df.X),
fabs(player_p2df.Y-node_p2df.Y));
if(distance_f > min_distance_f ||
max_axis_distance_f > 0.5*BS + sneak_max + 0.1*BS)
continue;
// The node to be sneaked on has to be walkable
node = map->getNodeNoEx(p, &is_valid_position);
if (!is_valid_position || nodemgr->get(node).walkable == false)
continue;
// And the node above it has to be nonwalkable
node = map->getNodeNoEx(p + v3s16(0,1,0), &is_valid_position);
if (!is_valid_position || nodemgr->get(node).walkable) {
continue;
}
if (!physics_override_sneak_glitch) {
node =map->getNodeNoEx(p + v3s16(0,2,0), &is_valid_position);
if (!is_valid_position || nodemgr->get(node).walkable)
continue;
}
min_distance_f = distance_f;
new_sneak_node = p;
}
bool sneak_node_found = (min_distance_f < 100000.0 * BS * 0.9);
m_sneak_node = new_sneak_node;
m_sneak_node_exists = sneak_node_found;
if (sneak_node_found) {
f32 cb_max = 0;
MapNode n = map->getNodeNoEx(m_sneak_node);
std::vector<aabb3f> nodeboxes;
n.getCollisionBoxes(nodemgr, &nodeboxes);
for (std::vector<aabb3f>::iterator it = nodeboxes.begin();
it != nodeboxes.end(); ++it) {
aabb3f box = *it;
if (box.MaxEdge.Y > cb_max)
cb_max = box.MaxEdge.Y;
}
m_sneak_node_bb_ymax = cb_max;
}
/*
If sneaking, the player's collision box can be in air, so
this has to be set explicitly
*/
if(sneak_node_found && control.sneak)
touching_ground = true;
}
/*
Set new position
*/
setPosition(position);
/*
Report collisions
*/
// Dont report if flying
if(collision_info && !(g_settings->getBool("free_move") && fly_allowed)) {
for(size_t i=0; i<result.collisions.size(); i++) {
const CollisionInfo &info = result.collisions[i];
collision_info->push_back(info);
}
}
if(!result.standing_on_object && !touching_ground_was && touching_ground) {
MtEvent *e = new SimpleTriggerEvent("PlayerRegainGround");
m_gamedef->event()->put(e);
// Set camera impact value to be used for view bobbing
camera_impact = getSpeed().Y * -1;
}
{
camera_barely_in_ceiling = false;
v3s16 camera_np = floatToInt(getEyePosition(), BS);
MapNode n = map->getNodeNoEx(camera_np);
if(n.getContent() != CONTENT_IGNORE){
if(nodemgr->get(n).walkable && nodemgr->get(n).solidness == 2){
camera_barely_in_ceiling = true;
}
}
}
/*
Update the node last under the player
*/
m_old_node_below = floatToInt(position - v3f(0,BS/2,0), BS);
m_old_node_below_type = nodemgr->get(map->getNodeNoEx(m_old_node_below)).name;
/*
Check properties of the node on which the player is standing
*/
const ContentFeatures &f = nodemgr->get(map->getNodeNoEx(getStandingNodePos()));
// Determine if jumping is possible
m_can_jump = touching_ground && !in_liquid;
if(itemgroup_get(f.groups, "disable_jump"))
m_can_jump = false;
// Jump key pressed while jumping off from a bouncy block
if (m_can_jump && control.jump && itemgroup_get(f.groups, "bouncy") &&
m_speed.Y >= -0.5 * BS) {
float jumpspeed = movement_speed_jump * physics_override_jump;
if (m_speed.Y > 1) {
// Reduce boost when speed already is high
m_speed.Y += jumpspeed / (1 + (m_speed.Y / 16 ));
} else {
m_speed.Y += jumpspeed;
}
setSpeed(m_speed);
m_can_jump = false;
}
}
int main(void)
{
/* Configure Oscillator to operate the device at 30Mhz
Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
Fosc= 7.37*(32)/(2*2)=58.96Mhz for Fosc, Fcy = 29.48Mhz */
/* Configure PLL prescaler, PLL postscaler, PLL divisor */
//PLLFBDbits.PLLDIV=38; /* M = PLLFBD + 2 */ // izlazna frekvencija = 30Mhz
//Fin=8MHz, Fcy=30MHz
// Configure PLL prescaler, PLL postscaler, PLL divisor
PLLFBD = 28; // M=40 ---> PLLFBD + 2 = M
CLKDIVbits.PLLPOST = 0; // N2=2 ---> 2x(PLLPOST + 2) = N2
CLKDIVbits.PLLPRE = 0; // N1=2 ---> PLLPRE + 2 = N1
//new oscillator selection
__builtin_write_OSCCONH(0b011); //0b011 ---> XT with PLL
//enable oscillator source switch
__builtin_write_OSCCONL (OSCCONL | (1<<0)); //OSWEN
//wait for PLL lock -> wait to new settings become available
while (OSCCONbits.COSC != 0b011);
//wait for PLL lock
while (OSCCONbits.LOCK != 0b1);
AD1PCFGL = 0xFFFF;// all PORT Digital
RPINR18bits.U1RXR = 0; //UART1 RX na RP0- pin 4
RPOR0bits.RP1R = 3; //UART1 TX na RP1- pin 5
RPINR14bits.QEA1R = 2; //QEI1A na RP2
RPINR14bits.QEB1R = 3; //QEI1B na RP3
RPINR16bits.QEA2R = 4; //QEI2A na RP4
RPINR16bits.QEB2R = 7; //QEI2B na RP7
CAN_init(DRIVER_IDENTIFICATOR); // inicijalizacija CAN BUS- a-> argument je adresa drajvera
int tmp;
char komanda, v, smer;
int Xc, Yc, ugao;
NewLine();
PortInit();
//UARTinit();
TimerInit();
QEIinit();
PWMinit();
// CloseMCPWM();
resetDriver();
setSpeed(0x80);
setSpeedAccel(K2); //K2 je za 1m/s /bilo je 2
int tmpX, tmpY, tmpO;
unsigned char rxBuffer[8];
while(1)
{
__delay_ms(1000);
setSpeed(30);
// kretanje_pravo(-1000, 0);
if(getStatus() == STATUS_MOVING)
CAN_getLastMessage(rxBuffer);
else
CAN_read(rxBuffer);
komanda = rxBuffer[0];
switch(komanda)
{
// zadavanje pozicije
case 'I':
tmpX = rxBuffer[1] << 8;
tmpX |= rxBuffer[2];
tmpY = rxBuffer[3] << 8;
tmpY |= rxBuffer[4];
tmpO = rxBuffer[5] << 8;
tmpO |= rxBuffer[6];
setPosition(tmpX, tmpY, tmpO);
break;
// citanje pozicije i statusa
case 'P':
sendStatusAndPosition();
break;
//zadavanje max. brzine (default K2/2)
case 'V':
tmp = rxBuffer[1];
setSpeed(tmp);
break;
//kretanje pravo [mm]
case 'D':
tmp = rxBuffer[1] << 8;
tmp |= rxBuffer[2];
v = rxBuffer[3];
PWMinit();
kretanje_pravo(tmp, v);
break;
//relativni ugao [stepen]
case 'T':
tmp = rxBuffer[1] << 8;
tmp |= rxBuffer[2];
PWMinit();
okret(tmp);
break;
//apsolutni ugao [stepen]
case 'A':
tmp = rxBuffer[1] << 8;
tmp |= rxBuffer[2];
PWMinit();
apsolutni_ugao(tmp);
break;
//idi u tacku (Xc, Yc) [mm]
case 'G':
tmpX = rxBuffer[1] << 8;
tmpX |= rxBuffer[2];
tmpY = rxBuffer[3] << 8;
tmpY |= rxBuffer[4];
v = rxBuffer[5];
smer = rxBuffer[6];
PWMinit();
gotoXY(tmpX, tmpY, v, smer);
break;
//kurva
case 'Q':
tmpX = rxBuffer[1] << 8;
tmpX |= rxBuffer[2];
tmpY = rxBuffer[3] << 8;
tmpY |= rxBuffer[4];
tmpO = rxBuffer[5] << 8;
tmpO |= rxBuffer[6];
smer = rxBuffer[7];
PWMinit();
kurva(tmpX, tmpY, tmpO, smer);
break;
//ukopaj se u mestu
case 'S':
stop();
break;
//stani i ugasi PWM
case 's':
stop();
CloseMCPWM();
break;
case 'R':
resetDriver();
break;
default:
forceStatus(STATUS_ERROR);
break;
}
}
return 0;
}
void GLWorld::mousePressEvent(QMouseEvent *event)
{
last_pos = event->pos();
if(!(event->buttons() & Qt::RightButton && current_selection.type != TYPE_NOTHING && editable))
return;
WorldObject &here = getObject(current_selection.coords);
bool steve_is_here = current_selection.coords == steve;
QPoint pos = mapToGlobal(event->pos());
QMenu menu;
QAction steve_here(trUtf8("Steve hierher teleportieren"), &menu);
steve_here.setDisabled(here.has_cube || steve_is_here);
menu.addAction(&steve_here);
QAction cube_here(trUtf8("Würfel"), &menu);
cube_here.setCheckable(true);
cube_here.setChecked(here.has_cube);
cube_here.setDisabled(steve_is_here); //No cube into steve
menu.addAction(&cube_here);
QAction stack_here(trUtf8("Stapel"), &menu);
stack_here.setCheckable(true);
if(here.stack_size > 0)
{
stack_here.setChecked(true);
stack_here.setText(trUtf8("Stapel (%1)").arg(here.stack_size));
}
menu.addAction(&stack_here);
QAction mark_here(trUtf8("Markierung"), &menu);
mark_here.setCheckable(true);
mark_here.setChecked(here.has_mark);
menu.addAction(&mark_here);
QAction *selected = menu.exec(pos);
if(selected == &steve_here)
{
steve = current_selection.coords;
//The user can't be really fast, so animations always on
setSpeed(2000);
setAnimation(ANIM_STEP);
updateAnimationTarget();
updateFront();
fbo_dirty = true;
}
else if(selected == &cube_here)
{
here.has_cube = !here.has_cube;
here.stack_size = 0;
here.has_mark = false;
fbo_dirty = true;
emit changed();
}
else if(selected == &stack_here)
{
bool ok;
int s = QInputDialog::getInt(this, trUtf8("Stapelhöhe"), trUtf8("Stapelhöhe auswählen:"), here.stack_size, 0, World::max_height, 1, &ok);
if(ok)
{
if(s > 0)
here.has_cube = false;
here.stack_size = s;
if(steve == current_selection.coords)
{
setAnimation(ANIM_STEP);
updateAnimationTarget();
}
fbo_dirty = true;
emit changed();
}
}
else if(selected == &mark_here)
{
here.has_mark = !here.has_mark;
here.has_cube = false;
fbo_dirty = true;
emit changed();
}
updateSelection();
}
void DreamWebEngine::processEvents() {
if (_eventMan->shouldQuit()) {
quit();
return;
}
soundHandler();
Common::Event event;
int softKey, hardKey;
while (_eventMan->pollEvent(event)) {
switch(event.type) {
case Common::EVENT_RTL:
quit();
break;
case Common::EVENT_KEYDOWN:
if (event.kbd.flags & Common::KBD_CTRL) {
switch (event.kbd.keycode) {
case Common::KEYCODE_d:
_console->attach();
_console->onFrame();
break;
case Common::KEYCODE_f:
setSpeed(_speed != 20? 20: 1);
break;
case Common::KEYCODE_g:
_turbo = !_turbo;
break;
case Common::KEYCODE_c: //skip statue puzzle
_symbolBotNum = 3;
_symbolTopNum = 5;
break;
default:
break;
}
return; //do not pass ctrl + key to the engine
}
// Some parts of the ASM code uses the hardware key
// code directly. We don't have that code, so we fake
// it for the keys where it's needed and assume it's
// 0 (which is actually an invalid value, as far as I
// know) otherwise.
hardKey = 0;
switch (event.kbd.keycode) {
case Common::KEYCODE_ESCAPE:
hardKey = 1;
break;
case Common::KEYCODE_SPACE:
hardKey = 57;
break;
default:
hardKey = 0;
break;
}
_lastHardKey = hardKey;
// The rest of the keys are converted to ASCII. This
// is fairly restrictive, and eventually we may want
// to let through more keys. I think this is mostly to
// keep weird glyphs out of savegame names.
softKey = 0;
if (event.kbd.keycode >= Common::KEYCODE_a && event.kbd.keycode <= Common::KEYCODE_z) {
softKey = event.kbd.ascii & ~0x20;
} else if (event.kbd.keycode == Common::KEYCODE_MINUS ||
event.kbd.keycode == Common::KEYCODE_SPACE ||
(event.kbd.keycode >= Common::KEYCODE_0 && event.kbd.keycode <= Common::KEYCODE_9)) {
softKey = event.kbd.ascii;
} else if (event.kbd.keycode >= Common::KEYCODE_KP0 && event.kbd.keycode <= Common::KEYCODE_KP9) {
softKey = event.kbd.keycode - Common::KEYCODE_KP0 + '0';
} else if (event.kbd.keycode == Common::KEYCODE_KP_MINUS) {
softKey = '-';
} else if (event.kbd.keycode == Common::KEYCODE_BACKSPACE ||
event.kbd.keycode == Common::KEYCODE_DELETE) {
softKey = 8;
} else if (event.kbd.keycode == Common::KEYCODE_RETURN
|| event.kbd.keycode == Common::KEYCODE_KP_ENTER) {
softKey = 13;
}
if (softKey)
keyPressed(softKey);
break;
default:
break;
}
}
}
void UrRealtimeCommunication::run() {
uint8_t buf[2048];
int bytes_read;
memset(buf, 0, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sockfd_, &readfds);
print_debug("Realtime port: Got connection");
connected_ = true;
while (keepalive_) {
while (connected_ && keepalive_) {
timeout.tv_sec = 0; //do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = recv(sockfd_, (char*) buf, 2048, 0);
if (bytes_read > 0) {
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
robot_state_->unpack(buf);
if (safety_count_ == safety_count_max_) {
setSpeed(0., 0., 0., 0., 0., 0.);
}
safety_count_ += 1;
} else {
connected_ = false;
CloseSocket(sockfd_);
}
}
if (keepalive_) {
//reconnect
ofLog()<<"Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."<<endl;
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
print_fatal("ERROR opening socket");
}
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_,
sizeof(int));
SetNonBlocking(sockfd_, true);
while (keepalive_ && !connected_) {
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sockfd_, (struct sockaddr *) &serv_addr_,
sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, (char*)&flag_, &flag_len);
if (flag_ < 0) {
print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
} else {
connected_ = true;
print_info("Realtime port: Reconnected");
}
}
}
}
setSpeed(0., 0., 0., 0., 0., 0.);
CloseSocket(sockfd_);
}
void MpvHandler::Speed(double d)
{
if(playState > 0)
mpv_set_property_async(mpv, MPV_REPLY_PROPERTY, "speed", MPV_FORMAT_DOUBLE, &d);
setSpeed(d);
}
Robot::~Robot()
{
setSpeed(0, 0);
}
Scene* Chapter10_5::createScene()
{
//cocos2d::Rect visibleRect = Director::getInstance()->getOpenGLView()->getVisibleRect();
Size visibleSize = Director::getInstance()->getVisibleSize();
Vec2 origin = Director::getInstance()->getVisibleOrigin();
auto winSize = Director::getInstance()->getWinSize();
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// create a scene
// 'scene' is an autorelease object
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
auto scene = Scene::create();
// add title
auto label = LabelTTF::create("camera test", "Arial", 24);
label->setPosition(Vec2(origin.x+visibleSize.width/2, origin.y+visibleSize.height/2).x,
Vec2(origin.x+visibleSize.width/2, origin.y+visibleSize.height).y - 30);
scene->addChild(label, -1);
//add the menu item for back to main menu
label = LabelTTF::create("MainMenu", "Arial", 24);
auto menuItem = MenuItemLabel::create(label);
menuItem->setCallback([&](cocos2d::Ref *sender) {
Director::getInstance()->replaceScene(Chapter10::createScene());
});
auto menu = Menu::create(menuItem, nullptr);
menu->setPosition( Vec2::ZERO );
menuItem->setPosition( Vec2( Vec2(origin.x+visibleSize.width, origin.y+visibleSize.height/2).x - 80, Vec2(origin.x+visibleSize.width/2, origin.y).y + 25) );
scene->addChild(menu, 1);
auto layer3D=Layer::create();
scene->addChild(layer3D,2);
std::string fileName = "orc.c3b";
auto sprite = Sprite3D::create(fileName);
sprite->setScale(5.f);
sprite->setRotation3D(Vec3(0,180,0));
sprite->setPosition( Vec2(origin.x+visibleSize.width/2, origin.y+visibleSize.height/2).x,
Vec2(origin.x+visibleSize.width/2, origin.y+visibleSize.height/2).y );
// play animation
auto animation = Animation3D::create(fileName);
if (animation)
{
auto animate = Animate3D::create(animation);
animate->setSpeed(1);
sprite->runAction(RepeatForever::create(animate));
}
//add to scene
layer3D->addChild(sprite);
// add camera
auto camera=Camera::createPerspective(60, (GLfloat)winSize.width/winSize.height, 1, 1000);
camera->setCameraFlag(CameraFlag::USER1);// set camera flag
camera->setPosition3D(Vec3(0, 0, 230) + sprite->getPosition3D());
camera->lookAt(sprite->getPosition3D(), Vec3(0,1,0));
// create camera action
auto action = MoveBy::create(3, Vec2(100, 0));
auto action_back = action->reverse();
auto action1 = MoveBy::create(3, Vec2(0, 100));
auto action_back1 = action1->reverse();
auto seq = Sequence::create( action, action_back, action1, action_back1, nullptr );
// run camera action
camera->runAction( RepeatForever::create(seq) );
layer3D->addChild(camera);
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// set camera mask
// when node's camera-mask & camer-flag result is true, the node is visible for this camera.
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
layer3D->setCameraMask(0x2);
// return the scene
return scene;
}
Robot::Robot()
: m_wheelLeft(0), m_wheelRight(0)
{
high(26);
setSpeed(m_wheelLeft, m_wheelRight);
}
Tricycle::Tricycle(int initialSpeed)
{
setSpeed(initialSpeed);
}
Enemy::Enemy() {
// TODO 自動生成されたコンストラクター・スタブ
setSpeed(0);
mCount = 0;
}
void Tricycle::pedal()
{
setSpeed(speed+1);
std::cout << "Pedaling; tricycle speed: " << getSpeed() << " kph\n";
}
/**
* Changes the craft's battlescape status.
* @param inbattle True if it's in battle, False otherwise.
*/
void Craft::setInBattlescape(bool inbattle)
{
if (inbattle)
setSpeed(0);
_inBattlescape = inbattle;
}
void Tricycle::brake()
{
setSpeed(speed - 1);
std::cout << "Braking; tricycle speed " << getSpeed() << "kph\n";
}
void Entity::loadData(TiXmlElement *element)
{
PropertyMap::loadData(element);
/* Get id. */
if(element->Attribute("id"))
setId(element->Attribute("id"));
/* Load basic data. */
Vector2D position = getPosition();
Vector2D::fromXML(element->FirstChildElement("position"),
&position);
setPosition(position);
Vector2D velocity = getVelocity();
Vector2D::fromXML(element->FirstChildElement("velocity"),
&velocity);
setVelocity(velocity);
Vector2D::fromXML(element->FirstChildElement("radius"), &radius);
queryBoolAttribute(element, "collisionchecker", &collisionChecker);
float speed = getSpeed();;
element->QueryFloatAttribute("speed", &speed);
setSpeed(speed);
/* Load controller. */
TiXmlElement *controllerElement =
element->FirstChildElement("controller");
if(controllerElement) {
string type = controllerElement->Attribute("type");
map<string, AffectorFactory<Controller>*>::iterator
controllerFactory = controllerFactories.find(type);
if(controllerFactory != controllerFactories.end())
controller = controllerFactory->second->create(this,
controllerElement);
else
LogManager::getInstance()->error("No controller: %s",
type.c_str());
}
/* Load positioner. */
TiXmlElement *positionerElement =
element->FirstChildElement("positioner");
if(positionerElement) {
string type = positionerElement->Attribute("type");
map<string, AffectorFactory<Positioner>*>::iterator
positionerFactory = positionerFactories.find(type);
if(positionerFactory != positionerFactories.end())
positioner = positionerFactory->second->create(this,
positionerElement);
else
LogManager::getInstance()->error("No positioner: %s",
type.c_str());
}
/* Load sprite. */
TiXmlElement *spriteElement = element->FirstChildElement("sprite");
sprite = spriteElement ? new Sprite(spriteElement) : sprite;
/* Load script. */
script = element->Attribute("script") ?
element->Attribute("script") : script;
}
/**
* @brief move the robot to the target position
*
* @param tarX is the x value of target position
* @param tarY is the y value of target position
* @param speed the robot should move at this speed
* @param tarP
* @param varDir defines whether the robot should move forwards or backwards
* @param dist The distance to the target - automatically computed if negative (default)
* @return bool
*/
bool NewRoboControl::cruisetoBias(double tarX, double tarY, int speed, double tarP, double varDir, double dist)
{
#ifdef SIMULATION //The cruisetoBias() does not work in simulation
if (IsOnTarget(Position(tarX, tarY)))
return true;
GotoXY(tarX, tarY);
return false;
#else
/* returniert true, wenn Roboter am Ziel angekommen ist. returniert false, wenn Roboter noch unterwegs ist
** Ablauf:
** 1. Anfahren zur Zielposition
** 1.1 Ziel anvisieren/andrehen
** 1.2 Beschleunigen
** 1.3 Während der Fahrt ggf leichte Richtungskorrekturen on-the-fly
** 1.4 Kurz vor Zielposition Abbremsvorgang einleiten
** 2. Zielwinkel andrehen
**
*/
//Position roboterpos = m_robot->GetPos(); //Hier müsst ihr die Roboterposition abrufen
double posX = GetX();
double posY = GetY();
double posP = GetPhi().Rad(); //Hier Roboterwinkel abrufen
const double variationTrans = 0.05; // maximale Abweichung zum Ziel (in Meter)
const double variationAngle = degToRad(5); // maximale Drehabweichung am Ende (in Rad) degToRad wandelt Grad in Rad um
const double variationDirec = degToRad(varDir); // maximale Drehabweichung am Start, bevor Roboter anfängt zu fahren
//Abweichungen berechnen
double diffX = tarX - posX;
double diffY = tarY - posY;
double diffP = atan2(diffY, diffX);
double diffAngle;
double speedAngle;
double speedAngleDrive;
//Berechnen ob Roboter vorwärts oder rückwärts fahren soll
//eDirection ist ein enum hier
eDirection eDir = getDirection(diffP, posP);
if (!((fabs(diffX) < variationTrans) && (fabs(diffY) < variationTrans)))
// Schritt 1: Roboter noch nicht an Zielposition?
{
diffAngle = getDiffAngle(diffP, posP);
speedAngle = getSpeedP(diffP, posP);
speedAngleDrive = getSpeedPt(diffP, posP, speed);
Log("Diff2: " + ToString(speedAngleDrive), DEBUG);
if ((fabs(diffAngle) > variationDirec) && (fabs(diffAngle) < degToRad(90))) // pi/2 = 1,57079633 Schritt 1.1
{
//Roboter rotieren
setSpeed(0, speedAngle, eDir);
}
else if ((fabs(diffAngle)) <= variationDirec) // Schritt 1.1 erfolgreich, es folgt Schritt 1.2
{
if (dist <= 0)
dist = sqrt((diffX * diffX) + (diffY * diffY));
setSpeed(speed * getSpeedT(dist), speedAngleDrive, eDir);
// Schritt 1.2 - 1.4 gleichzeitig!
}
}
else // Schritt 1: fertig. Roboter ist an Zielposition.
{
if (tarP == -10)
{
setSpeed(0, 0, FORWARD);
//StopMovement();
return true;
}
else if ((fabs(getDiffAngle(tarP*M_PI/180, posP))) > variationAngle)
{
setSpeed(0, getSpeedP(tarP, posP), getDirection(tarP, posP));
}
else
{
setSpeed(0, 0, FORWARD);
//StopMovement();
return true;
}
}
return false;
#endif
}
void MeStepper::computeNewSpeed()
{
setSpeed(desiredSpeed());
}
void MyRobot::run()
{
double ir0_val = 0.0, ir2_val = 0.0, ir14_val = 0.0,ir1_val = 0.0, compass_angle;
while (step(_time_step) != -1) {
// Read sensors and compass
ir0_val = _distance_sensor[0]->getValue();
ir2_val = _distance_sensor[1]->getValue();
ir14_val = _distance_sensor[2]->getValue();
ir1_val = _distance_sensor[3]->getValue();
const double *compass_val = _my_compass->getValues();
// Convert compass bearing vector to angle, in degrees
compass_angle = convert_bearing_to_degrees(compass_val);
//Show distance sensor values
cout << "ds14: " << ir14_val << "ds1: " << ir1_val << " ds0: " << ir0_val << " ds2: " << ir2_val << endl;
// Robot control logic
if (_mode == GOING_TO_INITIAL_POINT) {
// Shearch for the desired angle
// When sufficiently close to a wall in front of robot,
// switch mode to wall following
if ((ir0_val > DISTANCE_LIMIT -50) || (ir14_val > DISTANCE_LIMIT -20) || (ir1_val > DISTANCE_LIMIT -50)) {
_mode = WALL_FOLLOWER;
cout << "The desired initial position has been reached." << endl;
cout << "Mode " << WALL_FOLLOWER << ": Wall following mode activated" << endl;
}
}
else {
// Wall following
if ((ir0_val > DISTANCE_LIMIT - 50) || (ir14_val > DISTANCE_LIMIT -50) || (ir1_val > DISTANCE_LIMIT -50)) {
_mode = WALL_FOLLOWER;
cout << "Avoiding collision with a wall" << endl;
}
else {
if (ir2_val > DISTANCE_LIMIT + 50) {
_mode = TURN_RIGHT;
cout << "Turning left." << endl;
}
else {
if (ir2_val < DISTANCE_LIMIT - 50) {
_mode = TURN_LEFT;
cout << "Turning right." << endl;
}
else {
_mode = FORWARD;
cout << "Moving forward." << endl;
}
}
}
}
// Send actuators commands according to the mode
switch (_mode){
case FORWARD:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
break;
case GOING_TO_INITIAL_POINT:
if (compass_angle < (DESIRED_ANGLE - 2)) {
// Turn right
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED - 15;
}
else {
if (compass_angle > (DESIRED_ANGLE + 2)) {
// Turn left
_left_speed = MAX_SPEED - 15;
_right_speed = MAX_SPEED;
}
else {
// Move straight forward
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
}
}
break;
case TURN_LEFT:
_left_speed = MAX_SPEED / 1.25;
_right_speed = MAX_SPEED;
break;
case TURN_RIGHT:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED / 1.25;
break;
case WALL_FOLLOWER:
_left_speed = -MAX_SPEED / 20.0;
_right_speed = -MAX_SPEED / 4.0;
break;
default:
break;
}
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
/**
* Loads the UFO from a YAML file.
* @param node YAML node.
* @param mod The game mod. Use to access the trajectory rules.
* @param game The game data. Used to find the UFO's mission.
*/
void Ufo::load(const YAML::Node &node, const Mod *mod, SavedGame *game, Ufo *escorting)
{
MovingTarget::load(node);
_id = node["id"].as<int>(_id);
_escorting = escorting;
_escortId = node["escortId"].as<int>(_escortId);
_escortingId = node["escortingId"].as<int>(_escortingId);
_crashId = node["crashId"].as<int>(_crashId);
_landId = node["landId"].as<int>(_landId);
_damage = node["damage"].as<int>(_damage);
_shield = node["shield"].as<int>(_shield);
_shieldRechargeHandle = node["shieldRechargeHandle"].as<int>(_shieldRechargeHandle);
_altitude = node["altitude"].as<std::string>(_altitude);
_direction = node["direction"].as<std::string>(_direction);
_detected = node["detected"].as<bool>(_detected);
_hyperDetected = node["hyperDetected"].as<bool>(_hyperDetected);
_secondsRemaining = node["secondsRemaining"].as<size_t>(_secondsRemaining);
_inBattlescape = node["inBattlescape"].as<bool>(_inBattlescape);
double lon = _lon;
double lat = _lat;
if (const YAML::Node &dest = node["dest"])
{
lon = dest["lon"].as<double>();
lat = dest["lat"].as<double>();
}
_dest = new Waypoint();
_dest->setLongitude(lon);
_dest->setLatitude(lat);
if (const YAML::Node &status = node["status"])
{
_status = (UfoStatus)status.as<int>();
}
else
{
if (_damage >= _stats.damageMax)
{
_status = DESTROYED;
}
else if (_damage >= _stats.damageMax / 2)
{
_status = CRASHED;
}
else if (_altitude == "STR_GROUND")
{
_status = LANDED;
}
else
{
_status = FLYING;
}
}
if (game->getMonthsPassed() != -1 && node["mission"])
{
int missionID = node["mission"].as<int>();
std::vector<AlienMission *>::const_iterator found = std::find_if (game->getAlienMissions().begin(), game->getAlienMissions().end(), matchMissionID(missionID));
if (found == game->getAlienMissions().end())
{
// Corrupt save file.
throw Exception("Unknown UFO mission, save file is corrupt.");
}
_mission = *found;
_stats += _rules->getRaceBonus(_mission->getRace());
std::string tid = node["trajectory"].as<std::string>();
_trajectory = mod->getUfoTrajectory(tid);
if (_trajectory == 0)
{
// Corrupt save file.
throw Exception("Unknown UFO trajectory, save file is corrupt.");
}
_trajectoryPoint = node["trajectoryPoint"].as<size_t>(_trajectoryPoint);
}
_fireCountdown = node["fireCountdown"].as<int>(_fireCountdown);
_escapeCountdown = node["escapeCountdown"].as<int>(_escapeCountdown);
_retreating = node["retreating"].as<bool>(_retreating);
for (YAML::const_iterator ii = node["escorts"].begin(); ii != node["escorts"].end(); ++ii)
{
std::string type = (*ii)["type"].as<std::string>();
if (mod->getUfo(type))
{
Ufo *u = new Ufo(mod->getUfo(type), false);
u->load(*ii, mod, game, this);
_escorts.push_back(u);
}
/*else
{
Log(LOG_ERROR) << "Failed to load UFO " << type;
}*/
}
if (_inBattlescape)
setSpeed(0);
}
int Wire::initSpeed()
{
if(this->speed!=0) {
return this->speed;
}
if(this->connected1!=NULL && this->connected1->getParentElement()->getName()=="Ground") {
setSpeed(0);
return 0;
}
if(this->connected1!=NULL) {
if(connected1->getParentElement()->getName()=="Res") {
Resistor* resTemp = (Resistor*)connected1->getParentElement();
double tmpPotential = resTemp->getAnotherWire(this)->getPotential();
if(tmpPotential>this->potential) {
double i = abs(tmpPotential-this->potential)*resTemp->getValue();
//i%=10;
this->setSpeed((int)i);
return i;
} else {
double i = abs(tmpPotential-this->potential)*resTemp->getValue();
//i%=10;
this->setSpeed((int)-i);
return i;
}
} else if (connected1->getParentElement()->getName()=="Emf") {
if(this->connected2==NULL) {
EMF* emfTemp = (EMF*)connected1->getParentElement();
int direction = emfTemp->getEmfDirection(this);
QList<Wire*>::Iterator i;
for(i=this->wires->begin();i!=wires->end();i++) {
if((*i)==this) {
continue;
}
this->speed+=(*i)->initSpeed();
}
this->speed*direction;
return speed;
}
}
}
if(this->connected2!=NULL) {
if(connected2->getParentElement()->getName()=="Res") {
Resistor* resTemp = (Resistor*)connected2->getParentElement();
double tmpPotential = resTemp->getAnotherWire(this)->getPotential();
if(tmpPotential>this->potential) {
double i = abs(tmpPotential-this->potential)*resTemp->getValue();
//i%=10;
this->setSpeed((int)-i);
return i;
} else {
double i = abs(tmpPotential-this->potential)*resTemp->getValue();
//i%=10;
this->setSpeed((int)i);
return i;
}
} else if (connected2->getParentElement()->getName()=="Emf") {
if(this->connected1==NULL) {
EMF* emfTemp = (EMF*)connected2->getParentElement();
int direction = emfTemp->getEmfDirection(this);
QList<Wire*>::Iterator i;
for(i=this->wires->begin();i!=wires->end();i++) {
if((*i)==this) {
continue;
}
this->speed+=(*i)->initSpeed();
}
this->speed*direction;
return speed;
}
}
}
}
