SHARE
TWEET

Untitled

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