/** * Function : Priority_DeletMin * Description : delete a priority queue node * Input : * Output : nothing * return : OK or ERROR */ status Priority_DeletMin(Queue_Pat *Prio_Que) { if(!(Prio_Que->base)) return ERROR; if(PriorityQue_Empty(*Prio_Que)) return ERROR; //写入就诊状态 (Prio_Que->base)[Prio_Que->front].succorfail = 1;//就诊成功 //写入日志 LogSave((Prio_Que->base)[Prio_Que->front]); //清除堆栈 free((Prio_Que->base)[Prio_Que->front].arrive_time); Prio_Que->front ++ ; Prio_Que->length -- ; return OK; }
/** * Function : Clear_PriorityQue * Description : clear a priority queue * Input : * Output : nothing * return : OK or ERROR */ status Clear_PriorityQue(Queue_Pat *Prio_Que) { if(!(Prio_Que -> base)) return ERROR; while(!PriorityQue_Empty(*Prio_Que)) { (Prio_Que->base)[Prio_Que->front].succorfail = 0;//未就诊 //写入日志 LogSave((Prio_Que->base)[Prio_Que->front]); //释放堆栈 free((Prio_Que->base)[Prio_Que->front].arrive_time); Prio_Que -> front ++ ; Prio_Que -> length -- ; } Prio_Que -> front = Prio_Que -> tail = 1 ; return OK; }
/** * Periodic code for disabled mode should go here. * * Use this method for code which will be called periodically at a regular * rate while the robot is in disabled mode. */ void DisabledPeriodic() { // Keep watching wheel speeds during spin-down RunWheels(); // respond to log dump request even when disabled if (!eio->GetDigital(13)) { if (!dump) { LogSave("/ni-rt/system/k9.csv"); } dump = true; } else { dump = false; } }
/** * Function : Pat_LeaveJudge * Description : determine whether the patient left * Input : * Output : nothing * return : nothing */ void Pat_LeaveJudge(Queue_Pat *Prio_Que) { //检查队列中是否有病人到时离开 int i; time_t nowtime; struct tm *timeinfo; time ( &nowtime ); timeinfo = localtime ( &nowtime ); for(i = Prio_Que->length ; i > 0 ; -- i) { //删除离开病人 if(((Prio_Que->base)[i].arrive_time)->tm_hour * hour + ((Prio_Que->base)[i].arrive_time)->tm_min + (Prio_Que->base)[i].wait_time <= timeinfo->tm_hour * hour + timeinfo->tm_min) {//达到删除条件 //写入就诊状态 (Prio_Que->base)[i].succorfail = 0;//未就诊 //写入日志 LogSave((Prio_Que->base)[i]); (Prio_Que->base)[i] = (Prio_Que->base)[Prio_Que->tail - 1]; Prio_Que->tail -- ; Prio_Que->length -- ; } }//for }
/** * Periodic code for teleop mode should go here. * * Use this method for code which will be called periodically at a regular * rate while the robot is in teleop mode. */ void TeleopPeriodic() { if (!eio->GetDigital(1)) { StartWheels(); } else if (!eio->GetDigital(2)) { StopWheels(); } RunWheels(); #ifdef HAVE_LEGS if (!eio->GetDigital(3)) { legs->Set(true); } else if (!eio->GetDigital(4)) { legs->Set(false); } #endif #ifdef HAVE_INJECTOR if (!eio->GetDigital(5)) { injectorL->Set(DoubleSolenoid::kForward); injectorR->Set(DoubleSolenoid::kForward); } else if (!eio->GetDigital(6)) { injectorL->Set(DoubleSolenoid::kReverse); injectorR->Set(DoubleSolenoid::kReverse); } else { injectorL->Set(DoubleSolenoid::kOff); injectorR->Set(DoubleSolenoid::kOff); } #endif #ifdef HAVE_EJECTOR if (!eio->GetDigital(7)) { ejector->Set(true); } else if (!eio->GetDigital(8)) { ejector->Set(false); } #endif if (!eio->GetDigital(13)) { if (!dump) { LogSave("/ni-rt/system/k9.csv"); } dump = true; } else { dump = false; } }