API tools faq
paste
Advertisement
KRITSADA

IPST STEM FESTIVAL 2016 Full Code

KRITSADA
Oct 8th, 2016
277
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 14.59 KB | None | 0 0
raw download clone embed print report
  1. #include <ipst.h>   // IPST-SE Board
  2. word L, R, A, B, C,BAR, i;
  3. word whiteBrown = 700, blackBrown = 300, ref = 455, WHITE = 0, BLACK = 0, BROWN = 0;
  4. word N = 53, E = 122, S = 194, W = 300;
  5. void check()
  6. {
  7.   glcdClear();
  8.   compass_readConfig();
  9.   while (1)
  10.   {
  11.     glcd(1, 1, "R= %d   ", analog(1));
  12.     glcd(2, 1, "L= %d   ", analog(0));
  13.  
  14.   }
  15.  
  16. }
  17. void setup() {
  18.  compass_readConfig();
  19.  compass_factoryReset();
  20.  if(sw1()){
  21.    while(sw1());
  22.    setTextSize(2);glcdMode(3);
  23.    glcd(0,0,"ON BLACK");
  24.    glcd(1,0,"press OK()");
  25.    sw_OK_press();
  26.    while(sw_OK());
  27.    for(i=0;i<20;i++){BLACK=analog(0)+BLACK;}BLACK=BLACK/20;
  28.    glcd(2,0,"Black=%d",BLACK);delay(2000);setTextColor(GLCD_YELLOW);
  29.    glcd(0,0,"ON WHITE");
  30.    glcd(1,0,"press OK()");
  31.    sw_OK_press();
  32.    while(sw_OK());
  33.    for(i=0;i<20;i++){WHITE=analog(0)+WHITE;}WHITE=WHITE/20;
  34.    glcd(2,0,"WHITE=%d",WHITE);delay(2000);setTextColor(GLCD_BLUE);
  35.    glcd(0,0,"ON BROWN");
  36.    glcd(1,0,"press OK()");
  37.    sw_OK_press();
  38.    while(sw_OK());
  39.    for(i=0;i<20;i++){BROWN=analog(0)+BROWN;}BROWN=BROWN/20;
  40.    glcd(2,0,"BROWN=%d",BROWN);delay(2000);setTextColor(GLCD_WHITE);
  41.    whiteBrown=(WHITE+BROWN)/2;
  42.    glcd(3,0,"RefH=%d",whiteBrown);
  43.    ref=(WHITE+BLACK)/2;
  44.    glcd(4,0,"RefM=%d",ref);
  45.    blackBrown=(BLACK+BROWN)/2;
  46.    glcd(5,0,"RefL=%d",blackBrown);delay(2000);
  47.    glcd(0,0,"COMPASS");
  48.    glcd(1,0,"ON NORTH  ");
  49.    glcd(2,0,"press OK()");
  50.    sw_OK_press();
  51.    while(sw_OK());
  52.    N=compass_read();
  53.    glcd(3,0,"NORTH %d",N);delay(2000);
  54.    glcd(0,0,"COMPASS");
  55.    glcd(1,0,"ON EAST   ");
  56.    glcd(2,0,"press OK()");
  57.    sw_OK_press();
  58.    while(sw_OK());
  59.    E=compass_read();
  60.    glcd(3,0,"EAST  %d",E);delay(2000);
  61.    glcd(0,0,"COMPASS");
  62.    glcd(1,0,"ON SOUTH   ");
  63.    glcd(2,0,"press OK()");
  64.    sw_OK_press();
  65.    while(sw_OK());
  66.    S=compass_read();
  67.    glcd(3,0,"SOUTH  %d",S);delay(2000);
  68.    glcd(0,0,"COMPASS");
  69.    glcd(1,0,"ON WEST   ");
  70.    glcd(2,0,"press OK()");
  71.    sw_OK_press();
  72.    while(sw_OK());
  73.    W=compass_read();
  74.    glcd(3,0,"SOUTH  %d",W);delay(2000);
  75.    setTextColor(GLCD_RED);
  76.    glcd(0,0,"SETUP       ");
  77.    glcd(1,0,"COMPLETE    ");
  78.    glcd(2,0,"press OK()  ");
  79.    glcd(3,0,"to START    ");
  80.    glcd(4,0,"____________"); delay(1000);
  81.  }
  82.  OK();
  83.  barCode();fd(30);delay(300);
  84.  if(A){R_first();}
  85.  else {L_first();}
  86.  
  87.  
  88. }
  89. void loop() {
  90. }
  91. void L_first() {
  92.   LLL();              // เจอแยกเลี้ยวซ้าย
  93.   SSS();               // เจอสี่ออนกว่าสีดำหยุด
  94.   barCode();          // อ่านบาร์โค้ด
  95.   if (A) {
  96.     fire13(); // ดับไฟดวง 1 และ 3
  97.   }
  98.   else {
  99.     fire24(); // ดับไฟดวง 2 และ 4
  100.   }
  101.   MazeLeft();         // เดินในเขาวงกต
  102.   LLL();              // เจอแยกเลี้ยวซ้าย
  103.   SSS();               // เจอแยกหยุด
  104. }
  105. void R_first() {
  106.   RRR();
  107.   trackUntilWall();
  108.   MazeRight();
  109.   barCode();
  110.   if (A) {
  111.     fire31();
  112.   }
  113.   else {
  114.     fire24();
  115.   }
  116.   RRR();
  117.   SSS();
  118. }
  119. void trackUntilWall() {
  120.   while (in(27) && in(30)) {
  121.     L = analog(0);
  122.     R = analog(1);
  123.     if (L < ref && R < ref) {
  124.       fd2(30, 33);
  125.     }
  126.     else if (L > ref && R < ref) {
  127.       sl(40);
  128.     }
  129.     else if (L < ref && R > ref) {
  130.       sr(40);
  131.     }
  132.   }
  133. }
  134. void MazeRight() {
  135.   trackWallCompass(N);
  136.   bk(30); delay(200); ao();
  137.   R90(W);
  138.   trackWallCompass(W);  // ใข่ใบที่ 1
  139.   bk(30); delay(1000); ao();
  140.   trackTimeBackCompass(W, 1000);
  141.   L90(S);
  142.   trackWallCompass(S);   // ใข่ใบที่ 2
  143.   trackWallBackCompass(S);
  144.   fd(30); delay(200); ao();
  145.   L90(E);
  146.   trackWallCompass(E);   // ใข่ใบที่ 3
  147.   trackWallBackCompass(E);
  148.   fd(30); delay(250); ao();
  149.   R90(S);
  150.   trackWallCompass(S);
  151.   bk(30); delay(250); ao();
  152.   R90(W);
  153.   trackWallCompass(W);
  154.   bk(30); delay(250); ao();
  155.   L90(N);
  156.   trackWallCompass(N);   // ใข่ใบที่ 4
  157.   trackWallBackCompass(N);
  158.   fd(30); delay(250); ao();
  159.   R90(E);
  160.   trackTimeCompass(E, 1500);
  161.   R90(S);
  162.   trackWallCompass(S);
  163.   bk(30); delay(250); ao();
  164.   R90(W);
  165.   trackWallCompass(W);
  166.   bk(30); delay(250); ao();
  167.   L90(S);
  168.   trackWallCompass(S);  // ใขใบที่ 5
  169.   trackWallBackCompass(S);
  170.   fd(30); delay(250); ao();
  171.   L90(E);
  172.   trackWallCompass(E);
  173.   bk(30); delay(250); ao();
  174.   R90(S);
  175.   trackWallCompass(S);
  176.   bk(30); delay(250); ao();
  177.   R90(W);
  178.   trackTimeCompass(W, 1900);
  179.   L90(S);
  180.   trackTimeCompass(S, 1500);
  181. }
  182. void fire31() {
  183.   fd(30); delay(200);
  184.   trackTimeCompass(S, 2000);
  185.   R90(S + 37);
  186.   trackTimeCompass(S + 37, 500);
  187.   fire(2000);
  188.   bk(30); delay(500);
  189.   L90(E - 37);
  190.   trackTimeCompass(E - 37, 1000);
  191.   fire(2000);
  192.   bk(30); delay(600);
  193.   R90(E);
  194.   trackTimeCompass(E, 2400);
  195.   RRR();
  196.  
  197. }
  198.  
  199. void fire42() {
  200.   fd(30); delay(200);
  201.   trackTimeCompass(S, 2000);
  202.   L90(S - 42);
  203.   trackTimeCompass(S - 42, 500);
  204.   fire(2000);
  205.   bk(30); delay(800);
  206.   L90(N - 20);
  207.   trackTimeCompass(N - 20, 800);
  208.   fire(2000);
  209.   bk(30); delay(600);
  210.   R90(E);
  211.   trackTimeCompass(E, 2400);
  212.   RRR();
  213.  
  214. }
  215. void MazeLeft() {
  216.   bk(30); delay(100);
  217.   trackWallCompass(N);
  218.   bk(30); delay(300); ao();
  219.   R90(E);
  220.   trackWallCompass(E);
  221.   bk(30); delay(300); ao();
  222.   L90(N);
  223.   trackWallCompass(N);
  224.   bk(30); delay(250); ao();
  225.   R90(W);
  226.   trackWallCompass(W);
  227.   bk(30); delay(300); ao();
  228.   L90(S);
  229.   trackWallCompass(S);  //  ใข่ใบที่ 1
  230.   trackWallBackCompass(S);
  231.   fd(30); delay(250); ao();
  232.   L90(E);
  233.   trackTimeCompass(E, 1000);
  234.   L90(N);
  235.   trackWallCompass(N);
  236.   bk(30); delay(150); ao();
  237.   R90(W);
  238.   trackWallCompass(W);
  239.   bk(30); delay(100); ao();
  240.   L90(N);
  241.   trackWallCompass(N);   // ใข่ใบที่ 2
  242.   trackWallBackCompass(N);
  243.   fd(30); delay(250); ao();
  244.   R90(E);
  245.   trackWallCompass(E);
  246.   bk(30); delay(100); ao();
  247.   L90(N);
  248.   trackWallCompass(N);
  249.   bk(30); delay(150); ao();
  250.   R90(E);
  251.   trackWallCompass(E);  // ใข่ใบที่ 3
  252.   trackTimeBackCompass(E, 1000);
  253.   R90(S);
  254.   trackWallCompass(S);
  255.   bk(30); delay(150); ao();
  256.   R90(W);
  257.   trackWallCompass(W);  // ใข่ใบที่ 4
  258.   bk(30); delay(1000); ao();
  259.   trackTimeBackCompass(W, 1000);
  260.   L90(S);
  261.   trackWallCompass(S);
  262.   bk(30); delay(1000); ao(); // ใข่ใบที่ 5
  263.   L90(E);
  264.   trackWallCompass(E);
  265.   bk(30); delay(100); ao();
  266.   R90(S);
  267.   trackTimeCompass(S, 3000);
  268.   LLL(); ao();
  269. }
  270. void R90(int c) {
  271.   compass_readConfig();
  272.   while (compass_read() < c) {
  273.     sr(30);
  274.   }
  275. }
  276. void L90(int c) {
  277.   compass_readConfig();
  278.   while (compass_read() > c) {
  279.     sl(30);
  280.   }
  281. }
  282. void trackWallCompass(int c) { //แทร็กเข็มทิศ
  283.   compass_readConfig();
  284.   while (in(27) && in(30)) {
  285.     if (compass_read() <= c + 2 && compass_read() >= c - 2) {
  286.       fd(30);
  287.     }
  288.     else if (compass_read() < c + 2) {
  289.       tr(30);
  290.     }
  291.     else if (compass_read() > c - 2) {
  292.       tl(30);
  293.     }
  294.   } ao();
  295. }
  296. void trackTimeCompass(int c, long t) { //แทร็กเข็มทิศ
  297.   long m = millis();
  298.   compass_readConfig();
  299.   while (millis() < (m + t)) {
  300.     if (compass_read() <= c + 2 && compass_read() >= c - 2) {
  301.       fd(30);
  302.     }
  303.     else if (compass_read() < c + 2) {
  304.       tr(30);
  305.     }
  306.     else if (compass_read() > c - 2) {
  307.       tl(30);
  308.     }
  309.   } ao();
  310. }
  311. void trackTimeBackCompass(int c, long t) { //แทร็กเข็มทิศ หยุดเมื่อหมดเวลา
  312.   long m = millis();
  313.   compass_readConfig();
  314.   while (millis() < (m + t)) {
  315.     if (compass_read() <= c + 2 && compass_read() >= c - 2) {
  316.       bk(30);
  317.     }
  318.     else if (compass_read() < c + 2) {
  319.       bk2(0, 30);
  320.     }
  321.     else if (compass_read() > c - 2) {
  322.       bk2(30, 0);
  323.     }
  324.   } ao();
  325. }
  326.  
  327. void trackWallBackCompass(int c) { //แทร็กเข็มทิศ  หยุดเมื่อเจอกำแพง
  328.   compass_readConfig();
  329.   while (in(14) && in(15)) {
  330.     if (compass_read() <= c + 2 && compass_read() >= c - 2) {
  331.       bk(30);
  332.     }
  333.     else if (compass_read() < c + 2) {
  334.       bk2(0, 30);
  335.     }
  336.     else if (compass_read() > c - 2) {
  337.       bk2(30, 0);
  338.     }
  339.   } ao();
  340. }
  341.  
  342. void SSS() {               // เดินตามเส้นห หยุดเมื่อเจอ ทั้งคู่เจอขาว
  343.   while (1) {
  344.     L = analog(0);
  345.     R = analog(1);
  346.     if (L < ref && R < ref) {
  347.       fd2(30, 33);
  348.     }
  349.     else if (L > ref && R < ref) {
  350.       sl(40);
  351.     }
  352.     else if (L < ref && R > ref) {
  353.       sr(40);
  354.     }
  355.     else if (L > ref && R > ref) {
  356.       ao(); break;
  357.     }
  358.   }
  359. }
  360. void FFF() {
  361.   while (1) {
  362.     L = analog(0); R = analog(1);
  363.     if (L < ref && R < ref) {
  364.       fd2(30, 33);
  365.     }
  366.     else if (L > ref && R < ref) {
  367.       sl(40);
  368.     }
  369.     else if (L < ref && R > ref) {
  370.       sr(40);
  371.     }
  372.     else if (L > ref && R > ref) {
  373.       fd(30); delay(200);
  374.       break;
  375.     }
  376.   }
  377.  
  378. }
  379. void LLL() {
  380.   while (1) {
  381.     L = analog(0);
  382.     R = analog(1);
  383.     if (L < ref && R < ref) {
  384.       fd2(30, 33);
  385.     }
  386.     else if (L > ref && R < ref) {
  387.       sl(40);
  388.     }
  389.     else if (L < ref && R > ref) {
  390.       sr(40);
  391.     }
  392.     else if (L > ref && R > ref) {
  393.       fd(30); delay(200);
  394.       while (analog(0) < ref) {
  395.         sl(40);
  396.       }
  397.       while (analog(0) > ref) {
  398.         sl(40);
  399.       }
  400.       break;
  401.     }
  402.   }
  403. }
  404. void fire13() {
  405.   fd(30); delay(200);
  406.   while (analog(0) < ref) {
  407.     fd2(30, 29);
  408.   }
  409.   FFF();
  410.   trackTime(600);
  411.   fire(2000);
  412.   while (analog(0) < ref) {
  413.     sl(40);
  414.   }
  415.   while (analog(0) > ref) {
  416.     sl(40);
  417.   }
  418.   FFF();
  419.   trackTime(600);
  420.   fire(2000);
  421.   bk(30); delay(500);
  422.   sl(30); delay(225);
  423.   fd(30); delay(1000);
  424.   trackUntilWall(); ao();
  425. }
  426.  
  427. void fire24() {
  428.   fd(30); delay(200);
  429.   while (analog(0) < ref) {
  430.     fd2(40, 35);
  431.   }
  432.   LLL();
  433.   trackTime(600);
  434.   fire(2000);
  435.   while (analog(0) < ref) {
  436.     sl(40);
  437.   }
  438.   while (analog(0) > ref) {
  439.     sl(40);
  440.   }
  441.   FFF();
  442.   trackTime(600);
  443.   fire(2000);
  444.   bk(30); delay(500);
  445.   sr(30); delay(225);
  446.   fd(30); delay(1000);
  447.   trackUntilWall(); ao();
  448. }
  449. void RRR() {
  450.   while (in(27) && in(30)) {
  451.     L = analog(0);
  452.     R = analog(1);
  453.     if (L < ref && R < ref) {
  454.       fd2(30, 33);
  455.     }
  456.     else if (L > ref && R < ref) {
  457.       sl(40);
  458.     }
  459.     else if (L < ref && R > ref) {
  460.       sr(40);
  461.     }
  462.     else if (L > ref && R > ref) {
  463.       fd(30); delay(200);
  464.       while (analog(1) < ref) {
  465.         sr(40);
  466.       }
  467.       while (analog(1) > ref) {
  468.         sr(40);
  469.       }
  470.     }
  471.   }
  472. }
  473. void trackTime(long t) {
  474.   long m = millis();
  475.   while (millis() < (m + t)) {
  476.     L = analog(0);
  477.     R = analog(1);
  478.     if (L < ref && R < ref) {
  479.       fd2(30, 33);
  480.     }
  481.     else if (L > ref && R < ref) {
  482.       sl(40);
  483.     }
  484.     else if (L < ref && R > ref) {
  485.       sr(40);
  486.     }
  487.   }
  488.   ao();
  489. }
  490.  
  491.  
  492.  
  493. void barCode() {
  494.   setTextSize(2);
  495.   setTextColor(GLCD_BLACK);
  496.   setTextBackgroundColor(GLCD_WHITE);
  497.   while (analog(0) < blackBrown) {
  498.     fd(27);
  499.   } ao(); delay(100);
  500.   while (analog(0) > blackBrown && analog(0) < whiteBrown) {
  501.     fd(27);
  502.   } ao(); delay(100);
  503.   if (analog(0) < blackBrown) {
  504.     glcd(2, 0, "BLACK"); A = 1;
  505.     while (analog(0) < blackBrown) {
  506.       fd(27);
  507.     } ao(); delay(100);
  508.   }
  509.   else if (analog(0) > whiteBrown) {
  510.     glcd(2, 0, "WHITE"); A = 0;
  511.     while (analog(0) > whiteBrown) {
  512.       fd(27);
  513.     } ao(); delay(100);
  514.   }
  515.   while (analog(0) > blackBrown && analog(0) < whiteBrown) {
  516.     fd(27);
  517.   } ao(); delay(100);
  518.   if (analog(0) < blackBrown) {
  519.     glcd(3, 0, "BLACK"); B = 2;
  520.     while (analog(0) < blackBrown) {
  521.       fd(27);
  522.     } ao(); delay(100);
  523.   }
  524.   else if (analog(0) > whiteBrown) {
  525.     glcd(3, 0, "WHITE"); B = 0;
  526.     while (analog(0) > whiteBrown) {
  527.       fd(27);
  528.     } ao(); delay(100);
  529.   }
  530.   while (analog(0) > blackBrown && analog(0) < whiteBrown) {
  531.     fd(27);
  532.   } ao(); delay(100);
  533.   if (analog(0) < blackBrown) {
  534.     glcd(4, 0, "BLACK"); C = 4;
  535.     while (analog(0) < blackBrown) {
  536.       fd(27);
  537.     } ao(); delay(100);
  538.   }
  539.   else if (analog(0) > whiteBrown) {
  540.     glcd(4, 0, "WHITE"); C = 0;
  541.     while (analog(0) > whiteBrown) {
  542.       fd(27);
  543.     } ao(); delay(100);
  544.   }
  545.   BAR = A + B + C;
  546.   glcdFillScreen(GLCD_BLUE);
  547.   setTextSize(10);
  548.   setTextBackgroundColor(GLCD_BLUE);
  549.   setTextColor(GLCD_YELLOW);
  550.   glcd(0, 1, "%d ", A + B + C);
  551. }
  552.  
  553. void rdSensor() {
  554.   setTextSize(2); glcdMode(1);
  555.   while (sw_1() == 0) {
  556.     L = analog(0); R = analog(1);
  557.     glcd(0, 0, "L=%d   ", L);
  558.     glcd(1, 0, "R=%d   ", R);
  559.     glcd(2, 0, "%d  ", compass_read());
  560.   }
  561. }
  562. void fire(int t) {
  563.   out(12, 1);
  564.   delay(t);
  565.   out(12, 0);
  566. }
  567.  
  568.  
  569. //////////////////////////////////////////////////
  570. void barCodenat()
  571. {
  572.   setTextSize(2);
  573.   setTextColor(GLCD_BLACK);
  574.   setTextBackgroundColor(GLCD_WHITE);
  575.   /////// ทดสอบเดินบนพื่นที่สีดำ
  576.   ao(); delay(500);
  577.   while (analog(1) < 100) //ค่าสีดำ
  578.   {
  579.     fd(20);
  580.   }
  581.   ////////////////////////
  582.   ao(); delay(100);
  583.   bar1();
  584.  
  585. }
  586. void bar1()
  587. {
  588.   /////เส้นที่ 1
  589.   while (analog(1) > 100 && analog(1) < 320) ////เมื่อเซ็นเซอร์เจอสีน้ำตาล ให้เดินไปเรื่อย
  590.   {
  591.     fd(20);
  592.   }
  593.   //ao();delay(100);
  594.   fd(20); delay(100);
  595.  
  596.   if (analog(1) < 100)
  597.   {
  598.     A = 4;
  599.     while (analog(1) < 150)
  600.     {
  601.       fd(20);
  602.     }
  603.    }
  604.   else
  605.   {
  606.     A = 0;
  607.     while (analog(1) > 300)
  608.     {
  609.       fd(20);
  610.     }
  611.     }
  612. bar2();
  613. }
  614. void bar2()
  615. {  
  616.   /////เส้นที่ 2
  617.   while (analog(1) > 100 && analog(1) < 300) ////เมื่อเซ็นเซอร์เจอสีน้ำตาล ให้เดินไปเรื่อย
  618.   {
  619.     fd(20);
  620.   }
  621.   fd(20); delay(100);
  622.  
  623.   if (analog(1) < 100)
  624.   {
  625.     B = 2;
  626.     while (analog(1) < 150)
  627.     {
  628.       fd(20);
  629.     }
  630.     }
  631.   else
  632.   {
  633.     B = 0;
  634.     while (analog(1) > 300)
  635.     {
  636.       fd(20);
  637.     }
  638.   }
  639. bar3();
  640.  
  641. }
  642. void bar3()
  643. {
  644.   /////เส้นที่ 3
  645.   while (analog(1) > 100 && analog(1) < 400) ////เมื่อเซ็นเซอร์เจอสีน้ำตาล ให้เดินไปเรื่อย
  646.   {
  647.     fd(20);
  648.   }
  649.   fd(20); delay(100);
  650.  
  651.   if (analog(1) < 150)
  652.   {
  653.     C = 1;
  654.    
  655.   }
  656.   else
  657.   {
  658.     C = 0;
  659.      }
  660. ao();
  661.   BAR = A + B + C;
  662.   glcdFillScreen(GLCD_BLACK);
  663.   setTextSize(10);
  664.   setTextBackgroundColor(GLCD_BLACK);
  665.   setTextColor(GLCD_YELLOW);
  666.   glcd(0, 1, "%d   ",BAR);
  667. delay(200);
  668. }
  669.  
  670. //////////////////////////////////////////////////////////
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!