void GLLegoPIR::refresh()
{
unsigned long time = millis();
unsigned long dt = time - m_last_time;
if (m_current_speed != m_desired_speed) //different desired speed -> we have to do something
{
if (m_current_speed < m_desired_speed) //we are slower -> accelerate
{
while (dt > m_acc && m_current_speed != m_desired_speed) //we need to accelerate
{
++m_current_speed;
SendSpeed(m_current_speed);
dt -= m_acc;
m_last_time += m_acc;
}
}
else //we are faster -> decelerate
{
while (dt > m_dec && m_current_speed != m_desired_speed) //we need to accelerate
{
--m_current_speed;
SendSpeed(m_current_speed);
dt -= m_dec;
m_last_time += m_dec;
}
}
}
}
/* ================= SendSmartcarInfo ====================
desc: SCI串口发送当前激光管采集信息
pre: 1当前激光管采集数组，2当前激光管状态
Post: 无
*/
void SendSmartcarInfo(byte temp_laser_array[],byte temp_laserStatus,byte last_laserStatus,unsigned int temp_pulse) {
int i;
for(i=LASER_MAX-1;i>=0;i--) //发送激光管信息数组
SendOneData(temp_laser_array[i]);
SendLaserStatus(temp_laserStatus);//发送激光管状态
SendSteerInfo (); //发送舵机转角
SendSpeed(); //发送速度
SendTempPulse(temp_pulse); //发送周期测速的脉冲数
if(temp_laserStatus != last_laserStatus) //若状态发生改变，发送分隔符
SendSeparator();
SCISend(13);
SCISend(10);
} //SendSmartcarInfo
int GLLegoPIR::set( int addr, int drivemode, int v, int v_max, int fn[] )
{
//range check
if ( v > v_max )
v = v_max;
if ( v < 0 )
v = 0;
//map to 0..7
v = map( v, 0, v_max, 0, 7 );
if (drivemode == 2) //emergency break
{
SendBreak();
//reset speeds
m_desired_speed = 0;
m_current_speed = 0;
}
else
{
//if reverse -> -
if (drivemode == 0)
v = -v;
//set desired speed to calculated speed
m_desired_speed = v;
//if current and desired speed is the same -> send now
if (m_desired_speed == m_current_speed)
{
SendSpeed(m_desired_speed);
}
else //different desired/current speed -> check for 0 delay
{
if (m_current_speed < m_desired_speed) //we are slower -> accelerate
{
if (m_acc == 0) //no delay -> set it now
{
m_current_speed = m_desired_speed;
SendSpeed(m_current_speed);
}
else
{
#ifdef PIR_SEND_FIRST_STEP_IMMEDIATE //we have delay but want to send first immediately
++m_current_speed;
SendSpeed(m_current_speed);
#endif
}
}
else if (m_current_speed > m_desired_speed) //we are faster -> decelerate
{
if (m_dec == 0) //no delay -> set it now
{
m_current_speed = m_desired_speed;
SendSpeed(m_current_speed);
}
else
{
#ifdef PIR_SEND_FIRST_STEP_IMMEDIATE //we have delay but want to send first immediatly
--m_current_speed;
SendSpeed(m_current_speed);
#endif
}
}
}
}
m_last_time = millis(); //save time
return (200);
}
