void MovementController::tick(PhysicsComponent *phys, float delta)
{
float maxSpeed;
b2Vec2 steering(tick(delta, maxWalkSpeed));
phys->steering.Set(steering.x, steering.y);
phys->maxSpeed = maxWalkSpeed;
}
// per frame simulation update
void LowSpeedTurn::update(const float currentTime, const float elapsedTime){
// apply force to vehicle
applySteeringForce(steering(), elapsedTime);
// update Irrlicht node
setPosition(position().vector3df());
irr::scene::ISceneNode::setRotation(forward().vector3df().getHorizontalAngle());
// update motion trail
recordTrailVertex(currentTime, position());
}
int main(int argc, char *argv[]) {
log::Output2FILE::Init("apmaskin.log");
CommandInterpreterImpl commandInterpreter;
CameraModule camera(commandInterpreter);
IRSensor irSensor(commandInterpreter);
MotorControllerImpl motorController;
SteeringModule steering(commandInterpreter, motorController);
CrossCountry crossCountry(commandInterpreter, irSensor);
crossCountry.registerCommands();
commandInterpreter.registerCommands();
camera.registerCommands();
irSensor.registerCommands();
steering.registerCommands();
commandInterpreter.start();
log::Output2FILE::Close();
return 0;
}
void main(void)
{
char send_cnt=0;
SetBusCLK_40M();
SCI0_Init();
PIT_Init();
AD_Init();
CCD_IO_Init();
PWM_Init();
PAC_Init();
DDRT_DDRT0=1;
PTT_PTT0=1;
delay();
Motor_forward(26);
steering(STEER_MID);
DDRM=0XFF;
EnableInterrupts;
for(;;)
{
if(TimerFlag20ms == 1)
{
DisableInterrupts;
TimerFlag20ms = 0;
ImageCapture(Pixel);
//CalculateIntegrationTime();
//mid_val_3(Pixel);
//send_cnt++;
/*if(send_cnt>10)
{
send_cnt=0;
SendImageData(Pixel);
} */
find(Pixel,5,3,20);
//CCD_P2(Pixel,3,18);
steer_pd();
EnableInterrupts;
}
}
}
/**
calcule la force necessaire pour éviter les collision à venir avec d'autres unitées
*/
Vector Vehicule::unallignedCollisionAvoidance(std::vector<Vehicule*>& others)
{
Vector steering(0,0);
for(unsigned int i=0; i<others.size(); i++)
{
if(others[i] != this)
{
/// si les deux unité sont assez proches
if(Vector(others[i]->getPosition()-m_position).norme() < 5*m_radius)
{
Vector futurDiff(others[i]->getFuturePosition(UCATIME) - getFuturePosition(UCATIME));
if(futurDiff.norme() < m_radius+others[i]->m_radius)
{
if(futurDiff.norme()>m_radius+others[i]->m_radius/2)
futurDiff.setLengh(futurDiff.norme()-m_radius);
steering -= futurDiff;
}
}
}
}
return steering;
}
void steer_pd(void)
{
//float D=0;
//float P=0;
g_line_new=(int)Calculate_line();
//g_line_new=DirectionControl();
g_steer_err_cur=Middle-g_line_new;
if(fabs(g_line_new-g_line_old)>40)
{
g_line_new=g_line_old;
}
b_left_last_flag=b_left_flag;
b_right_last_flag=b_right_flag;
if(b_right_last_flag==1)
{
b_right_last=b_right;
}
if(b_left_last_flag==1)
{
b_left_last=b_left;
}
//if(fabs(g_steer_err_cur)<20)
{
steer_control_P=23;
}
//else
{
// steer_control_P=(g_steer_err_cur*g_steer_err_cur*0.015+10);//0.2 0.5 0.9
}
// 0.02 有点小 //d 150
/*****************************单系数*************/
// 0.06 d 150 打大角度晚了 有点摇晃大角度好
//0.06 d 120 感觉进弯道 打大角度晚了 不摇晃了
// 0.08 打角度小
//0.16 感觉提前打角度了
//0.115 看到赛道外
// 0.095 感觉小
//0.005 轻轻走着 近似可以出弯道
//0.003 轻轻走着 弯道出去
//0.00
/*系数ab********************************/
//0.002 ,5 小，过不去
//0.003 ,10效果不错
//0.004+5 拐弯的时候靠外 出弯道车身不正，偏外。
//0.003+5 拐弯的时候靠外+1 出弯道车身不正，偏外。
//0.0045+5 拐弯的时候靠外,出弯道车身不正，偏外。
steer_out_this=STEER_MID-steer_control_P*g_steer_err_cur-steer_control_D*(g_steer_err_cur-g_steer_err_pre);//+steer_control_D*D;
if(steer_out_this>=steer_control_out_max)
{
steer_out_this=steer_control_out_max;
}
else if(steer_out_this<=steer_control_out_min)
{
steer_out_this=steer_control_out_min;
}
steering((unsigned int)steer_out_this);
g_steer_err_pre=g_steer_err_cur;
steer_out_last=steer_out_this;
g_line_old=g_line_new;
}
