private bool initEnv()
{
bool envOK = false;
lock (LockController.getLockController().getLockTask()) {
ForkLiftWrappersService.getInstance().connectForks();
}
Thread.Sleep(100);
while (true)
{
foreach (ForkLiftWrapper fl in AGVCacheData.getForkLiftWrapperList())
{
if (fl.getForkLift().isUsed == 1)
{
if (fl.getPosition().getPx() == 0 || fl.getPosition().getPy() == 0)
{
Console.WriteLine("Wait for Fork " + fl.getForkLift().id + " to update position");
//continue;
}
}
}
break;
}
return(envOK);
}
public static List <SingleTask> getUpPickSingleTaskList() //获取供选择任务列表
{
lock (LockController.getLockController().getLockData()) {
if (singleTaskList == null || upPickSingleTaskList == null || downPickSingleTaskList == null)
{
getSingleTaskList();
}
}
return(upPickSingleTaskList);
}
private void _updateFrm()
{
while (!isStop)
{
_updateAgvPanel();
_updateSystemPause(); //如果设置系统暂停后，界面需要更新，用户不能再操作
if (!isUpdateFrm)
{
Thread.Sleep(1000);
continue;
}
lock (LockController.getLockController().getLockTask()) {
foreach (DictionaryEntry de in buttonStHash)
{
int taskID = (int)de.Key;
TaskButton tb = (TaskButton)de.Value;
//Console.WriteLine(" look task id = " + taskID);
TaskRecord tr = lookTaskRecordByTaskId(taskID);
tb.bindValue(tr);
if (tr != null)
{
if (tr.taskRecordStat == TASKSTAT_T.TASK_READY_SEND)
{
tb.BackColor = Color.LightGray;
}
else if (tr.taskRecordStat == TASKSTAT_T.TASK_SEND || tr.taskRecordStat == TASKSTAT_T.TASK_SEND_SUCCESS)
{
tb.BackColor = Color.LightGreen;
}
}
else
{
tb.BackColor = Color.White; //没有任务记录表示，任务执行完成或没有发送任务，颜色改为白色
}
}
}
isUpdateFrm = false;
}
}
public static List <SingleTask> getSingleTaskList() //获取供选择任务列表
{
lock (LockController.getLockController().getLockData()) {
if (singleTaskList == null)
{
upPickSingleTaskList = new List <SingleTask>();
downPickSingleTaskList = new List <SingleTask>();
singleTaskList = DBDao.getDao().SelectSingleTaskList();
foreach (SingleTask st in singleTaskList)
{
if (st.taskType == TASKTYPE_T.TASK_TYPE_UP_PICK)
{
upPickSingleTaskList.Add(st); //总共只有两个楼上取货任务
}
else if (st.taskType == TASKTYPE_T.TASK_TYPE_DOWN_PICK)
{
downPickSingleTaskList.Add(st);
}
}
}
}
return(singleTaskList);
}
public void scheduleInstruction()
{
while (ScheduleFactory.getSchedule().getScheduleFlag())
{
Thread.Sleep(500);
sheduleLift();
if (AGVSystem.getSystem().getSystemPause())
{
if (AGVSystem.getSystem().getLastPause() != SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_SYSTEM_WITH_START && AGVSystem.getSystem().getLastPause() != SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_SYSTEM_WITHOUT_START) //避免多次设置
{
foreach (ForkLiftWrapper fl in AGVCacheData.getForkLiftWrapperList())
{
if (fl.getForkLift().shedulePause == 0)
{
AGVUtil.setForkCtrl(fl, 1); //向不是暂停的车发送暂停指令
}
}
}
AGVSystem.getSystem().setLastPause(AGVSystem.getSystem().getCurrentPause());
continue; //系统暂停后不需要调度
}
else if (AGVSystem.getSystem().getCurrentPause() == SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_UP_WITH_START || AGVSystem.getSystem().getCurrentPause() == SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_UP_WITHOUT_START) //暂停楼上的车，有时候卸货不及时
{
if (AGVSystem.getSystem().getLastPause() != SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_UP_WITH_START && AGVSystem.getSystem().getLastPause() != SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_UP_WITHOUT_START) //避免多次设置
{
foreach (ForkLiftWrapper fl in AGVCacheData.getForkLiftWrapperList())
{
if (fl.getForkLift().forklift_number != 3 && fl.getForkLift().shedulePause == 0) //只调度楼上的车
{
AGVUtil.setForkCtrl(fl, 1);
}
}
}
AGVSystem.getSystem().setLastPause(AGVSystem.getSystem().getCurrentPause());
continue; //楼上的车子被暂停后，不需要调度
}
else if (AGVSystem.getSystem().getCurrentPause() == SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_DOWN_WITH_START || AGVSystem.getSystem().getCurrentPause() == SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_DOWN_WITHOUT_START)
{
if (AGVSystem.getSystem().getLastPause() != SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_DOWN_WITH_START && AGVSystem.getSystem().getLastPause() != SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_DOWN_WITHOUT_START) //避免多次设置
{
ForkLiftWrapper fl = ForkLiftWrappersService.getInstance().getForkLiftByNunber(3);
AGVUtil.setForkCtrl(fl, 1);
}
AGVSystem.getSystem().setLastPause(AGVSystem.getSystem().getCurrentPause());
}
else
{
if (AGVSystem.getSystem().getLastPause() == SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_SYSTEM_WITH_START)
{
foreach (ForkLiftWrapper fl in AGVCacheData.getForkLiftWrapperList())
{
if (fl.getForkLift().shedulePause == 0)
{
AGVUtil.setForkCtrl(fl, 0); //之前不是暂停的车，发送启动指令
}
}
}
if (AGVSystem.getSystem().getLastPause() == SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_UP_WITH_START)
{
foreach (ForkLiftWrapper fl in AGVCacheData.getForkLiftWrapperList())
{
if (fl.getForkLift().forklift_number != 3 && fl.getForkLift().shedulePause == 0)
{
AGVUtil.setForkCtrl(fl, 0); //之前不是暂停的车，发送启动指令
}
}
}
if (AGVSystem.getSystem().getLastPause() == SHEDULE_PAUSE_TYPE_T.SHEDULE_PAUSE_DOWN_WITH_START)
{
ForkLiftWrapper fl = ForkLiftWrappersService.getInstance().getForkLiftByNunber(3);
AGVUtil.setForkCtrl(fl, 0);
}
AGVSystem.getSystem().setLastPause(AGVSystem.getSystem().getCurrentPause());
}
lock (LockController.getLockController().getLockForkLift()) //加锁，避免车的状态不一致
{
_sheduleRunning();
}
}
}
/*处理车子反馈报文 msg格式cmd=position;battery=%d;error=%d;x=%d;y=%d;a=%f;z=%d;
* speed=%d;task=%s;veer_angle=%f;
* task_step=%d;task_isfinished=%d;task_error=%d;walk_path_id=%d */
/// <summary>
///
/// </summary>
/// <param name="id">表示车子</param>
/// <param name="msg"></param>
/// <returns></returns>
private void handleForkLiftMsg(int id, byte[] buffer, int length)
{
int pos = -1;
int pos_e = -1;
int pos_t = -1;
string taskName = "";
int x = 0; //车子横坐标
int y = 0; //车子纵坐标
int pause_stat = -1; //默认是错误状态
int battery_soc = -1;
int finish_stat = -1;
int gAlarm = 1; //AGV防撞信号 默认1 表示没有报警
string msg = parseForkLiftMsg(buffer, length);
if (string.IsNullOrEmpty(msg))
{
AGVLog.WriteError("msg is null", new StackFrame(true));
return;
}
if (!(msg.StartsWith("cmd=position;") && msg.EndsWith("?")))
{
AGVLog.WriteError("msg is error patten", new StackFrame(true));
return;
}
lastMsg = msg;
CommandService.getInstance().setLatestMsgFromClient(msg);
DBDao.getDao().InsertConnectMsg(msg, "receive");
//Console.WriteLine("msg = " + msg);
//解析taskName
try {
//if (id == 2)
// AGVLog.WriteError(msg, new StackFrame(true));
pos_t = msg.IndexOf("task=");
if (pos_t != -1)
{
pos_e = msg.Substring(pos_t, msg.Length - pos_t).IndexOf(";");
if (pos_e != -1)
{
taskName = msg.Substring(pos_t + 5, pos_e - 5);
//AGVLog.WriteInfo("forklift taskName = " + taskName, new StackFrame(true));
//Console.WriteLine("forklift taskName = " + taskName);
}
}
if (string.IsNullOrEmpty(taskName))
{
//AGVLog.WriteError("forklift taskName is null", new StackFrame(true));
//Console.WriteLine("msg format err: taskName is null");
//return ; //主要判断车的finished状态
}
//解析坐标位置 x,y
pos_t = msg.IndexOf(";x=");
if (pos_t != -1)
{
pos_e = msg.Substring(pos_t + 1, msg.Length - pos_t - 1).IndexOf(";");
if (pos_e != -1)
{
// Console.WriteLine("x = " + msg.Substring(pos_t + 3, pos_e - 2) + " id = " + id);
x = int.Parse(msg.Substring(pos_t + 3, pos_e - 2));
}
}
pos_t = msg.IndexOf(";y=");
if (pos_t != -1)
{
pos_e = msg.Substring(pos_t + 1, msg.Length - pos_t - 1).IndexOf(";");
if (pos_e != -1)
{
// Console.WriteLine("y = " + msg.Substring(pos_t + 3, pos_e - 2));
y = int.Parse(msg.Substring(pos_t + 3, pos_e - 2));
}
}
pos_t = msg.IndexOf("pause_stat=");
if (pos_t != -1)
{
pos_e = msg.Substring(pos_t, msg.Length - pos_t).IndexOf(";");
pause_stat = int.Parse(msg.Substring(pos_t + 11, pos_e - 11));
}
pos_t = msg.IndexOf("gAlarm=");
if (pos_t != -1)
{
pos_e = msg.Substring(pos_t, msg.Length - pos_t).IndexOf(";");
gAlarm = int.Parse(msg.Substring(pos_t + 7, pos_e - 7));
}
pos_t = msg.IndexOf("battery=");
if (pos_t != -1) //获取电池数据
{
pos_e = msg.Substring(pos_t, msg.Length - pos_t).IndexOf(";");
battery_soc = int.Parse(msg.Substring(pos_t + 8, pos_e - 8));
//Console.WriteLine("battery = " + battery_soc);
}
pos = msg.IndexOf("task_isfinished=");
finish_stat = Convert.ToInt16(msg[pos + 16]) - 48; //转的对象是单个字符 0会转成48
} catch (Exception ex) {
Console.WriteLine(ex.ToString());
Console.WriteLine("接收 数据异常");
return;
}
if (!checkForkLiftMsg(battery_soc, pause_stat, finish_stat, gAlarm))
{
//Console.WriteLine("接收数据异常");
return;
}
lock (LockController.getLockController().getLockTask()) {
if (pos != -1) //成功匹配到状态
{
foreach (ForkLiftWrapper fl in forkLiftWrapperList)
{
if (fl.getForkLift().id == id)
{
if (x != 0 && y != 0)
{
fl.setPosition(x, y);
}
if (id == 1 && pauseSetTime_f1 == 0)
{
fl.getForkLift().shedulePause = pause_stat; //只在启动的时候设置一次
pauseSetTime_f1 = 1;
fl.getPosition().calcPositionArea();
}
else if (id == 2 && pauseSetTime_f2 == 0)
{
fl.getForkLift().shedulePause = pause_stat; //只在启动的时候设置一次
pauseSetTime_f2 = 1;
fl.getPosition().calcPositionArea();
}
fl.getForkLift().pauseStat = pause_stat;
if (pause_stat >= 0)
{
checkForkliftPauseStat(fl, pause_stat);
}
fl.getForkLift().finishStatus = finish_stat;
fl.getBatteryInfo().setBatterySoc(battery_soc);
fl.updateAlarm(gAlarm);
AGVLog.WriteInfo("forklift id " + id + "taskName = " + taskName + "forklift stat = " + fl.getForkLift().finishStatus, new StackFrame(true));
{
bool stat = checkTaskSendStat(fl, taskName);
if (stat == false)
{
AGVLog.WriteError("任务列表中不能匹配正确状态的任务", new StackFrame(true));
}
}
}
}
}
}
return;
}
public void sheduleTask() {
ForkLiftWrapper tmpForkLiftWrapper = null;
SingleTask tmpSingleTask = null;
int upRecordStep = 0; //没有上货， 该值大于0的时候，表示上货还没有结束，可能存在升降机正在运送，车子当前没有任务
int downRecordStep = 0; //没有下货，该值大于0的时候， 表示下货没有结束
int result = -1;
///***开始处理任务的循环语句***///
while (ScheduleFactory.getSchedule().getScheduleFlag()) {
Thread.Sleep(2000);
if (AGVSystem.getSystem().getSystemPause()) { //系统暂停后，调度程序不执行
Console.WriteLine("system pause");
continue;
}
//Console.WriteLine(" shedule Task");
lock (LockController.getLockController().getLockTask()) {
taskRecordList = TaskReordService.getInstance().getTaskRecordList();
upTaskRecord = checkUpTaskRecord();
}
//if (upTaskRecord != null)
// Console.WriteLine(" upTaskRecord name = " + upTaskRecord.taskRecordName);
if (ScheduleFactory.getSchedule().getDownDeliverPeriod()) //当前处于上货阶段，有的话控制升降机上升
{
//读取升降机上料信号
if (ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_UP) //检测到楼下有货，发送指令到升降机运货到楼上
{
ElevatorFactory.getElevator().setDataCommand(COMMAND_FROMS2E.LIFT_IN_COMMAND_UP);
while (ElevatorFactory.getElevator().getOutCommand() != COMMAND_FROME2S.LIFT_OUT_COMMAND_DOWN) //等待升降机送货到楼上
{
Console.WriteLine("wait lifter up goods");
Thread.Sleep(100);
}
if (upRecordStep > 0) //升降机将货运到楼上,保证upRecordStep不小于0
{
upRecordStep--;
AGVLog.WriteError("上货期间 升降机将货运送到楼上: step = " + upRecordStep, new StackFrame(true));
}
}
if (ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_DOWN
|| ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_UP_DOWN) //上货期间 楼上楼下都有货， 楼上的车子需要继续运行
{
lock (LockController.getLockController().getLockForkLift()) {
//运动到楼上后，发送指令到楼上AGV，把货取走
tmpForkLiftWrapper = getSheduleForkLift();
if (tmpForkLiftWrapper != null) {
tmpSingleTask = getUpPickSingleTaskOnTurn();
TaskRecord tr_tmp = new TaskRecord();
tr_tmp.singleTask = tmpSingleTask;
tr_tmp.taskRecordName = tmpSingleTask.taskName;
TaskReordService.getInstance().addTaskRecord(tr_tmp);
tmpForkLiftWrapper.sendTask(tr_tmp); //发送任务
upPicSingleTaskkUsed++; //用于下次切换卸货点
} else {
Console.WriteLine(" 楼上没有可用的车去卸货");
AGVLog.WriteError(" 楼上没有可用的车去卸货", new StackFrame(true));
}
}
if (tmpForkLiftWrapper != null) {
while (ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_DOWN
|| ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_UP_DOWN) //等待楼上货物被取走,如果车的状态回到idle,说明任务发送失败
{
if (tmpForkLiftWrapper.getForkLift().taskStep == TASK_STEP.TASK_IDLE) {
break;
}
Console.WriteLine("wait lifter goods to be pick");
Thread.Sleep(500);
}
if (upRecordStep > 0) //升降机将货运到楼上,保证upRecordStep不小于0
{
upRecordStep--;
AGVLog.WriteError("上货期间 楼上货物被取走: step = " + upRecordStep, new StackFrame(true));
}
}
}
if (checkDownDeliverPeriodOver(upRecordStep)) //检测上料任务有没有结束 条件1：没有上料任务缓存 条件2：所有车子空闲 条件3：升降机上没有货物
{
ScheduleFactory.getSchedule().setDownDeliverPeriod ( false);
}
downRecordStep = 0; //下料信号置0
Console.WriteLine("上料阶段");
}
if (upTaskRecord != null) {
if (!ScheduleFactory.getSchedule().getDownDeliverPeriod() && !checkUpDeliverPeriodOver(downRecordStep)) //检查下货任务有没有结束，升降机从楼上到楼下流程走完，没有正在执行的下货任务
{
Console.WriteLine("当前下货任务没有执行完成，执行完后再开始执行上货任务");
} else if (upTaskRecord.taskRecordStat == TASKSTAT_T.TASK_READY_SEND) {
tmpForkLiftWrapper = ForkLiftWrappersService.getInstance().getForkLiftByNunber(3); //获取楼下三号车
if (tmpForkLiftWrapper.getForkLift().taskStep != TASK_STEP.TASK_IDLE) {
Console.WriteLine("上料任务正在执行，等待上料任务执行完成");
continue;
}
if (ElevatorFactory.getElevator().getOutCommand() != COMMAND_FROME2S.LIFT_OUT_COMMAND_MIN) //只要升降机上有货或有异常都不发送上货任务，否则容易造成楼上楼下都要货
{
Console.WriteLine(" 升降机楼下有货，不发送上货任务");
} else {
lock (LockController.getLockController().getLockForkLift()) //锁住车的状态
{
result = tmpForkLiftWrapper.sendTask(upTaskRecord); //发送任务
ScheduleFactory.getSchedule().setDownDeliverPeriod(true);//上货任务发送后，才进入上料阶段
if (result == 0) //发送成功 才正式进入上货阶段
{
if (upRecordStep <= 2) //避免上货 step被加得太多，不能进入下货阶段
{
upRecordStep += 2;
AGVLog.WriteError("上货期间 发送任务: step = " + upRecordStep, new StackFrame(true));
}
}
}
}
}
}
//检测升降机2楼有货物，发送指令将升降机送货到楼下
//检测升降机1楼有货物，调度1楼AGV送货
//读取升降机上料信号
if (!ScheduleFactory.getSchedule().getDownDeliverPeriod()) {
Console.WriteLine(" 下料阶段");
upRecordStep = 0; //上料信号置0
if (ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_DOWN) //检测到楼上有货，发送指令到升降机运货到楼下
{
int times_tmp = 0;
ElevatorFactory.getElevator().setDataCommand(COMMAND_FROMS2E.LIFT_IN_COMMAND_DOWN);
while (ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_DOWN && times_tmp < 60) //等待升降机送货到楼下
{
Console.WriteLine("wait lifter down goods"); //光电感应大概10S可以结束
times_tmp++;
Thread.Sleep(1000);
}
if (times_tmp < 60) {
if (downRecordStep > 0) //升降机将货运到楼上,保证upRecordStep不小于0
{
downRecordStep--;
AGVLog.WriteError("下货期间 楼上货物送到楼下: step = " + downRecordStep, new StackFrame(true));
}
}
times_tmp = 0;
}
if (ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_UP ||
ElevatorFactory.getElevator().getOutCommand() == COMMAND_FROME2S.LIFT_OUT_COMMAND_UP_DOWN) //检测到楼下有货，通知AGV来取货
{
TaskRecord tr_tmp = new TaskRecord();
tmpForkLiftWrapper = ForkLiftWrappersService.getInstance().getForkLiftByNunber(3);
tmpSingleTask = getDownPickSingleTaskOnTurn();
tr_tmp.singleTask = tmpSingleTask;
tr_tmp.taskRecordName = tmpSingleTask.taskName;
if (tmpForkLiftWrapper != null && tmpForkLiftWrapper.getForkLift().taskStep == TASK_STEP.TASK_IDLE) {
TaskReordService.getInstance().addTaskRecord(tr_tmp); //发货后，才确认添加该记录
result = tmpForkLiftWrapper.sendTask(tr_tmp);
if (result == 0) //任务发送成功
{
downPicSingleTaskkUsed++; //用于下次切换卸货点
if (downRecordStep > 0) //升降机将货运到楼上,保证upRecordStep不小于0
{
downRecordStep--;
AGVLog.WriteError("下货期间 楼下货物被取走: step = " + downRecordStep, new StackFrame(true));
}
} else {
TaskReordService.getInstance().removeTaskRecord(tr_tmp.singleTask, tr_tmp.taskRecordStat); //如果任务没发送成功，删除该条记录
}
}
}
lock (LockController.getLockController().getLockTask()) {
foreach (TaskRecord tr in taskRecordList) {
lock (LockController.getLockController().getLockForkLift()) {
if (tr.taskRecordStat == TASKSTAT_T.TASK_READY_SEND) {
tmpForkLiftWrapper = null;
if (tr.singleTask.taskType == TASKTYPE_T.TASK_TYPE_DOWN_PICK)
tmpForkLiftWrapper = ForkLiftWrappersService.getInstance().getForkLiftByNunber(3);
else if (tr.singleTask.taskType == TASKTYPE_T.TASK_TYPE_UP_DILIVERY)
tmpForkLiftWrapper = getSheduleForkLift(); //有任务执行的时候，才考虑检查车子状态
//if (fl.getForkLift().taskStep == TASK_STEP.TASK_IDLE && fl.finishStatus == 1) //检查车子的状态，向空闲的车子发送任务,如果发送失败，后面会检测发送状态，
//并将该任务状态改成待发重新发送
if (tmpForkLiftWrapper != null && tmpForkLiftWrapper.getForkLift().taskStep == TASK_STEP.TASK_IDLE) {
result = tmpForkLiftWrapper.sendTask(tr); //发送任务
if (result == -1) //任务没有发送成功会中断本次循环，防止发送任务到后面的车
{
break;
}
if (tr.singleTask.taskType == TASKTYPE_T.TASK_TYPE_UP_DILIVERY && downRecordStep < 4) //发送的是楼上送货，并且送货发送次数小于2次
{
downRecordStep += 2;
AGVLog.WriteError("下货期间 发送任务: step = " + downRecordStep, new StackFrame(true));
}
}
}
}
}
}
}
}
}