KRITSADA

robot 3 sensors and and drop

Nov 4th, 2019
226
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <popx2.h>  // POP-X2 Board
  2. int speed = 55, del = 280;
  3. float var = 0.96;
  4. int s1, s2, s3;
  5.  
  6. void setup() {
  7.   glcdMode(1);
  8.   setTextSize(3);
  9.   servo(1,120);
  10. //  while(!(sw_ok()))
  11. //  {
  12. //    var = knob(95,120);
  13. //    var = var/100;
  14. //    glcdClear();
  15. //    glcd(1,0,"%f",var);
  16. //  }
  17. //  delay(1);
  18. //  while(!(sw_ok()))
  19. //  {
  20. //    speed = knob(45,60);
  21. //    glcdClear();
  22. //    glcd(1,0,"%d",speed);
  23. //  }
  24.   OK();
  25.   glcdClear();
  26.   delay(1000);
  27.   RR();
  28.   RR();
  29.   RR();
  30.   LL();
  31.   track();
  32.   drop();           //green
  33.   BL();
  34.   RR();
  35.   RR();
  36.   track();
  37.   drop();           //blue
  38.   bk2(speed*var,speed);
  39.   delay(300);
  40.   fd2(speed*var,-speed);
  41.   delay(del*2);
  42.   LL();
  43.   fd2(speed*var,speed);
  44.   delay(1500);
  45.   fd2(speed*var,-speed);
  46.   delay(del);
  47.   RR();
  48.   track();
  49.   drop();           //red
  50.   fd2(speed*var,-speed);
  51.   delay(del*2-30);
  52.   track();
  53.   drop();           //yellow
  54.   fd2(-speed*var*1.05,-speed);
  55.   delay(750);
  56.   fd2(-speed*var,speed);
  57.   delay(del+20);
  58.   RR();
  59.   LL();
  60.   LL();
  61.   RR();
  62.   track();
  63.   fd2(-speed*var,-speed);
  64.   delay(1000);
  65.   fd2(speed*var,-speed);
  66.   delay(del);
  67.   fd2(speed*var,speed);
  68.   delay(800);
  69.   fd2(-speed*var,speed);
  70.   delay(del);
  71.   track();
  72.   fd2(speed*var,speed);
  73.   delay(400);
  74.   RR();
  75.   RR();
  76.   LL();
  77.   LL();
  78.   LL();
  79.   track();
  80.   ao();
  81. }
  82.  
  83. void track()
  84. {
  85.   while(true)
  86.   {
  87.     s1 = analogRead(0);
  88.     s2 = analogRead(1);
  89.     s3 = analogRead(2);
  90.     if(s1 >= 512 && s2 >= 512)
  91.     {
  92.       fd2(65*0.99,65);
  93.     }
  94.     else if(s1 <= 512 && s2 >= 512)
  95.     {
  96.       fd2(speed*var,speed/3);
  97.     }
  98.     else if(s1 >= 512 && s2 <= 512)
  99.     {
  100.       fd2(speed*var/3,speed);
  101.     }
  102.     if(s3 < 440)
  103.     {
  104.       break;
  105.     }
  106.   }
  107.   check();
  108. }
  109.  
  110. void check()
  111. {
  112.   while(true)
  113.   {
  114.     s1 = analogRead(0);
  115.     s2 = analogRead(1);
  116.     s3 = analogRead(2);
  117.     if(s1 >= 512 && s2 >= 512)
  118.     {
  119.       fd2(speed*var/2,speed/2);
  120.     }
  121.     else if(s1 >= 512 && s2 <= 512)
  122.     {
  123.       fd2(speed*var*2.18/2,speed/2.5);
  124.     }
  125.     else if(s1 <= 512 && s2 >= 512)
  126.     {
  127.       fd2(speed*var/2.5,speed*2.18/2);
  128.     }
  129.     else
  130.     {
  131.       break;
  132.     }
  133.   }
  134. }
  135.  
  136. void RR()
  137. {
  138.   track();
  139.   fd2(-speed*var,-speed);
  140.   delay(150);
  141.   fd2(speed*var,-speed);
  142.   delay(del);
  143.   ao();
  144.   delay(200);
  145. }
  146.  
  147. void BL()
  148. {
  149.   fd2(-speed*var*1.05,-speed);
  150.   delay(800);
  151.   fd2(-speed*var,speed);
  152.   delay(del);
  153.   delay(20);
  154.   track();
  155.   fd2(speed*var,speed);
  156.   delay(400);
  157.     s1 = analogRead(0);
  158.     s2 = analogRead(1);
  159.   while(true)
  160.   {
  161.     s1 = analogRead(0);
  162.     s2 = analogRead(1);
  163.     if(s1 <= 512 && s2 <= 512)
  164.     {
  165.       fd2(speed*var,speed);
  166.     }
  167.     else if(s1 <= 512 && s2 >= 512)
  168.     {
  169.       fd2(speed*var,-speed);
  170.     }
  171.     else if(s1 >= 512 && s2 <= 512)
  172.     {
  173.       fd2(-speed*var,speed);
  174.     }
  175.     else
  176.     {
  177.       break;
  178.     }
  179.   }
  180. }
  181.  
  182. void LL()
  183. {
  184.   track();
  185.   fd2(-speed*var,-speed);
  186.   delay(150);
  187.   fd2(-speed*var,speed);
  188.   delay(del);
  189.   ao();
  190.   delay(200);
  191. }
  192.  
  193. void drop()
  194. {
  195. fd2(-speed*var,-speed);
  196. delay(400);
  197. ao();
  198. servo(1,0);
  199. delay(600);
  200. servo(1,150);
  201. delay(800);
  202. servo(1,120);
  203. }
  204.  
  205. void loop() {
  206. }
Add Comment
Please, Sign In to add comment