API tools faq
paste
kennyho-rass

rass_serva

kennyho-rass
May 3rd, 2020
575
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 13.70 KB | None | 0 0
raw download clone embed print report
  1. #include <SoftwareSerial.h>                   // knihovna pro seriovou linku
  2. #include <Wire.h>                             // knihovna pro WIRE komunikaci
  3. #include <Adafruit_SSD1306.h>                 // knihovna pro OLED
  4. #include <MPU6050_tockn.h>                    // knihovna pro GYRO
  5. #include <Adafruit_BMP280.h>                  // knihovna pro BMP280
  6. #include "HCPCA9685.h"                        // knihovna pro
  7. #define BMP280_ADRESA (0x76)                  // nastavení adresy senzoru tlaku
  8. #define I2CAdd 0x40                           // nastaveni adresy SERVO SHIELDU PCA9685
  9. HCPCA9685 HCPCA9685(I2CAdd);                  // nastaveni adresy pca9685
  10. #define SERVO_TRIM_MIN 10
  11. #define SERVO_TRIM_MAX 330
  12. #define MPU6050_ADRESA (0x68)                 // nastavení adresy GYRA
  13. MPU6050 mpu6050(Wire);
  14. #define SCREEN_WIDTH 128                      // OLED display delka, v pixelech
  15. #define SCREEN_HEIGHT 32                      // OLED display vyska, v pixelech
  16. #define OLED_RESET    -1                      // Reset pin # (or -1 if sharing Arduino reset pin)
  17. Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
  18. int servo;                                   // promenna pro servo
  19. int uhel;                                    // promenna pro uhel
  20. int led1 = A2;
  21. int led2 = A3;
  22. int led3 = A1;
  23. int ledpasek = 3;
  24. int svetlo1 = 5;
  25. int svetlo2 = 6;
  26. int pTrig = 7;                              // pin pro připojení Trig z modulu HC-SR04
  27. int pEcho = 8;                              // pin pro připojení Echo z modulu HC-SR04
  28. int svetlo = 9;
  29. int led = 13;
  30. long odezva, vzdalenost;                    // inicializace proměnných, do kterých se uloží data
  31.  
  32. void setup() {
  33.   Wire.begin();
  34.   Serial.begin(115200);
  35.   display.begin();
  36.   display.clearDisplay();                   // vycisteni displeje
  37.   pinMode(led, OUTPUT);
  38.   pinMode(led1, OUTPUT);
  39.   pinMode(led2, OUTPUT);
  40.   pinMode(led3, OUTPUT);
  41.   pinMode(pTrig, OUTPUT);                   // nastaveni Trig-pinu jako vystup
  42.   pinMode(pEcho, INPUT);                    // nastaveni Echo-pinu jako vstup
  43.   pinMode(ledpasek, OUTPUT);                // nastaveni pinu pro ledpasek jako vystup
  44.   pinMode(svetlo, OUTPUT);                  // nastaveni LED-pinu jako vystup
  45.   pinMode(svetlo1, OUTPUT);
  46.   pinMode(svetlo2, OUTPUT);
  47.   digitalWrite(led1, LOW);
  48.   digitalWrite(led2, LOW);
  49.   digitalWrite(led3, LOW);
  50.   HCPCA9685.Init(SERVO_MODE);              // prepnuti pca9685 do modu-SERVO
  51.   HCPCA9685.Sleep(false);                  // vypnout spanek PCA9685
  52.   mpu6050.begin();                         // zapnuti komunikace s GYROSKOPEM
  53.   mpu6050.calcGyroOffsets(true);           // kalibrace GYRA
  54.   display.display();
  55.   Serial.println(".");
  56.   Serial.println(" nastaveno ");
  57. }
  58.  
  59. void loop() {
  60.   if (Serial.available() > 3)   {                 // pokud je na seriove lince bt buffer
  61.     servo =  Serial.readStringUntil('.').toInt(); // nacteni dat pred teckou do promenne
  62.     uhel =  Serial.readStringUntil('\n').toInt(); // nacteni dat za teckou s ukoncenim do promenne
  63.     Serial.print(servo);
  64.     Serial.print(" . ");
  65.     Serial.println(uhel);
  66.     HCPCA9685.Servo(servo, uhel);                 // provede nastaveni podle promennych
  67.   }                                               //   konec cteni
  68.   int x = Serial.available();
  69.   if ( x == 2 )
  70.   {
  71.     char pismeno = Serial.read();
  72.     Serial.println(pismeno);
  73.     switch (pismeno)
  74.     {
  75.       case 'z':
  76.         digitalWrite(led1, HIGH);
  77.         digitalWrite(led3, HIGH);
  78.         break;
  79.       case 'x':
  80.         digitalWrite(led1, LOW);
  81.         digitalWrite(led3, LOW);
  82.         break;
  83.       case 'c':
  84.         digitalWrite(led2, HIGH);
  85.         break;
  86.       case 'd':
  87.         digitalWrite(led2, LOW);
  88.         break;
  89.       case 'f':
  90.         digitalWrite(led3, HIGH);
  91.         break;
  92.       case 'g':
  93.         digitalWrite(led3, LOW);
  94.         break;
  95.       case 't':
  96.         digitalWrite(ledpasek, HIGH);
  97.         break;
  98.       case 'y':
  99.         digitalWrite(ledpasek, LOW);
  100.         break;
  101.       case 'q':    {                              // pohyb pro KROKY
  102.           HCPCA9685.Servo(10, 150);         delay(500);
  103.           HCPCA9685.Servo(1, 300);          delay(500);
  104.           HCPCA9685.Servo(8, 190);          delay(500);
  105.           HCPCA9685.Servo(4, 200);          delay(500);
  106.           HCPCA9685.Servo(8, 140);          delay(500);
  107.           HCPCA9685.Servo(10, 180);         delay(500);
  108.           HCPCA9685.Servo(1, 20);           delay(500);
  109.           HCPCA9685.Servo(9, 150);          delay(500);
  110.           HCPCA9685.Servo(5, 150);          delay(500);
  111.           HCPCA9685.Servo(9, 200);          delay(500);
  112.  
  113.           HCPCA9685.Servo(10, 170);         delay(200);
  114.           HCPCA9685.Servo(9, 140);          delay(200);
  115.           HCPCA9685.Servo(5, 120);          delay(200);
  116.           HCPCA9685.Servo(6, 130);          delay(200);
  117.           HCPCA9685.Servo(8, 130);          delay(200);
  118.           HCPCA9685.Servo(4, 250);          delay(200);
  119.           HCPCA9685.Servo(10, 170);         delay(200);
  120.           HCPCA9685.Servo(6, 250);          delay(200);
  121.         }        break;
  122.       case 'b':                        // LED13  BLIKANI
  123.         digitalWrite(led, HIGH);        delay (70);
  124.         digitalWrite(led, LOW);         delay (70);
  125.         digitalWrite(led, HIGH);        delay (70);
  126.         digitalWrite(led, LOW);         delay (70);
  127.         digitalWrite(led, HIGH);        delay (70);
  128.         digitalWrite(led, LOW);         delay (70);
  129.         digitalWrite(led, HIGH);        delay (70);
  130.         digitalWrite(led, LOW);         delay (70);
  131.         digitalWrite(led, HIGH);        delay (70);
  132.         digitalWrite(led, LOW);         delay (70);
  133.         break;
  134.       case 'v':       {                       // vrteni ocasem
  135.           HCPCA9685.Servo(1, 80);           delay(100);
  136.           HCPCA9685.Servo(1, 280);          delay(100);
  137.           HCPCA9685.Servo(1, 80);           delay(100);
  138.           HCPCA9685.Servo(1, 280);          delay(100);
  139.           HCPCA9685.Servo(1, 80);           delay(100);
  140.           HCPCA9685.Servo(1, 280);          delay(100);
  141.           HCPCA9685.Servo(1, 80);           delay(100);
  142.           HCPCA9685.Servo(1, 280);          delay(100);
  143.           HCPCA9685.Servo(1, 170);          delay(100);
  144.         }        break;
  145.       case 'k':        {                          // jeden krok
  146.           HCPCA9685.Servo(8, 200);
  147.           HCPCA9685.Servo(10, 110);          delay(400);
  148.           HCPCA9685.Servo(4, 180);
  149.           HCPCA9685.Servo(6, 150);           delay(200);
  150.           HCPCA9685.Servo(8, 120);
  151.           HCPCA9685.Servo(10, 200);          delay(400);
  152.           HCPCA9685.Servo(4, 250);
  153.           HCPCA9685.Servo(6, 70);            delay(200);
  154.           HCPCA9685.Servo(8, 170);
  155.           HCPCA9685.Servo(10, 150);          delay(200);
  156.         }        break;
  157.       case 's':         {                        // sedni
  158.           HCPCA9685.Servo(1, 20);           delay(100);
  159.           HCPCA9685.Servo(10, 20);
  160.           HCPCA9685.Servo(11, 300);         delay(500);
  161.           HCPCA9685.Servo(6, 10);
  162.           HCPCA9685.Servo(7, 330);          delay(500);
  163.           HCPCA9685.Servo(4, 170);
  164.           HCPCA9685.Servo(5, 140);          delay(500);
  165.           HCPCA9685.Servo(8, 130);
  166.           HCPCA9685.Servo(9, 190);          delay(500);
  167.  
  168.         }        break;
  169.       case 'p':       {                             // postoj
  170.           HCPCA9685.Servo(4, 240);
  171.           HCPCA9685.Servo(5, 80);           delay(500);
  172.           HCPCA9685.Servo(6, 90);
  173.           HCPCA9685.Servo(7, 250);          delay(500);
  174.           HCPCA9685.Servo(8, 140);
  175.           HCPCA9685.Servo(9, 180);          delay(500);
  176.           HCPCA9685.Servo(10, 190);
  177.           HCPCA9685.Servo(11, 140);         delay(500);
  178.           HCPCA9685.Servo(1, 180);          delay(100);
  179.         }        break;
  180.       case 'l':        {                        // povel LEHNI
  181.           HCPCA9685.Servo(1, 20);          delay(100);
  182.           HCPCA9685.Servo(10, 80);
  183.           HCPCA9685.Servo(11, 250);        delay(500);
  184.           HCPCA9685.Servo(9, 80);
  185.           HCPCA9685.Servo(8, 250);         delay(500);
  186.           HCPCA9685.Servo(10, 40);
  187.           HCPCA9685.Servo(11, 290);        delay(500);
  188.           HCPCA9685.Servo(6, 50);
  189.           HCPCA9685.Servo(7, 280);         delay(500);
  190.           HCPCA9685.Servo(9, 60);
  191.           HCPCA9685.Servo(8, 270);         delay(500);
  192.           HCPCA9685.Servo(4, 280);
  193.           HCPCA9685.Servo(5, 50);          delay(500);
  194.         }        break;
  195.       case 'r':         {                       // povel  CUPNOUT NA KOLENA
  196.           HCPCA9685.Servo(1, 20);           delay(100);
  197.           HCPCA9685.Servo(8, 160);          delay(300);
  198.           HCPCA9685.Servo(9, 180);          delay(300);
  199.           HCPCA9685.Servo(10, 160);         delay(300);
  200.           HCPCA9685.Servo(11, 170);         delay(500);
  201.           HCPCA9685.Servo(4, 160);          delay(300);
  202.           HCPCA9685.Servo(5, 170);          delay(300);
  203.           HCPCA9685.Servo(6, 180);          delay(300);
  204.           HCPCA9685.Servo(7, 160);          delay(300);
  205.         }        break;
  206.       case 'm':          {                       // Mavani levou predni packou
  207.           HCPCA9685.Servo(10, 150);         delay(200);
  208.           HCPCA9685.Servo(8, 200);          delay(200);
  209.           HCPCA9685.Servo(4, 160);          delay(200);
  210.           HCPCA9685.Servo(8, 160);          delay(200);
  211.           HCPCA9685.Servo(8, 200);          delay(200);
  212.           HCPCA9685.Servo(8, 160);          delay(200);
  213.           HCPCA9685.Servo(8, 200);          delay(200);
  214.           HCPCA9685.Servo(4, 240);          delay(200);
  215.           HCPCA9685.Servo(8, 160);          delay(200);
  216.           HCPCA9685.Servo(10, 170);         delay(200);
  217.         }        break;
  218.       case 'a':                       // v případě přejetí znaku 'a' vypíše  čas od spuštění Arduina
  219.         Serial.print("Cas od spusteni Rasse: ");
  220.         Serial.print(millis() / 1000);
  221.         Serial.println(" vterin.");
  222.         break;
  223.  
  224.      /*    case '\r':         // přesun na začátek řádku - znak CR
  225.         break;
  226.       case '\n':         // odřádkování - znak LF
  227.         break;
  228.   */
  229.       case 10:        // přesun na začátek řádku - znak CR
  230.         break;
  231.       case 13:        // odřádkování - znak LF
  232.         break;
  233.       default:
  234.         Serial.println(" Neznam prikaz ");
  235.     }
  236.   }
  237.   display.clearDisplay();                             // vycisteni displeje
  238.   Adafruit_BMP280 bmp;                                // inicializace senzoru BME z knihovny
  239.   bmp.begin(0x76);                                    // Start the bmp
  240.   float T = bmp.readTemperature();                    // Read temperature in °C
  241.   float P = bmp.readPressure() / 100;                 // Read Pressure in Pa and conversion to hPa
  242.   float A = bmp.readAltitude(1013.26);                // Calthe Altitude, the "1019.66"  in (hPa)level at day in your region
  243.   mpu6050.update();                                   // nacteni novych dat z gyroskopu
  244.   digitalWrite(pTrig, LOW);                           // vypne Trig-pin
  245.   delayMicroseconds(2);                               // nastaví na 2 mikrosekundy výstup na GND (pro jistotu)
  246.   digitalWrite(pTrig, HIGH);                          // zapne Trig-pin
  247.   delayMicroseconds(5);                               // nastaví na 5 mikrosekund výstup rovný napájení
  248.   digitalWrite(pTrig, LOW);                           // vypne Trig-pin
  249.   odezva = pulseIn(pEcho, HIGH);                      // pomocí funkce pulseIn získáme následně délku pulzu v mikrosekundách
  250.   vzdalenost = odezva / 58.31;                        // přepočet získaného času na vzdálenost v cm
  251.   if (vzdalenost > 10) digitalWrite(svetlo2, LOW);
  252.   if (vzdalenost < 10) digitalWrite(svetlo2, HIGH);   // pokud je vzdalenost min nez 10cm rozsviti led2  cervena
  253.   if (vzdalenost > 15) digitalWrite(svetlo1, LOW);
  254.   if (vzdalenost < 15) digitalWrite(svetlo1, HIGH);   // pokud je vzdalenost min nez 15cm rozsviti led1  zluta
  255.   if (vzdalenost > 20) digitalWrite(svetlo, LOW);
  256.   if (vzdalenost < 20) digitalWrite(svetlo, HIGH);    // pokud je vzdalenost min nez 20cm rozsviti led   zelena
  257.   display.setTextColor(WHITE);                        // nastavi OLED na bilou barvu
  258.   display.setTextSize(2);                             // OLED display nastaveni velikosti pisma
  259.   display.setCursor(0, 0);                            // OLED display nastaveni pozice
  260.   display.print(T, 1);                                // OLED display vypis teploty
  261.   display.setTextSize(1);
  262.   display.setCursor(55, 0);
  263.   display.print(P, 0);
  264.   display.setCursor(100, 0);
  265.   display.print(A, 0);
  266.   display.setCursor(122, 0);
  267.   display.print("m");
  268.   display.setCursor(55, 8);
  269.   display.print(mpu6050.getTemp(), 1);
  270.   display.setCursor(90, 8);
  271.   display.print(vzdalenost);
  272.   display.setCursor(115, 10);
  273.   display.print("cm");
  274.   display.setCursor(0, 16);
  275.   display.print(mpu6050.getGyroX(), 1);
  276.   display.setCursor(40, 16);
  277.   display.print(mpu6050.getGyroY(), 1);
  278.   display.setCursor(90, 16);
  279.   display.print(mpu6050.getGyroZ(), 1);
  280.   display.setCursor(0, 24);
  281.   display.print(mpu6050.getAngleX(), 1);
  282.   display.setCursor(40, 24);
  283.   display.print(mpu6050.getAngleY(), 1);
  284.   display.setCursor(90, 24);
  285.   display.print(mpu6050.getAngleZ(), 1);
  286.   display.display();
  287.   Serial.print("\tgX "); Serial.print(mpu6050.getGyroX(), 1);
  288.   Serial.print("\tgY "); Serial.print(mpu6050.getGyroY(), 1);
  289.   Serial.print("\tgZ "); Serial.print(mpu6050.getGyroZ(), 1);
  290.   Serial.print("\tAX "); Serial.print(mpu6050.getAngleX(), 1);
  291.   Serial.print("\tAY "); Serial.print(mpu6050.getAngleY(), 1);
  292.   Serial.print("\tAZ "); Serial.println(mpu6050.getAngleZ(), 1);
  293. }
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!