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); }