Prezentace 9, ovladani ramen-

2026-03-12 13:55:38 +01:00
parent 9473b81b21
commit b5fb3c904b
3 changed files with 390 additions and 0 deletions
@@ -0,0 +1,187 @@
+// PROGRAM OVLADANI ROBOTICKEHO RAMENE POMOCI BLUETOOTH APLIKACE LAFVIN
+// VojtaR, brezen 2026
+
+
+#include <Servo.h> // servomotory
+
+
+// ------------------------------------------------------------ //
+
+
+// mapovani vystupnich GPIO pinu
+const uint8_t SERVO_ROTACE = -1; // nutno PWM, napriklad 3
+const uint8_t SERVO_SPODNI = -1; // nutno PWM, napriklad 5
+const uint8_t SERVO_VRCHNI = -1; // nutno PWM, napriklad 6
+const uint8_t SERVO_CELISTI = -1; // nutno PWM, napriklad 9
+
+// dalsi globalni nastaveni
+const uint16_t GLOBALNI_PERIODA_OBNOVOVANI = 0; // ms
+const uint8_t SERVO_KROK_TRVANI = 15; // ms
+const uint8_t SERVO_ROTACE_MIN = 0;
+const uint8_t SERVO_ROTACE_MAX = 180;
+const uint8_t SERVO_ROTACE_KROK = 8;
+const uint8_t SERVO_SPODNI_MIN = 25;
+const uint8_t SERVO_SPODNI_MAX = 180;
+const uint8_t SERVO_SPODNI_KROK = 8;
+const uint8_t SERVO_VRCHNI_MIN = 0;
+const uint8_t SERVO_VRCHNI_MAX = 135;
+const uint8_t SERVO_VRCHNI_KROK = 8;
+const uint8_t SERVO_CELISTI_MIN = 45;
+const uint8_t SERVO_CELISTI_MAX = 120;
+const uint8_t SERVO_CELISTI_KROK = 8;
+const uint8_t SERVO_VYCHOZI_POZICE = 90;
+
+
+// ------------------------------------------------------------ //
+
+
+// promenne periferii
+uint8_t prijaty_bajt = 0;
+String retezec_prijatych = "";
+bool prijato_odradkovani = false;
+bool prijat_start_bit = false;
+uint8_t prijatych_bajtu = 0;
+Servo servo_rotace;
+Servo servo_spodni;
+Servo servo_vrchni;
+Servo servo_celisti;
+int32_t servo_rotace_pozice = 0;
+int32_t servo_spodni_pozice = 0;
+int32_t servo_vrchni_pozice = 0;
+int32_t servo_celisti_pozice = 0;
+uint32_t nejdelsi_krok = 0;
+
+// serva do vychozi pozice
+void servo_reset() {
+    servo_rotace.write(SERVO_VYCHOZI_POZICE);
+    servo_spodni.write(SERVO_VYCHOZI_POZICE);
+    servo_vrchni.write(SERVO_VYCHOZI_POZICE);
+    servo_celisti.write(SERVO_VYCHOZI_POZICE);
+    delay(90*SERVO_KROK_TRVANI); // cekani nez operace probehne (nejhorsi pripad: 90 °)
+}
+
+// vychozi nastaveni servomotoru
+void servo_init() {
+    servo_rotace.attach(SERVO_ROTACE);
+    servo_spodni.attach(SERVO_SPODNI);
+    servo_vrchni.attach(SERVO_VRCHNI);
+    servo_celisti.attach(SERVO_CELISTI);
+}
+
+// ulozeni aktualnich pozic servomotoru
+void servo_pozice_vse() {
+    servo_rotace_pozice = servo_rotace.read();
+    servo_spodni_pozice = servo_spodni.read();
+    servo_vrchni_pozice = servo_vrchni.read();
+    servo_celisti_pozice = servo_celisti.read();
+}
+
+// z globalnich konstant urcit nejdelsi krok napric vsemy servy
+void servo_najit_nejdelsi_krok() {
+    nejdelsi_krok = (SERVO_ROTACE_KROK >= SERVO_SPODNI_KROK) ? SERVO_ROTACE_KROK : SERVO_SPODNI_KROK;
+    nejdelsi_krok = (nejdelsi_krok >= SERVO_VRCHNI_KROK) ? nejdelsi_krok : SERVO_VRCHNI_KROK;
+    nejdelsi_krok = (nejdelsi_krok >= SERVO_CELISTI_KROK) ? nejdelsi_krok : SERVO_CELISTI_KROK;
+}
+
+
+// rotace ramene
+void servo_pohyb_rotace(int8_t smer) {
+    int32_t nova_pozice = servo_rotace_pozice;
+    nova_pozice = (smer > 0) ? nova_pozice + SERVO_ROTACE_KROK : nova_pozice - SERVO_ROTACE_KROK; // posun dle joysticku
+    if(nova_pozice > SERVO_ROTACE_MAX) {nova_pozice = SERVO_ROTACE_MAX;} // horni mez
+    if(nova_pozice < SERVO_ROTACE_MIN) {nova_pozice = SERVO_ROTACE_MIN;} // dolni mez
+    servo_rotace.write(nova_pozice);
+    delay(SERVO_ROTACE_KROK*SERVO_KROK_TRVANI); 
+}
+
+// zdvih spodni casti ramene
+void servo_pohyb_spodni(int8_t smer) {
+    int32_t nova_pozice = servo_spodni_pozice;
+    nova_pozice = (smer > 0) ? nova_pozice + SERVO_SPODNI_KROK : nova_pozice - SERVO_SPODNI_KROK;
+    if(nova_pozice > SERVO_SPODNI_MAX) {nova_pozice = SERVO_SPODNI_MAX;}
+    if(nova_pozice < SERVO_SPODNI_MIN) {nova_pozice = SERVO_SPODNI_MIN;}
+    servo_spodni.write(nova_pozice);
+    delay(SERVO_SPODNI_KROK*SERVO_KROK_TRVANI); 
+}
+
+// zdvih spodni casti ramene
+void servo_pohyb_vrchni(int8_t smer) {
+    int32_t nova_pozice = servo_vrchni_pozice;
+    nova_pozice = (smer > 0) ? nova_pozice + SERVO_VRCHNI_KROK : nova_pozice - SERVO_VRCHNI_KROK;
+    if(nova_pozice > SERVO_VRCHNI_MAX) {nova_pozice = SERVO_VRCHNI_MAX;}
+    if(nova_pozice < SERVO_VRCHNI_MIN) {nova_pozice = SERVO_VRCHNI_MIN;}
+    servo_vrchni.write(nova_pozice);
+    delay(SERVO_VRCHNI_KROK*SERVO_KROK_TRVANI); 
+}
+
+// pohyb celisti ramene
+void servo_pohyb_celisti(int8_t smer) {
+    int32_t nova_pozice = servo_celisti_pozice;
+    nova_pozice = (smer > 0) ? nova_pozice + SERVO_CELISTI_KROK : nova_pozice - SERVO_CELISTI_KROK;
+    if(nova_pozice > SERVO_CELISTI_MAX) {nova_pozice = SERVO_CELISTI_MAX;}
+    if(nova_pozice < SERVO_CELISTI_MIN) {nova_pozice = SERVO_CELISTI_MIN;}
+    servo_celisti.write(nova_pozice);
+    delay(SERVO_CELISTI_KROK*SERVO_KROK_TRVANI); 
+}
+
+
+// ------------------------------------------------------------ //
+
+
+// inicilalizace a nastaveni
+void setup() {
+    servo_init();
+    servo_najit_nejdelsi_krok();
+    servo_reset();
+    Serial.begin(9600); // BT
+}
+
+// hlavni smycka
+void loop() {
+
+    while (Serial.available()) {
+        prijaty_bajt = Serial.read();
+
+        if (prijaty_bajt == '%') {
+          prijatych_bajtu = 0;
+          prijat_start_bit = true;
+        }
+
+        if (prijat_start_bit == true) {
+          prijatych_bajtu++;
+          retezec_prijatych += (char)prijaty_bajt;
+        }
+
+        if (prijat_start_bit == true && prijaty_bajt == '#') {
+          prijato_odradkovani = true;
+          prijat_start_bit = false;
+        }
+
+        if (prijatych_bajtu >= 20) {
+          prijatych_bajtu = 0;
+          prijat_start_bit = false;
+          prijato_odradkovani = false;
+          retezec_prijatych = "";
+        }
+    }
+
+    if(prijato_odradkovani) {
+        servo_pozice_vse();
+        switch(retezec_prijatych[1]) {
+            case 'B': servo_pohyb_rotace(1); break; // vlevo
+            case 'C': servo_pohyb_rotace(-1); break; // vpravo
+            case 'A': servo_pohyb_spodni(1); break; // vzad
+            case 'D': servo_pohyb_spodni(-1); break; // vpred
+            case '5': servo_pohyb_celisti(-1); break; // zavrit
+            case '6': servo_pohyb_celisti(1); break; // otevrit
+            case '4': servo_pohyb_vrchni(1); break; // nahoru
+            case '7': servo_pohyb_vrchni(-1); break; // dolu
+            default: break;
+        }
+
+      retezec_prijatych = "";
+      prijato_odradkovani = false;
+    }       
+        
+    delay(GLOBALNI_PERIODA_OBNOVOVANI);
+}