API tools faq
paste
TungstenVn

Untitled

TungstenVn
Jun 24th, 2019
201
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 8.60 KB | None | 0 0
raw download clone embed print report
  1. //File lấy dữ liệu
  2. //Số đc lấy từ trái sang phảiic
  3. #define motor1 10
  4. #define motor2 11
  5. const int trig = 8;     // chân trig của HC-SR04
  6. const int echo = 7;     // chân echo của HC-SR04
  7.  
  8.  
  9. const int qtro[7] = {A0,A1,A2,A3,A4,A5,A6};
  10. int ktraqtro[7] = {0,0,0,0,0,0,0};
  11.  
  12. int maxspeed = 120;
  13. bool isRun = false;
  14. int timer = 0;
  15. //---------------------------------------------------------------------
  16.  
  17. void setup() {
  18.    pinMode(trig,OUTPUT);   // chân trig sẽ phát tín hiệu
  19.    pinMode(echo,INPUT);    // chân echo sẽ nhận tín hiệu
  20.  
  21.   Serial.begin(19200);
  22.   for(int i = 0; i<=6;i++){
  23.    pinMode(qtro[i], INPUT);
  24.   }
  25. }
  26. //--------------------------------------------------------------------
  27.  
  28. void loop() {
  29.   ktravatcan();
  30.   //laydulieu();
  31. }
  32. //--------------------------------------------------------------
  33. void ktravatcan(){
  34.   unsigned long duration; // biến đo thời gian
  35.   int distance;           // biến lưu khoảng cách
  36.  
  37.   digitalWrite(trig,0);   // tắt chân trig
  38.   delayMicroseconds(2);
  39.   digitalWrite(trig,1);   // phát xung từ chân trig
  40.   delayMicroseconds(5);   // xung có độ dài 5 microSeconds
  41.   digitalWrite(trig,0);   // tắt chân trig
  42.   duration = pulseIn(echo,HIGH);
  43.   distance = int(duration/2/29.412);
  44.   if(distance < 20){
  45.      analogWrite(motor1,0);
  46.      analogWrite(motor2,0);
  47.      isRun = false;
  48.   }else{
  49.     laydulieu();
  50.   }
  51. }
  52.  
  53. //-------------------------------------------------------------------
  54.  
  55.  
  56. int pid;
  57. void LinePIDFilter() { //PID control in line tracking mode
  58.   static double kP = 1, kI = 0, kD = 0;
  59.   static double gain = 100;
  60.   static double prev_error = 0, prev_I = 0;
  61.   double p = 0, i = 0, d = 0, pid_value;
  62.  
  63.   p = xulyloi();
  64.   i = p + prev_I;
  65.   d = p - prev_error;
  66.  
  67.   pid_value = kP * p + kI * i + kD * d;
  68.   pid = pid_value;
  69.   prev_I = i;
  70.   prev_error = p;
  71.   pid_value *= gain/100;
  72.  
  73.   float ratio=0;
  74.   float x = 255+pid;
  75.   float y = 255-pid;
  76.  
  77.   if(x > 255 || y > 255){
  78.       if(x<y){
  79.         ratio = 255/y;
  80.       }else{
  81.         ratio = 255/x;
  82.       }
  83.       x *= ratio;
  84.       y *= ratio;
  85.   }
  86.  
  87.   if(!isRun){
  88.     isRun = true;
  89.     timer = millis();
  90.   }  
  91.  
  92.   if(millis() - timer <= 1000){
  93.     maxspeed = map(millis() - timer,0,1000,170,255);
  94.   }
  95.   constrain(maxspeed,0,255)
  96.   analogWrite(motor1,maxspeed-(int)x);
  97.   analogWrite(motor2,maxspeed-(int)y);
  98. }
  99.  
  100. //----------Ktra cac kieu-------------------------------------------------------
  101. void digital(){
  102.   for(int i = 0; i<7;i++){
  103.    Serial.println(ktraqtro[i]);
  104.   }
  105.    Serial.println("=======");
  106. }
  107.  
  108. void analog(){
  109.   for(int i = 0; i<7;i++){
  110.    Serial.println(analogRead(qtro[i]);
  111.   }
  112.    Serial.println("=======");
  113. }
  114. void pidinfo(x,y,pid){
  115.   Serial.print("X: ");
  116.   Serial.println(x);
  117.   Serial.print("Y: ");
  118.   Serial.println(y);
  119.   Serial.print("Pid: ");
  120.   Serial.println(pid);
  121. }
  122. //-------------------------------------------------------------------
  123.  
  124.  
  125. void laydulieu(){
  126.  
  127.   //clear ktraqtro
  128.   for(int i = 0; i<7;i++){
  129.    ktraqtro[i] = 0;
  130.   }
  131.  
  132.    
  133.   //Lấy max,min
  134.   int max = analogRead(qtro[0]);
  135.   int min = analogRead(qtro[0]);
  136.  
  137.   for(int i = 0; i<=6;i++){
  138.    if(analogRead(qtro[i]) > max){
  139.     max = analogRead(qtro[i]);
  140.    }
  141.    else if(analogRead(qtro[i]) < min){
  142.     min = analogRead(qtro[i]);
  143.    }
  144.   }
  145.  
  146.   //Lấy trb rồi gán digital từng cái
  147.   int trungbinh = (min + max) /2;
  148.  
  149.   for(int i = 0; i<=6;i++){
  150.    if(analogRead(qtro[i]) > trungbinh){
  151.     ktraqtro[i] = 0;
  152.    }else{
  153.     ktraqtro[i] = 1;
  154.    };
  155.   }
  156.  
  157.   ktraqtro[6]=0;
  158.   //Chuyển động
  159.   LinePIDFilter();
  160.  
  161. }
  162.  
  163.  
  164. //---------------------------------------------------------------------------------
  165.  
  166.  
  167. bool Line[7] = {false};
  168. int soluongqtrophiatrenline;
  169. int xulyloi(){
  170.  //Kiểm tra số quang trở phía trên line
  171.     for(int i = 0; i<7;i++){
  172.    Line[i] = false;
  173.   }
  174.     soluongqtrophiatrenline  =0;
  175.  for(int i = 0; i<7;i++){
  176.   if(ktraqtro[i] == 1){
  177.    soluongqtrophiatrenline += 1;
  178.   }
  179.  }
  180.  //Chuyển từ 0001000 sang false,true
  181.  for(int i = 0; i<7;i++){
  182.   if(ktraqtro[i] == 1){
  183.     Line[i] = true;
  184.   }
  185.  }
  186.  
  187.  //Xác định lỗi
  188.  static int prev_error = 0;
  189.  switch(soluongqtrophiatrenline){
  190.     case 0: {      
  191.         if (prev_error == 0) {
  192.           return 0;
  193.         } else if (prev_error == 6 || prev_error == 7) {
  194.           prev_error = 7;
  195.           return 7;
  196.         } else if (prev_error == -6 || prev_error == -7) {
  197.           prev_error = -7;
  198.           return -7;
  199.         }
  200.         else return prev_error;
  201.         break;
  202.       }
  203.     case 1: {
  204.         for (int i = 0; i < 7; i++){
  205.           if (Line[i]) {
  206.             switch (i) {
  207.               case 0: {
  208.                   prev_error = -6;
  209.                   return -6;
  210.                   break;
  211.                 }
  212.               case 1: {
  213.                   prev_error = -4;
  214.                   return -4;
  215.                   break;
  216.                 }
  217.               case 2: {
  218.                   prev_error = -2;
  219.                   return -2;
  220.                   break;
  221.                 }
  222.               case 3: {
  223.                   prev_error = 0;
  224.                   return 0;
  225.                   break;
  226.                 }
  227.               case 4: {
  228.                   prev_error = 2;
  229.                   return 2;
  230.                   break;
  231.                 }
  232.               case 5: {
  233.                   prev_error = 4;
  234.                   return 4;
  235.                   break;
  236.                 }
  237.               case 6: {
  238.                   prev_error = 6;
  239.                   return 6;
  240.                   break;
  241.                 }
  242.               default: {
  243.                   return prev_error;
  244.                   break;
  245.                 }
  246.             }
  247.             break;
  248.           }
  249.         }
  250.         break;
  251.       }
  252.     case 2: {
  253.         if (Line[0] && Line[1]) {
  254.           prev_error = -5;
  255.           return -5;
  256.         } else if (Line[1] && Line[3]) {
  257.           prev_error = -7;
  258.           return -7;
  259.         } else if (Line[1] && Line[2]) {
  260.           prev_error = -3;
  261.           return -3;
  262.         } else if (Line[2] && Line[3]) {
  263.           prev_error = -1;
  264.           return -1;
  265.         } else if (Line[3] && Line[4]) {
  266.           prev_error = 1;
  267.           return 1;
  268.         } else if (Line[4] && Line[5]) {
  269.           prev_error = 3;
  270.           return 3;
  271.         } else if (Line[5] && Line[3]) {
  272.           prev_error = 7;
  273.           return 7;
  274.         } else if (Line[5] && Line[6]) {
  275.           prev_error = 5;
  276.           return 5;
  277.         } else return prev_error;
  278.         break;
  279.       }
  280.     case 3: {
  281.         if (Line[1] && Line[2] && Line[3]) {
  282.           switch (-1) {
  283.             case 0: {
  284.                 prev_error = 0;
  285.                 return 0;
  286.               }
  287.             case -1: case 1: {
  288.                 prev_error = -4;
  289.                 return -4;
  290.               }
  291.           }
  292.         } else if (Line[0] && Line[1] && Line[2]) {
  293.           prev_error = -6;
  294.           return -6;
  295.         } else if (Line[3] && Line[4] && Line[5]) {
  296.           switch (1) {
  297.             case 0: {
  298.                 prev_error = 0;
  299.                 return 0;
  300.               }
  301.             case 1: case -1: {
  302.                 prev_error = 4;
  303.                 return 4;
  304.               }
  305.           }
  306.         } else if (Line[4] && Line[5] && Line[6]) {
  307.           prev_error = 6;
  308.           return 6;
  309.         } else if (Line[2] && Line[3] && Line[4]) {
  310.           prev_error = 0;
  311.           return 0;
  312.         } else return prev_error;
  313.         break;
  314.       }
  315.     case 4: {
  316.         if (Line[0] && Line[1] && Line[2] && Line[3]) {    
  317.                 prev_error = -8;
  318.                 return -8;        
  319.         }    
  320.         else if (Line[6] && Line[5] && Line[4] && Line[3]) {        
  321.                 prev_error = 8;
  322.                 return 8;          
  323.         }
  324.         else {
  325.           return prev_error;
  326.         }
  327.         break;
  328.       }
  329.     case 5: {
  330.         if (Line[0] && Line[1] && Line[2] && Line[3] && Line[4]) {
  331.           prev_error = -2;
  332.           return -2;
  333.         } else if (Line[1] && Line[2] && Line[3] && Line[4] && Line[5]) {
  334.           prev_error = 0;
  335.           return 0;
  336.         } else if (Line[2] && Line[3] && Line[4] && Line[5] && Line[6]) {
  337.           prev_error = 2;
  338.           return 2;
  339.         }
  340.       }
  341.     case 6: {
  342.       if (Line[0] && Line[1] && Line[2] && Line[3] && Line[4] && Line[5]) {
  343.         prev_error = -1;
  344.         return -1;
  345.       } else if (Line[1] && Line[2] && Line[3] && Line[4] && Line[5] && Line[6]) {
  346.         prev_error = 1;
  347.         return 1;
  348.       }
  349.     }
  350.     case 7: {
  351.       prev_error = 0;
  352.     }
  353.  }
  354. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!