Prezentace 9, ovladani ramen-
This commit is contained in:
vojta@alfred
@@ -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);
|
||||
}
|
||||
@@ -0,0 +1,203 @@
|
||||
// PROGRAM OVLADANI ROBOTICKEHO RAMENE
|
||||
// VojtaR, brezen 2026
|
||||
|
||||
// joysticky neovladaji motory plynule, pouze urcuji, zda (a) nedelaji nic, (b) toci se vlevo, (c) toci se vpravo
|
||||
|
||||
#include <Servo.h> // servomotory
|
||||
|
||||
|
||||
// ------------------------------------------------------------ //
|
||||
|
||||
|
||||
// mapovani vstupnich GPIO pinu
|
||||
const uint8_t JOY_1_KEY = -1; // napriklad 8
|
||||
const uint8_t JOY_1_X = -1; // nutno analogovy, napriklad A3
|
||||
const uint8_t JOY_1_Y = -1; // nutno analogovy, napriklad A4
|
||||
const uint8_t JOY_2_KEY = -1; // napriklad 7
|
||||
const uint8_t JOY_2_X = -1; // nutno analogovy, napriklad A2
|
||||
const uint8_t JOY_2_Y = -1; // nutno analogovy, napriklad A5
|
||||
|
||||
// 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 = 20; // ms
|
||||
const uint16_t JOY_MEZ_HORNI 1000; // 0-1023
|
||||
const uint16_t JOY_MEZ_DOLNI 50; // 0-1023
|
||||
const uint8_t SERVO_KROK_TRVANI = 5; // ms
|
||||
const uint8_t SERVO_ROTACE_MIN = 0;
|
||||
const uint8_t SERVO_ROTACE_MAX = 180;
|
||||
const uint8_t SERVO_ROTACE_KROK = 2;
|
||||
const uint8_t SERVO_SPODNI_MIN = 25;
|
||||
const uint8_t SERVO_SPODNI_MAX = 180;
|
||||
const uint8_t SERVO_SPODNI_KROK = 2;
|
||||
const uint8_t SERVO_VRCHNI_MIN = 0;
|
||||
const uint8_t SERVO_VRCHNI_MAX = 135;
|
||||
const uint8_t SERVO_VRCHNI_KROK = 2;
|
||||
const uint8_t SERVO_CELISTI_MIN = 45;
|
||||
const uint8_t SERVO_CELISTI_MAX = 120;
|
||||
const uint8_t SERVO_CELISTI_KROK = 3;
|
||||
const uint8_t SERVO_VYCHOZI_POZICE = 90;
|
||||
|
||||
|
||||
// ------------------------------------------------------------ //
|
||||
|
||||
|
||||
// promenne periferii
|
||||
int8_t joy_1_zakl_key = 0;
|
||||
int8_t joy_1_akt_x = 0;
|
||||
int8_t joy_1_akt_y = 0;
|
||||
int8_t joy_1_akt_key = 0;
|
||||
int8_t joy_2_zakl_key = 0;
|
||||
int8_t joy_2_akt_x = 0;
|
||||
int8_t joy_2_akt_y = 0;
|
||||
int8_t joy_2_akt_key = 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;
|
||||
|
||||
// zkratky stavu tlacitek
|
||||
typedef enum {VOLNO, STISKNUTO} stisk_t;
|
||||
|
||||
// inicializace zakladni pozice joysticku
|
||||
void joy_init() {
|
||||
pinMode(JOY_1_KEY, INPUT_PULLUP);
|
||||
joy_1_zakl_key = digitalRead(JOY_1_KEY);
|
||||
pinMode(JOY_2_KEY, INPUT_PULLUP);
|
||||
joy_2_zakl_key = digitalRead(JOY_2_KEY);
|
||||
}
|
||||
|
||||
// precteni stavu joysticku a ulozeni do promennych
|
||||
void joy_stav() {
|
||||
joy_1_akt_x = 0; // nacteni z joysticku a mapovani na hodnoty -1, 0, 1
|
||||
if(analogRead(JOY_1_X) > JOY_MEZ_HORNI) joy_1_akt_x = 1;
|
||||
if(analogRead(JOY_1_X) < JOY_MEZ_DOLNI) joy_1_akt_x = -1;
|
||||
|
||||
joy_1_akt_y = 0;
|
||||
if(analogRead(JOY_1_Y) > JOY_MEZ_HORNI) joy_1_akt_y = 1;
|
||||
if(analogRead(JOY_1_Y) < JOY_MEZ_DOLNI) joy_1_akt_y = -1;
|
||||
|
||||
joy_1_akt_key = (digitalRead(JOY_1_KEY) == joy_1_zakl_key) ? VOLNO : STISKNUTO;
|
||||
|
||||
joy_2_akt_x = 0;
|
||||
if(analogRead(JOY_2_X) > JOY_MEZ_HORNI) joy_2_akt_x = 1;
|
||||
if(analogRead(JOY_2_X) < JOY_MEZ_DOLNI) joy_2_akt_x = -1;
|
||||
|
||||
joy_2_akt_y = 0;
|
||||
if(analogRead(JOY_2_Y) > JOY_MEZ_HORNI) joy_2_akt_y = 1;
|
||||
if(analogRead(JOY_2_Y) < JOY_MEZ_DOLNI) joy_2_akt_y = -1;
|
||||
|
||||
joy_2_akt_key = (digitalRead(JOY_2_KEY) == joy_2_zakl_key) ? VOLNO : STISKNUTO;
|
||||
}
|
||||
|
||||
// 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 (joystick 2, osa x)
|
||||
void servo_pohyb_rotace() {
|
||||
if(joy_2_akt_x == 0) return; // zkratka
|
||||
int32_t nova_pozice = servo_rotace_pozice;
|
||||
nova_pozice = (joy_2_akt_x > 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);
|
||||
}
|
||||
|
||||
// zdvih spodni casti ramene (joystick 2, osa y)
|
||||
void servo_pohyb_spodni() {
|
||||
if(joy_2_akt_y == 0) return;
|
||||
int32_t nova_pozice = servo_spodni_pozice;
|
||||
nova_pozice = (joy_2_akt_y > 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);
|
||||
}
|
||||
|
||||
// zdvih spodni casti ramene (joystick 1, osa y)
|
||||
void servo_pohyb_vrchni() {
|
||||
if(joy_1_akt_y == 0) return;
|
||||
int32_t nova_pozice = servo_vrchni_pozice;
|
||||
nova_pozice = (joy_1_akt_y > 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);
|
||||
}
|
||||
|
||||
// pohyb celisti ramene (joystick 1, osa x)
|
||||
void servo_pohyb_celisti() {
|
||||
if(joy_1_akt_x == 0) return;
|
||||
int32_t nova_pozice = servo_celisti_pozice;
|
||||
nova_pozice = (joy_1_akt_x > 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);
|
||||
}
|
||||
|
||||
void servo_pohyb_vse() {
|
||||
// pohnout vsemi relevantnimi servomotory dle obou joysticku
|
||||
servo_pohyb_rotace();
|
||||
servo_pohyb_spodni();
|
||||
servo_pohyb_vrchni();
|
||||
servo_pohyb_celisti();
|
||||
// vyckavat dobu trvani nejdelsiho kroku kterehokoliv ze servomotoru
|
||||
delay(nejdelsi_krok*SERVO_KROK_TRVANI);
|
||||
}
|
||||
|
||||
|
||||
// ------------------------------------------------------------ //
|
||||
|
||||
|
||||
// inicilalizace a nastaveni
|
||||
void setup() {
|
||||
joy_init();
|
||||
servo_init();
|
||||
servo_najit_nejdelsi_krok();
|
||||
servo_reset();
|
||||
}
|
||||
|
||||
// hlavni smycka
|
||||
void loop() {
|
||||
servo_pozice_vse();
|
||||
joy_stav();
|
||||
servo_pohyb_vse();
|
||||
delay(GLOBALNI_PERIODA_OBNOVOVANI);
|
||||
}
|
||||
Reference in New Issue
Block a user