SHARE
TWEET

Untitled

a guest Feb 27th, 2020 74 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.  
  36. void turn_left()
  37. {
  38.    
  39.   motor_turn(20, 70, 200);
  40.  
  41. }
  42. void turn_right()
  43. {
  44.    
  45.  motor_turn(70, 20, 200);  
  46.  
  47. }
  48.  
  49. void mild_turn_left()
  50. {
  51.    
  52.   motor_turn(30, 70, 100);
  53.  
  54. }
  55.  
  56. void mild_turn_right()
  57. {
  58.    
  59. motor_turn(70, 30, 100);
  60.  
  61. }
  62.  
  63. void hard_turn_left()
  64. {
  65.    
  66. motor_turn(0, 70, 500);
  67.  
  68. }
  69.  
  70.  
  71. void hard_turn_right()
  72. {
  73.    
  74. motor_turn(70, 0, 500);
  75. }
  76.  
  77.  
  78.  
  79.  void zmain(void)
  80. {
  81.     IR_Start(); //käynnistetään IR sensori
  82.     IR_flush();
  83.    
  84.     reflectance_start(); //Käynnistetään reflectance sensorit
  85.     //struct sensors_ ref;
  86.     struct sensors_ dig;
  87.     reflectance_set_threshold(9000, 9000, 11000, 11000, 9000, 9000);
  88.    
  89.     motor_start(); //käynnistetään moottori
  90.     motor_forward(0, 0); //pysäytetään moottorit
  91.    
  92.     int linjalaskuri = 0;
  93.    
  94.     bool led = false; //ledi päälle kun robotti saa IR arvon
  95.    
  96.     while (SW1_Read()==1)
  97.     {
  98.         vTaskDelay(200);
  99.     }
  100.    
  101.    for(;;)
  102.     {
  103.         reflectance_digital(&dig); // dig.l3, dig.l2, dig.l1, dig.r1, dig.r2, dig.r3    
  104.         motor_forward_fixed(70,0);
  105.        
  106.         // VIIVAN SEURANTA
  107.      
  108.         if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0)
  109.         {
  110.             // mene suoraan
  111.             motor_forward_fixed(70, 0);
  112.         }
  113.         else if (dig.l3 == 0 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
  114.         {
  115.             // käänny oikea
  116.             turn_right();
  117.         }
  118.         else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 0)
  119.         {
  120.             // käänny vasen
  121.             turn_left();
  122.         }
  123.         else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
  124.         {
  125.             // käänny vahva oikea
  126.             hard_turn_right();
  127.         }
  128.         else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 1 && dig.r3 == 1)
  129.         {
  130.             // käänny vahva vasen
  131.             hard_turn_right();
  132.         }
  133.         else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0)
  134.         {
  135.             // käänny loiva vasen
  136.             mild_turn_left();
  137.         }
  138.         else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
  139.         {
  140.             // käänny loiva oikea
  141.             mild_turn_right();
  142.         }
  143.         else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1)
  144.         {
  145.             // kaikki on ykkösiä
  146.             if(linjalaskuri==0)
  147.             {
  148.                 motor_forward(0,0);
  149.                 IR_wait();
  150.                 led=!led;
  151.                 linjalaskuri++;
  152.             }
  153.             else if(linjalaskuri<=4)
  154.             {
  155.                 motor_forward_fixed(100,0);
  156.                 linjalaskuri++;
  157.             }
  158.             else
  159.             {
  160.                 motor_forward(0,0);          
  161.             }
  162.             // odota IR tai linjalaskuri++;
  163.         }
  164.         else
  165.         {
  166.             motor_forward_fixed(70,0);
  167.         }
  168.             // lisää iffejä...
  169.      
  170.      
  171.         vTaskDelay(50);
  172.     }
  173. }    
  174. #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