SHARE
TWEET

Untitled

a guest Feb 27th, 2020 77 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. void motor_forward_fixed(uint8 speed, uint32 delay)
  2. {
  3.     MotorDirLeft_Write(0);
  4.     MotorDirRight_Write(0);
  5.     float ratio = 1.05; // pitää säätää, 1.3 vähän liikaa
  6.    
  7.     uint8 l_speed = speed;
  8.     uint8 r_speed = (float)speed / ratio;
  9.    
  10.     printf("l_speed: %d, r_speed: %d\n", l_speed, r_speed); // tulostaa nopeudet
  11.    
  12.     motor_turn(l_speed, r_speed, delay);
  13. }  
  14. void motor_tank_turn_90dg(int left_dir, int right_dir)
  15.     {
  16.         MotorDirLeft_Write(left_dir);
  17.         MotorDirRight_Write(right_dir);
  18.         PWM_WriteCompare1(75);
  19.         PWM_WriteCompare2(75);
  20.         vTaskDelay(1500);
  21.     }
  22.  
  23. void turn_left()
  24. {
  25.    
  26.   motor_turn(20, 100, 50);
  27.  
  28. }
  29. void turn_right()
  30. {
  31.    
  32.  motor_turn(100, 20, 50);  
  33.  
  34. }
  35. void mild_turn_left()
  36. {
  37.    
  38.     motor_turn(40, 100, 50);
  39.  
  40. }
  41. void mild_turn_right()
  42. {
  43.    
  44.     motor_turn(100, 40, 50);
  45.  
  46. }
  47. void hard_turn_left()
  48. {
  49.    
  50. motor_turn(0, 100, 50);
  51.  
  52. }
  53. void hard_turn_right()
  54. {
  55.    
  56. motor_turn(100, 0, 50);
  57. }
  58.  
  59.  
  60.  
  61. void zmain(void)
  62. {
  63.     IR_Start(); //käynnistetään IR sensori
  64.     IR_flush();
  65.    
  66.     reflectance_start(); //Käynnistetään reflectance sensorit
  67.     //struct sensors_ ref;
  68.     struct sensors_ dig;
  69.     reflectance_set_threshold(9000, 9000, 11000, 11000, 9000, 9000);
  70.    
  71.     motor_start(); //käynnistetään moottori
  72.     motor_forward(0, 0); //pysäytetään moottorit
  73.    
  74.     int linjalaskuri = 0;
  75.     bool linjalla = false;
  76.    
  77.     bool led = false; //ledi päälle kun robotti saa IR arvon
  78.    
  79.     while (SW1_Read()==1)
  80.     {
  81.         vTaskDelay(200);
  82.     }
  83.    
  84.     vTaskDelay(1000);
  85.    
  86.     for(;;)
  87.     {
  88.         reflectance_digital(&dig); // dig.l3, dig.l2, dig.l1, dig.r1, dig.r2, dig.r3    
  89.         motor_forward_fixed(70,0);
  90.        
  91.         // VIIVAN SEURANTA
  92.      
  93.         if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0)
  94.         {
  95.             linjalla = false;
  96.             // mene suoraan
  97.             motor_forward_fixed(70, 0);
  98.         }
  99.         else if (dig.l3 == 0 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
  100.         {
  101.             linjalla = false;
  102.             // käänny oikea
  103.             turn_right();
  104.         }
  105.         else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 0)
  106.         {
  107.             linjalla = false;
  108.             // käänny vasen
  109.             turn_left();
  110.         }
  111.         else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
  112.         {
  113.             linjalla = false;
  114.             // käänny vahva oikea
  115.             hard_turn_right();
  116.         }
  117.         else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 1 && dig.r3 == 1)
  118.         {
  119.             linjalla = false;
  120.             // käänny vahva vasen
  121.             hard_turn_right();
  122.         }
  123.         else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0)
  124.         {
  125.             linjalla = false;
  126.             // käänny loiva vasen
  127.             mild_turn_left();
  128.         }
  129.         else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
  130.         {
  131.             linjalla = false;
  132.             // käänny loiva oikea
  133.             mild_turn_right();
  134.         }
  135.         else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1)
  136.         {
  137.             // kaikki on ykkösiä
  138.             if (linjalaskuri == 0)
  139.             {
  140.                 motor_forward(0, 0);
  141.                 IR_wait();
  142.                 led = !led;
  143.                 linjalla = true;
  144.                 linjalaskuri++;
  145.             }
  146.             else if (linjalaskuri == 1)
  147.             {
  148.                 if (!linjalla)
  149.                 {
  150.                     motor_forward_fixed(70, 200);
  151.                     motor_tank_turn_90dg(1, 0);
  152.                     linjalla = true;
  153.                     linjalaskuri++;
  154.                 }
  155.                 //linjalaskuri++;
  156.             }    
  157.             else if (linjalaskuri <= 3)
  158.             {
  159.                 if (!linjalla)
  160.                 {
  161.                     motor_forward_fixed(70, 200);
  162.                     motor_tank_turn_90dg(0, 1);
  163.                     linjalla = true;
  164.                     linjalaskuri++;
  165.                 }
  166.                 //linjalaskuri++;
  167.             }
  168.             else
  169.             {
  170.                 motor_forward(0,0);          
  171.             }
  172.             /*
  173.             if (!linjalla)
  174.             {
  175.                 linjalaskuri++;
  176.                 linjalla = true;
  177.             }
  178.                */
  179.            
  180.         }
  181.         else
  182.         {
  183.             motor_forward_fixed(70,0);
  184.         }
  185.             // lisää iffejä...
  186.      
  187.      
  188.         vTaskDelay(50);
  189.     }
  190. }
RAW Paste Data
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
Top