//PID制御ライントレース
void RA_linetrace_PID(int forward_speed) {
if(runner_mode == RN_MODE_CONTROL){
shock();
}
RA_speed(forward_speed,2);
if(forward_speed > 0)
hensa = (float)GRAY_VALUE - (float)ecrobot_get_light_sensor(NXT_PORT_S3);
else
hensa = (float)ecrobot_get_light_sensor(NXT_PORT_S3) - (float)GRAY_VALUE;
i_hensa = i_hensa+(hensa*0.0005);
d_hensa = (hensa - bf_hensa)/0.0005;
bf_hensa = hensa;
cmd_turn = -(Kp * hensa + Ki * i_hensa + Kd * d_hensa);
if (-100 > cmd_turn) {
cmd_turn = -100;
} else if (100 < cmd_turn) {
cmd_turn = 100;
}
/*倒立制御OFF時*/
//nxt_motor_set_speed(NXT_PORT_C, forward_speed + cmd_turn/2, 1);
//nxt_motor_set_speed(NXT_PORT_B, forward_speed - cmd_turn/2, 1);
}
void Asteroid::update(unsigned long elapsed_millis) {
_physics->updatePosition(elapsed_millis);
_physics->updateAngle(_anglestep);
int _shotColliding = _weaponsManager->isColliding(_physics->getPosition(), _medradius);
if ( _shotColliding == 1) {
_ship1->addScore(getScore());
explode();
return;
}
if ( _shotColliding == 2) {
_ship2->addScore(getScore());
explode();
return;
}
if (_ship1->isInvincible() && _ship1->isColliding(_physics->getPosition(), _medradius)) {
cg::Vector2d collisionPoint = _physics->getCollisionPoint(_ship1->getPosition());
double Force = _physics->getMass()*length(_physics->getVelocity());
shock(collisionPoint);
_ship1->shock(collisionPoint, Force);
/*_ship1->addScore(getScore());
explode();*/
return;
}
_ship1->isColliding(_physics->getPosition(), _medradius);
if (_ship2->isInvincible() && _ship2->isColliding(_physics->getPosition(), _medradius)) {
cg::Vector2d collisionPoint = _physics->getCollisionPoint(_ship2->getPosition());
double Force = _physics->getMass()*length(_physics->getVelocity());
shock(collisionPoint);
_ship2->shock(collisionPoint, Force);
/*_ship1->addScore(getScore());
explode();*/
return;
}
_ship2->isColliding(_physics->getPosition(), _medradius);
}
void Ship2::update(unsigned long elapsed_millis) {
Ship::update(elapsed_millis);
if (_ship1->isColliding(_physics->getPosition(),_shieldrad)) {
if(_ship1->isInvincible() && isInvincible()) {
cg::Vector2d collisionPoint = cg::Vector2d( (getXposition() + _ship1->getXposition())/2 , (getYposition() + _ship1->getYposition())/2);
cg::Vector2d ship2Velocity = getVelocity();
shock(collisionPoint, 0.5 * _ship1->getMass()*length(_ship1->getVelocity()));
_ship1->shock(collisionPoint, 0.5 * _physics->getMass()*length(ship2Velocity));
return;
}
explode();
return;
}
}
