kennyho-rass

aimy

Sep 29th, 2019 (edited)
188
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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. #define BMP280_ADRESA (0x76)                  // nastavení adresy senzoru tlaku
  7. #include "HCPCA9685.h"                        // knihovna pro
  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 leduv2 = A2;
  21. int ledoci = A3;
  22. int leduv1 = A1;
  23. int laser = 4;
  24. int ledpasek = 3;
  25. int svetlo1 = 5;
  26. int svetlo2 = 6;
  27. int pTrig = 7;                              // pin pro připojení Trig z modulu HC-SR04
  28. int pEcho = 8;                              // pin pro připojení Echo z modulu HC-SR04
  29. int svetlo = 9;
  30. int led = 13;
  31. long odezva, vzdalenost;                    // inicializace proměnných, do kterých se uloží data
  32.  
  33. void kroky() {               //  Q
  34.   HCPCA9685.Servo(10, 150);         delay(500);
  35.   HCPCA9685.Servo(1, 300);          delay(500);
  36.   HCPCA9685.Servo(8, 190);          delay(500);
  37.   HCPCA9685.Servo(4, 180);          delay(500);
  38.   HCPCA9685.Servo(8, 170);          delay(500);
  39.   HCPCA9685.Servo(10, 180);         delay(500);
  40.   HCPCA9685.Servo(1, 20);           delay(500);
  41.   HCPCA9685.Servo(9, 200);          delay(500);
  42.   HCPCA9685.Servo(10, 150);         delay(500);
  43.   HCPCA9685.Servo(6, 140);          delay(500);
  44.   HCPCA9685.Servo(10, 190);         delay(500);
  45.   HCPCA9685.Servo(8, 110);          delay(500);
  46.   HCPCA9685.Servo(11, 170);         delay(500);
  47.   HCPCA9685.Servo(9, 140);          delay(500);
  48.   HCPCA9685.Servo(5, 130);          delay(500);
  49.   HCPCA9685.Servo(9, 220);          delay(500);
  50.   HCPCA9685.Servo(10, 170);         delay(500);
  51.   HCPCA9685.Servo(11, 120);         delay(500);
  52.   HCPCA9685.Servo(10, 210);         delay(500);
  53.   HCPCA9685.Servo(9, 170);          delay(500);
  54.   HCPCA9685.Servo(7, 210);          delay(500);
  55.   HCPCA9685.Servo(1, 340);          delay(500);
  56.   HCPCA9685.Servo(11, 110);         delay(500);
  57.   HCPCA9685.Servo(9, 150);          delay(500);
  58.   HCPCA9685.Servo(11, 190);         delay(500);
  59.   HCPCA9685.Servo(7, 200);          delay(500);
  60.   HCPCA9685.Servo(11, 140);         delay(500);
  61.   HCPCA9685.Servo(9, 200);          delay(500);
  62.   HCPCA9685.Servo(6, 90);           delay(500);
  63.   HCPCA9685.Servo(4, 250);          delay(500);
  64.   HCPCA9685.Servo(7, 270);          delay(500);
  65.   HCPCA9685.Servo(5, 80);           delay(500);
  66.   HCPCA9685.Servo(6, 90);           delay(500);
  67.   HCPCA9685.Servo(10, 170);          delay(500);
  68.  
  69.   HCPCA9685.Servo(4, 240);
  70.   HCPCA9685.Servo(5, 80);           delay(500);
  71.   HCPCA9685.Servo(6, 90);
  72.   HCPCA9685.Servo(7, 250);          delay(500);
  73.   HCPCA9685.Servo(8, 140);
  74.   HCPCA9685.Servo(9, 180);          delay(500);
  75.   HCPCA9685.Servo(10, 170);
  76.   HCPCA9685.Servo(11, 170);         delay(500);
  77.   HCPCA9685.Servo(1, 340);          delay(100);
  78. }
  79. void krok() {                //  K
  80.   HCPCA9685.Servo(8, 200);
  81.   HCPCA9685.Servo(10, 110);    delay(250);
  82.   HCPCA9685.Servo(4, 200);
  83.   HCPCA9685.Servo(6, 120);     delay(300);
  84.   HCPCA9685.Servo(8, 150);
  85.   HCPCA9685.Servo(10, 170);    delay(300);
  86.   HCPCA9685.Servo(9, 130);
  87.   HCPCA9685.Servo(11, 210);    delay(250);
  88.   HCPCA9685.Servo(5, 130);
  89.   HCPCA9685.Servo(7, 200);     delay(300);
  90.   HCPCA9685.Servo(9, 150);
  91.   HCPCA9685.Servo(11, 160);    delay(200);
  92.   HCPCA9685.Servo(10, 170);
  93.   HCPCA9685.Servo(11, 160);    delay(300);
  94.   HCPCA9685.Servo(8, 140);
  95.   HCPCA9685.Servo(9, 180);     delay(300);
  96.   HCPCA9685.Servo(6, 90);
  97.   HCPCA9685.Servo(7, 250);     delay(300);
  98.   HCPCA9685.Servo(4, 240);
  99.   HCPCA9685.Servo(5, 80);      delay(300);
  100.   HCPCA9685.Servo(1, 200);     delay(100);
  101. }
  102. void postoj() {              //  P
  103.   HCPCA9685.Servo(10, 170);
  104.   HCPCA9685.Servo(11, 160);         delay(500);
  105.   HCPCA9685.Servo(8, 140);
  106.   HCPCA9685.Servo(9, 180);          delay(500);
  107.   HCPCA9685.Servo(6, 90);
  108.   HCPCA9685.Servo(7, 250);          delay(500);
  109.   HCPCA9685.Servo(4, 240);
  110.   HCPCA9685.Servo(5, 80);           delay(500);
  111.   HCPCA9685.Servo(1, 200);          delay(100);
  112. }
  113. void postoj2() {             //  O
  114.   HCPCA9685.Servo(1, 200);        //  delay(200);
  115.   HCPCA9685.Servo(4, 240);
  116.   HCPCA9685.Servo(5, 80);           delay(200);
  117.   HCPCA9685.Servo(6, 90);
  118.   HCPCA9685.Servo(7, 250);          delay(200);
  119.   HCPCA9685.Servo(8, 140);
  120.   HCPCA9685.Servo(9, 180);          delay(200);
  121.   HCPCA9685.Servo(10, 170);
  122.   HCPCA9685.Servo(11, 160);         delay(200);
  123.   HCPCA9685.Servo(1, 350);          delay(100);
  124.   HCPCA9685.Servo(4, 200);
  125.   HCPCA9685.Servo(5, 120);          delay(200);
  126.   HCPCA9685.Servo(6, 120);
  127.   HCPCA9685.Servo(7, 210);          delay(200);
  128.   HCPCA9685.Servo(8, 60);
  129.   HCPCA9685.Servo(9, 270);          delay(200);
  130.   HCPCA9685.Servo(10, 240);
  131.   HCPCA9685.Servo(11, 80);          delay(200);
  132. }
  133. void vyskok()  {             //  E
  134.   HCPCA9685.Servo(4, 200);
  135.   HCPCA9685.Servo(5, 120);          delay(200);
  136.   HCPCA9685.Servo(6, 120);
  137.   HCPCA9685.Servo(7, 210);          delay(200);
  138.   HCPCA9685.Servo(8, 60);
  139.   HCPCA9685.Servo(9, 270);          delay(200);
  140.   HCPCA9685.Servo(10, 240);
  141.   HCPCA9685.Servo(11, 90);          delay(200);
  142.   HCPCA9685.Servo(11, 80);
  143.  
  144. }
  145. void lehni() {              //  L
  146.   HCPCA9685.Servo(1, 340);           delay(100);
  147.   HCPCA9685.Servo(10, 130);
  148.   HCPCA9685.Servo(11, 200);         delay(500);
  149.   HCPCA9685.Servo(9, 80);
  150.   HCPCA9685.Servo(8, 250);          delay(500);
  151.   HCPCA9685.Servo(10, 80);
  152.   HCPCA9685.Servo(11, 250);         delay(500);
  153.   HCPCA9685.Servo(10, 40);
  154.   HCPCA9685.Servo(11, 290);         delay(500);
  155.   HCPCA9685.Servo(6, 50);
  156.   HCPCA9685.Servo(7, 290);          delay(500);
  157.   HCPCA9685.Servo(9, 60);
  158.   HCPCA9685.Servo(8, 270);          delay(500);
  159.   HCPCA9685.Servo(4, 290);
  160.   HCPCA9685.Servo(5, 50);           delay(500);
  161. }
  162. void sedni() {               //  S
  163.   HCPCA9685.Servo(1, 340);           delay(100);
  164.   HCPCA9685.Servo(10, 20);
  165.   HCPCA9685.Servo(11, 300);         delay(500);
  166.   HCPCA9685.Servo(6, 10);
  167.   HCPCA9685.Servo(7, 330);          delay(500);
  168.   HCPCA9685.Servo(4, 170);
  169.   HCPCA9685.Servo(5, 140);          delay(500);
  170.   HCPCA9685.Servo(8, 130);
  171.   HCPCA9685.Servo(9, 190);          delay(500);
  172. }
  173. void cupnout() {            //  R
  174.   HCPCA9685.Servo(1, 340);           delay(100);
  175.   HCPCA9685.Servo(8, 160);
  176.   HCPCA9685.Servo(9, 180);          delay(500);
  177.   HCPCA9685.Servo(10, 160);
  178.   HCPCA9685.Servo(11, 170);         delay(500);
  179.   HCPCA9685.Servo(4, 160);
  180.   HCPCA9685.Servo(5, 160);          delay(500);
  181.   HCPCA9685.Servo(6, 160);
  182.   HCPCA9685.Servo(7, 160);          delay(500);
  183. }
  184. void vrteni() {             //  V
  185.   HCPCA9685.Servo(1, 80);           delay(100);
  186.   HCPCA9685.Servo(1, 280);          delay(100);
  187.   HCPCA9685.Servo(1, 80);           delay(100);
  188.   HCPCA9685.Servo(1, 280);          delay(100);
  189.   HCPCA9685.Servo(1, 80);           delay(100);
  190.   HCPCA9685.Servo(1, 280);          delay(100);
  191.   HCPCA9685.Servo(1, 80);           delay(100);
  192.   HCPCA9685.Servo(1, 280);          delay(100);
  193.   HCPCA9685.Servo(1, 340);           delay(100);
  194. }
  195. void mavani() {             //  M
  196.   HCPCA9685.Servo(10, 150);         delay(200);
  197.   HCPCA9685.Servo(8, 200);          delay(200);
  198.   HCPCA9685.Servo(4, 160);          delay(200);
  199.   HCPCA9685.Servo(8, 160);          delay(200);
  200.   HCPCA9685.Servo(8, 200);          delay(200);
  201.   HCPCA9685.Servo(8, 160);          delay(200);
  202.   HCPCA9685.Servo(8, 200);          delay(200);
  203.   HCPCA9685.Servo(4, 240);          delay(200);
  204.   HCPCA9685.Servo(8, 140);          delay(200);
  205.   HCPCA9685.Servo(10, 170);         delay(200);
  206. }
  207. void blikani() {            //  B
  208.   digitalWrite(led, HIGH);          delay (70);
  209.   digitalWrite(led, LOW);           delay (70);
  210.   digitalWrite(led, HIGH);          delay (70);
  211.   digitalWrite(led, LOW);           delay (70);
  212.   digitalWrite(led, HIGH);          delay (70);
  213.   digitalWrite(led, LOW);           delay (70);
  214.   digitalWrite(led, HIGH);          delay (70);
  215.   digitalWrite(led, LOW);           delay (70);
  216.   digitalWrite(led, HIGH);          delay (70);
  217.   digitalWrite(led, LOW);           delay (70);
  218. }
  219. void cas() {                //  A
  220.   Serial.print("Cas od spusteni Rasse: ");
  221.   Serial.print(millis() / 1000);
  222.   Serial.println(" vterin.");
  223. }
  224.  
  225.  
  226. void setup() {
  227.   Wire.begin();
  228.   Serial.begin(115200);
  229.   Serial.println(" sestavuji projekt ");
  230.   HCPCA9685.Init(SERVO_MODE);              // prepnuti pca9685 do modu-SERVO
  231.   HCPCA9685.Sleep(false);                  // vypnout spanek PCA9685
  232.   Serial.println(" nastavuji Servo shield ");
  233.   lehni();
  234.   display.begin();
  235.   Serial.println(" nastavuji Oled ");
  236.   pinMode(laser, OUTPUT);
  237.   digitalWrite(laser, HIGH);
  238.   display.clearDisplay();                   // vycisteni displeje
  239.   Serial.println(" nastavuji vystupni piny");
  240.   pinMode(led, OUTPUT);
  241.   pinMode(leduv2, OUTPUT);
  242.   pinMode(ledoci, OUTPUT);
  243.   pinMode(leduv1, OUTPUT);
  244.   pinMode(pTrig, OUTPUT);                   // nastaveni Trig-pinu jako vystup
  245.   pinMode(pEcho, INPUT);                    // nastaveni Echo-pinu jako vstup
  246.   pinMode(ledpasek, OUTPUT);                // nastaveni pinu pro ledpasek jako vystup
  247.   pinMode(svetlo, OUTPUT);                  // nastaveni LED-pinu jako vystup
  248.   pinMode(svetlo1, OUTPUT);
  249.   pinMode(svetlo2, OUTPUT);
  250.   digitalWrite(leduv2, HIGH); delay(300);
  251.   digitalWrite(ledoci, HIGH); delay(300);
  252.   digitalWrite(leduv1, HIGH); delay(400);
  253.   digitalWrite(ledoci, LOW);
  254.   digitalWrite(leduv2, LOW);
  255.   digitalWrite(leduv1, LOW);
  256.   Serial.println(" nastavuji vstupni piny");
  257.   mpu6050.begin();                         // zapnuti komunikace s GYROSKOPEM
  258.   mpu6050.calcGyroOffsets(true);           // kalibrace GYRA
  259.   display.display();
  260.   Serial.println(" vse nastaveno. .. ");
  261.   Serial.println(" Pripraven : ");
  262.   digitalWrite(laser, LOW);
  263.   Serial.println(" Zadej povel ");
  264. }
  265.  
  266.  
  267. void loop() {
  268.   if (Serial.available() > 3)   {                 // pokud je na seriove lince buffer
  269.     servo =  Serial.readStringUntil('.').toInt(); // nacteni dat pred teckou do promenne
  270.     uhel =  Serial.readStringUntil('\n').toInt(); // nacteni dat za teckou s ukoncenim do promenne
  271.     Serial.print(servo);
  272.     Serial.print(" . ");
  273.     Serial.println(uhel);
  274.     HCPCA9685.Servo(servo, uhel);                 // provede nastaveni podle promennych
  275.   }                                               //   konec cteni
  276.   int x = Serial.available();
  277.   if ( x == 2 )
  278.   {
  279.     char pismeno = Serial.read();
  280.     Serial.println(pismeno);
  281.     switch (pismeno)
  282.     {
  283.       case 'z':
  284.         digitalWrite(leduv2, HIGH);
  285.         digitalWrite(leduv1, HIGH);
  286.         break;
  287.       case 'x':
  288.         digitalWrite(leduv2, LOW);
  289.         digitalWrite(leduv1, LOW);
  290.         break;
  291.       case 'c':
  292.         digitalWrite(ledoci, HIGH);
  293.         break;
  294.       case 'd':
  295.         digitalWrite(ledoci, LOW);
  296.         break;
  297.       case 'f':
  298.         digitalWrite(leduv1, HIGH);
  299.         break;
  300.       case 'g':
  301.         digitalWrite(leduv1, LOW);
  302.         break;
  303.       case 'j':
  304.         digitalWrite(laser, HIGH);      // zapne laser
  305.         break;
  306.       case 'n':
  307.         digitalWrite(laser, LOW);       //vypne laser
  308.         break;
  309.       case 't':
  310.         digitalWrite(ledpasek, HIGH);
  311.         break;
  312.       case 'y':
  313.         digitalWrite(ledpasek, LOW);
  314.         break;
  315.       case 'q':                        // pohyb pro KROKY
  316.         kroky();
  317.         break;
  318.       case 'o':                        // Postoj2
  319.         postoj2();
  320.         break;
  321.       case 'b':                        // LED13  BLIKANI
  322.         blikani();
  323.         break;
  324.       case 'v':                        // vrteni ocasem
  325.         vrteni();
  326.         break;
  327.       case 'k':                        // jeden krok
  328.         krok();
  329.         break;
  330.       case 's':                        // sedni
  331.         sedni();
  332.         break;
  333.       case 'p':                        // postoj
  334.         postoj();
  335.         break;
  336.       case 'l':                        // lehni
  337.         lehni();
  338.         break;
  339.       case 'r':                        // povel  CUPNOUT NA KOLENA
  340.         cupnout();
  341.         break;
  342.       case 'm':                        // Mavani levou predni packou
  343.         mavani();
  344.         break;
  345.       case 'e':                       // vyskok
  346.         vyskok();
  347.         break;
  348.       case 'a':                        // v případě přejetí znaku 'a' vypíše  čas od spuštění Arduina
  349.         cas();
  350.         break;
  351.       /*    case '\r':         // přesun na začátek řádku - znak CR
  352.          break;
  353.         case '\n':         // odřádkování - znak LF
  354.          break;
  355.       */
  356.       case 10:        // přesun na začátek řádku - znak CR
  357.         break;
  358.       case 13:        // odřádkování - znak LF
  359.         break;
  360.       default:
  361.         Serial.println(" Neznam prikaz ");
  362.     }
  363.   }
  364.   display.clearDisplay();                             // vycisteni displeje
  365.   Adafruit_BMP280 bmp;                                // inicializace senzoru BME z knihovny
  366.   bmp.begin(0x76);                                    // Start the bmp
  367.   float T = bmp.readTemperature();                    // Read temperature in °C
  368.   float P = bmp.readPressure() / 100;                 // Read Pressure in Pa and conversion to hPa
  369.   float A = bmp.readAltitude(1013.26);                // Calthe Altitude, the "1019.66"  in (hPa)level at day in your region
  370.   mpu6050.update();                                   // nacteni novych dat z gyroskopu
  371.   digitalWrite(pTrig, LOW);                           // vypne Trig-pin
  372.   delayMicroseconds(2);                               // nastaví na 2 mikrosekundy výstup na GND (pro jistotu)
  373.   digitalWrite(pTrig, HIGH);                          // zapne Trig-pin
  374.   delayMicroseconds(5);                               // nastaví na 5 mikrosekund výstup rovný napájení
  375.   digitalWrite(pTrig, LOW);                           // vypne Trig-pin
  376.   odezva = pulseIn(pEcho, HIGH);                      // pomocí funkce pulseIn získáme následně délku pulzu v mikrosekundách
  377.   vzdalenost = odezva / 58.31;                        // přepočet získaného času na vzdálenost v cm
  378.   if (vzdalenost > 10) digitalWrite(svetlo2, LOW);
  379.   if (vzdalenost < 10) digitalWrite(svetlo2, HIGH);   // pokud je vzdalenost min nez 10cm rozsviti led2  cervena
  380.   if (vzdalenost > 15) digitalWrite(svetlo1, LOW);
  381.   if (vzdalenost < 15) digitalWrite(svetlo1, HIGH);   // pokud je vzdalenost min nez 15cm rozsviti led1  zluta
  382.   if (vzdalenost > 20) digitalWrite(svetlo, LOW);
  383.   if (vzdalenost < 20) digitalWrite(svetlo, HIGH);    // pokud je vzdalenost min nez 20cm rozsviti led   zelena
  384.   display.setTextColor(WHITE);                        // nastavi OLED na bilou barvu
  385.   display.setTextSize(2);                             // OLED display nastaveni velikosti pisma
  386.   display.setCursor(0, 0);                            // OLED display nastaveni pozice
  387.   display.print(T, 1);                                // OLED display vypis teploty
  388.   display.setTextSize(1);
  389.   display.setCursor(55, 0);
  390.   display.print(P, 0);
  391.   display.setCursor(100, 0);
  392.   display.print(A, 0);
  393.   display.setCursor(122, 0);
  394.   display.print("m");
  395.   display.setCursor(55, 8);
  396.   display.print(mpu6050.getTemp(), 1);
  397.   display.setCursor(90, 8);
  398.   display.print(vzdalenost);
  399.   display.setCursor(115, 10);
  400.   display.print("cm");
  401.   display.setCursor(0, 16);
  402.   display.print(mpu6050.getGyroX(), 1);
  403.   display.setCursor(40, 16);
  404.   display.print(mpu6050.getGyroY(), 1);
  405.   display.setCursor(90, 16);
  406.   display.print(mpu6050.getGyroZ(), 1);
  407.   display.setCursor(0, 24);
  408.   display.print(mpu6050.getAngleX(), 1);
  409.   display.setCursor(40, 24);
  410.   display.print(mpu6050.getAngleY(), 1);
  411.   display.setCursor(90, 24);
  412.   display.print(mpu6050.getAngleZ(), 1);
  413.   display.display();
  414.   Serial.print("\tgX "); Serial.print(mpu6050.getGyroX(), 1);
  415.   Serial.print("\tgY "); Serial.print(mpu6050.getGyroY(), 1);
  416.   Serial.print("\tgZ "); Serial.print(mpu6050.getGyroZ(), 1);
  417.   Serial.print("\tAX "); Serial.print(mpu6050.getAngleX(), 1);
  418.   Serial.print("\tAY "); Serial.print(mpu6050.getAngleY(), 1);
  419.   Serial.print("\tAZ "); Serial.print(mpu6050.getAngleZ(), 1);
  420.   Serial.print(" : ");
  421.   Serial.print(T, 2); Serial.print(" °C, ");
  422.   Serial.print(P, 2); Serial.print(" hPa, ");
  423.   Serial.print(A, 0); Serial.print(" mnm, ");
  424.   Serial.print(mpu6050.getTemp(), 1); Serial.print("-°C mpu, ");
  425.   Serial.print(vzdalenost); Serial.println(" cm, ");
  426.  
  427.  
  428.   delay(10);
  429. }
RAW Paste Data Copied