//雙斜線後面, 表示是註解的文字
init: //表 initialization 程式一開始執行的部分
W = 99; // W代表White, 用來紀錄白色罐子所在位置, 可能是1,2,3,4或5.
B1 = 99; // B代表Blue, 99代表還沒有資料更新
B2 = 99; // B1,B2 用來紀錄藍色罐子, 所在的位置, 可能是1,2,3,4或5.
R1 = 99; R2 = 99; // R1,R2 用來紀錄紅色罐子, 所在的位置, 可能是1,2,3,4或5.
Pos = 99; // 表示機器人對應罐子的位置, 可能是1,2,3,4或5.
M1 = 99; // 表示完成任務的罐子顏色. 可能是 1(紅色)或2(藍色).
Loop = 0; // 表示機器人已經完成任務的圈數, 可能是0,1或2.
init_pos:
機器人位於出發位置
if (Loop == 2){ // 判斷 Loop 是否等於 2 的值.
TASK_COMPLETED; // 表示機器人已經完成2圈的任務, 停止程式執行, 讓機器人靜止不動.
}
init2grab: // from initial postion to grab zone. init2grab 表 init to(two 發音同) grab.
執行機器人由出發位置到進入抓罐子區域(下斜坡準備進入)的程式
//================================================================================================
grab2: // 執行抓2根罐子的演算法
//---------------------
// 第一圈抓取任務開始
//-----------------------------------------------------------begin >> 1 to 2
if(Loop==0 AND Pos = 99){ //表示第一次進入抓罐子區域
執行前往查看第一根罐子顏色的程式.
Pos = 1; 表機器人位置對應在第 1 根罐子
if (RGB == "白"){
W = 1;
不抓取罐子,執行到 RGB看下一根罐子的程式.
Pos = 2;
}
if (RGB == "紅"){
R1 = 1; //表示第一根紅罐子的位置.
執行左邊抓起罐子並到 RGB看下一根罐子的程式.(1->2)
Pos = 2;
}
if (RGB == "藍"){
B1 = 1; //表示第一根藍罐子的位置.
執行左邊抓起罐子並到 RGB看下一根罐子的程式.(1->2)
Pos = 2;
}
}
//-----------------------------------------------------------end
//-----------------------------------------------------------begin >> 2 to 3
if(Loop==0 AND Pos = 2){ //
if (RGB == "白"){
W = 2;
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (2->3)
Pos = 3;
}
if (RGB == "紅" AND R1 <99 ){ //表示看到第二根紅罐子
R2 = 2; //表示第二根紅罐子的位置.
執行右邊抓起罐子, 後退到準備返回的位置. (return)
Pos = 2;
}
if (RGB == "紅" AND R1 ==99 AND B1<99){ //表示看到第一根藍罐子被左邊抓起後, 再看到紅罐子
R1 = 2; //表示第一根紅罐子的位置.
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (2->3)
Pos = 3;
}
if (RGB == "紅" AND R1==99 AND B1==99){ //表示看到第一根紅罐子
R1 = 2; //表示第一根紅罐子的位置.
執行左邊抓起罐子並到 RGB看下一根罐子的程式. (2->3)
Pos = 3;
}
if (RGB == "藍" AND B1 <99 ){ //表示看到第二根藍罐子
B2 = 2; //表示第二根藍罐子的位置.
執行右邊抓起罐子, 後退到準備返回的位置. (return)
Pos = 2;
}
if (RGB == "藍" AND B1 ==99 AND R1<99){ //表示看到第一根紅罐子被左邊抓起後, 再看到藍罐子
B1 = 2; //表示第一根藍罐子的位置.
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (2->3)
Pos = 3;
}
if (RGB == "藍" AND B1==99 AND R1==99){ //表示看到第一根藍罐子
B1 = 2; //表示第一根藍罐子的位置.
執行左邊抓起罐子並到 RGB看下一根罐子的程式. (2->3)
Pos = 3;
}
}
//-----------------------------------------------------------end
//-----------------------------------------------------------begin >> 3 to 4
if(Loop==0 AND Pos = 3){ //
if (RGB == "白"){
W = 3;
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (3->4)
Pos = 4;
}
if (RGB == "紅" AND R1 <99 ){ //表示看到第二根紅罐子
R2 = 3; //表示第二根紅罐子的位置.
執行右邊抓起罐子, 後退到準備返回的位置. (return)
Pos = 3;
}
if (RGB == "紅" AND R1 ==99 AND B1<99){ //表示看到第一根藍罐子被左邊抓起後, 再看到紅罐子
R1 = 3; //表示第一根紅罐子的位置.
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (3->4)
Pos = 4;
}
if (RGB == "紅" AND R1==99 AND B1==99){ //表示看到第一根紅罐子
R1 = 3; //表示第一根紅罐子的位置.
執行左邊抓起罐子並到 RGB看下一根罐子的程式. (3->4)
Pos = 4;
}
if (RGB == "藍" AND B1 <99 ){ //表示看到第二根藍罐子
B2 = 3; //表示第二根藍罐子的位置.
執行右邊抓起罐子, 後退到準備返回的位置. (return)
Pos = 3;
}
if (RGB == "藍" AND B1 ==99 AND R1<99){ //表示看到第一根紅罐子被左邊抓起後, 再看到藍罐子
B1 = 3; //表示第一根藍罐子的位置.
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (3->4)
Pos = 4;
}
if (RGB == "藍" AND B1==99 AND R1==99){ //表示看到第一根藍罐子
B1 = 3; //表示第一根藍罐子的位置.
執行左邊抓起罐子並到 RGB看下一根罐子的程式. (3->4)
Pos = 4;
}
}
//-----------------------------------------------------------end
//-----------------------------------------------------------begin >> 4 to 5
if(Loop==0 AND Pos = 4){ //
if (RGB == "白"){
W = 4;
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (4->5)
Pos = 5;
}
if (RGB == "紅" AND R1 <99 ){ //表示看到第二根紅罐子
R2 = 4; //表示第二根紅罐子的位置.
執行右邊抓起罐子, 後退到準備返回的位置. (return)
Pos = 4;
}
if (RGB == "紅" AND R1 ==99 AND B1<99){ //表示看到第一根藍罐子被左邊抓起後, 再看到紅罐子
R1 = 4; //表示第一根紅罐子的位置.
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (4->5)
Pos = 5;
}
if (RGB == "紅" AND R1==99 AND B1==99){ //表示看到第一根紅罐子
R1 = 4; //表示第一根紅罐子的位置.
執行左邊抓起罐子並到 RGB看下一根罐子的程式. (4->5)
Pos = 5;
}
if (RGB == "藍" AND B1 <99 ){ //表示看到第二根藍罐子
B2 = 4; //表示第二根藍罐子的位置.
執行右邊抓起罐子, 後退到準備返回的位置. (return)
Pos = 4;
}
if (RGB == "藍" AND B1 ==99 AND R1<99){ //表示看到第一根紅罐子被左邊抓起後, 再看到藍罐子
B1 = 4; //表示第一根藍罐子的位置.
不抓取罐子,執行到 RGB看下一根(第三)罐子的程式. (4->5)
Pos = 5;
}
if (RGB == "藍" AND B1==99 AND R1==99){ //表示看到第一根藍罐子
B1 = 4; //表示第一根藍罐子的位置.
執行左邊抓起罐子並到 RGB看下一根罐子的程式. (4->5)
Pos = 5;
}
}
//-----------------------------------------------------------end
//-----------------------------------------------------------begin >> 5 to grab2init
if(Loop==0 AND Pos = 5){ //
if (RGB == "白"){
W = 5;
不抓取罐子,執行到 返回斜坡的程式. (5->grab2init)
Pos = 5;
}
if (RGB == "紅" AND R1 <99 ){ //表示看到第二根紅罐子
R2 = 5; //表示第二根紅罐子的位置.
執行右邊抓起罐子, 後退到準備返回的位置. (return)
Pos = 5;
}
if (RGB == "紅" AND R1 ==99 AND B1<99){ //表示看到第一根藍罐子被左邊抓起後, 再看到紅罐子
R1 = 5; //表示第一根紅罐子的位置.
不抓取罐子,執行到 返回斜坡的程式. (5->grab2init)
Pos = 5;
}
if (RGB == "紅" AND R1==99 AND B1==99){ //表示看到第一根紅罐子
R1 = 5; //表示第一根紅罐子的位置.
執行左邊抓起罐子並到 返回斜坡的程式. (5->grab2init)
Pos = 5;
}
if (RGB == "藍" AND B1 <99 ){ //表示看到第二根藍罐子
B2 = 5; //表示第二根藍罐子的位置.
執行右邊抓起罐子, 後退到準備返回的位置. (return)
Pos = 5;
}
if (RGB == "藍" AND B1 ==99 AND R1<99){ //表示看到第一根紅罐子被左邊抓起後, 再看到藍罐子
B1 = 5; //表示第一根藍罐子的位置.
不抓取罐子,執行到 返回斜坡的程式. (5->grab2init)
Pos = 5;
}
if (RGB == "藍" AND B1==99 AND R1==99){ //表示看到第一根藍罐子
B1 = 5; //表示第一根藍罐子的位置.
執行左邊抓起罐子並到 返回斜坡的程式. (5->grab2init)
Pos = 5;
}
}
//-----------------------------------------------------------end
// 第一圈抓取任務結束
//---------------------
//---------------------
// 第二圈抓取任務開始
//-----------------------------------------------------------begin
if(Loop==1){ //
???????
}
//-----------------------------------------------------------end
//================================================================================================
grab2init:
if(Pos ==2){
機器人抓了2根罐子後, 位置對應在第 2 根罐子, 執行適當的程式, 準備上斜坡回罐子置放區.
}
if(Pos ==3){
機器人抓了2根罐子後, 位置對應在第 3 根罐子, 執行適當的程式, 準備上斜坡回罐子置放區.
}
if(Pos ==4){
機器人抓了2根罐子後, 位置對應在第 4 根罐子, 執行適當的程式, 準備上斜坡回罐子置放區.
}
if(Pos ==5){
機器人抓了2根罐子後, 位置對應在第 5 根罐子, 執行適當的程式, 準備上斜坡回罐子置放區.
}
執行上斜坡返回罐子置放區, 並放下罐子回到出發位置(init_pos)的控制程式.
並設定完成任務罐子的顏色. M1 = 1(紅色); 或 M1 = 2(藍色);
Loop = Loop + 1; // 表示增加完成任務圈數1圈
控制程式再回到 init_pos: 重新執行控制程式.
沒有留言:
張貼留言