deneyap by şehitkamil bel kız aihl

imu kodu

sehitkamilbelkaihl — 2022-05-22 08:13:47 UTC

#include "deneyap.h"
#include "lsm6dsm.h"
typedef struct struct_message { float accel_x; float accel_y; float accel_z;
}
struct_message;
LSM6DSM IMU;
struct_message imuData;
void setup() {
Serial.begin(115200);
IMU.begin();
}
void loop() {
float accAngleX;
float accAngleY;
accAngleX = atan(imuData.accel_x / sqrt(pow(imuData.accel_y, 2) + pow(imuData.accel_z, 2)+0.001)) * 180 / PI;
accAngleY = atan(-1 * imuData.accel_y / sqrt(pow(imuData.accel_x, 2) + pow(imuData.accel_z, 2)+0.001)) * 180 / PI;
Serial.print("accAngleX: "); Serial.println(accAngleX); Serial.print("accAngleY: "); Serial.println(accAngleY);
imuData.accel_x = IMU.readFloatAccelX(); Serial.print("accel_x: ");
Serial.println(imuData.accel_x);
imuData.accel_y = IMU.readFloatAccelY(); Serial.print("accel_y: ");
Serial.println(imuData.accel_y); imuData.accel_z = IMU.readFloatAccelZ();
Serial.print("accel_z: ");
Serial.println(imuData.accel_z); delay(50);
}

imu ile servo motor kontrolü

sehitkamilbelkaihl — 2022-05-22 09:30:50 UTC

#include "deneyap.h"
#include "lsm6dsm.h"
#include "ESP32Servo.h"
#define SERVOPIN D0
typedef struct struct_message
{
float accel_x;
float accel_y;
float accel_z;
}
struct_message;
float angle_x=0.;
float angle_y=0.;
float angle_z=0.;
Servo myservo;
int pos=0;
int ref_pos=0;
LSM6DSM IMU;
struct_message imuData;
void setup()
{
IMU.begin();
myservo.attach(SERVOPIN);
}
void loop()
{ ref_pos=90;
imuData.accel_x = IMU.readFloatAccelX();
imuData.accel_y = IMU.readFloatAccelY();
imuData.accel_z = IMU.readFloatAccelZ();
angle_x = atan(imuData.accel_x / sqrt(pow(imuData.accel_y, 2) + pow(imuData.accel_z, 2)+0.001)) * 180 / PI; pos=angle_x;
myservo.write(ref_pos+pos);
delay(10);
}

Buzzer ve mesafe

sehitkamilbelkaihl — 2022-05-22 11:00:18 UTC

joystik

sehitkamilbelkaihl — 2022-05-29 07:12:28 UTC

#include "deneyap.h"
#define SW_pin D0
#define X_pin A0
#define Y_pin A1
int X_Val, Y_Val, Dig_Val;
void setup ()
{ Serial.begin(115200); // Seri portun 115200 baudrate hızında başlatılması.
pinMode(SW_pin, INPUT_PULLUP); // Switch (anahtar) değişkeni mod’u HIGH’ e ayarlanır. Butona basıldığında LOW değerini alır.
digitalWrite(SW_pin, HIGH); // Joystick eksen değişken değerlerinin alınacağı 2 pin’in INPUT (giriş) olarak tanımlanması.
pinMode(X_pin, INPUT); pinMode(Y_pin, INPUT);
}
void loop ()
{
X_Val = analogRead(X_pin);
Y_Val = analogRead(Y_pin);
Dig_Val =digitalRead(SW_pin);
Serial.print("X_Val: ");
Serial.println(X_Val);
Serial.print("Y_Val: ");
Serial.println(Y_Val);
Serial.print("Dig_Val: ");
Serial.println(Dig_Val);
delay(100);
}

joystik ve servo motor

sehitkamilbelkaihl — 2022-05-29 09:43:13 UTC

#include "deneyap.h"
#include
#define SERVOPIN1 D14
#define SERVOPIN2 D9
Servo myservo1, myservo2; // Servo motorların tanımlanması.
int pos = 0;
void setup()
{
myservo1.attach(SERVOPIN1); myservo2.attach(SERVOPIN2);
}
void loop()
{
for (pos = 0;
pos <= 180;
pos += 1)
{ // Motorlara pos değerinin yollanması
myservo1.write(pos);
myservo2.write(pos);
delay (10); // Her bir döngüde 10 milisaniye bir gecikme ilave edilmesi.
}
for (pos = 180; pos >= 0; pos -= 1)
{ // Motorlara pos değerinin yollanması
myservo1.write(pos);
myservo2.write(pos);
delay (10); // Her bir döngüde 10 milisaniye bir gecikme ilave edilmesi.
}
}

joystik ve servo 2

sehitkamilbelkaihl — 2022-06-05 09:31:53 UTC

#include "deneyap.h"
#include
#define SERVOPIN1 D14
#define SERVOPIN2 D9
#define SW_pin D0
#define X_pin A0
#define Y_pin A1
Servo myservo1, myservo2;
int X_Val, Y_Val; // Joystick eksen değişkenleri.
int pos1=0, pos2=0; // Motorların pozisyon değişkenleri. int dly = 10; // Genel döngüde ilave edilen gecikme için kullanılan değişken.
int stp = 1; // Motorların dönüş açısının değiştirilmesi durumunda her döngüde kullanılan adım değişkeni. Örneğin servo 1 pozitif yöne dönsün istendiğinde her döngüde, Pos1+=stp olacaktır (kod aşağıda).
int Dig_Val = 0; // Buton değeri
int teta1min=0;
int teta1max=180;
int teta2min=0;
int teta2max=180;
void setup()
{ // Servo motorların hangi dijital pinlere bağlı olduklarının belirtilmesi.
myservo1.attach(SERVOPIN1); myservo2.attach(SERVOPIN2);
pinMode(SW_pin, INPUT_PULLUP); // Switch (anahtar) değişkeni mod’u HIGH’ e ayarlanır. Butona basıldığında LOW değerini alır.
digitalWrite(SW_pin, HIGH);
pinMode(X_pin, INPUT);
pinMode(Y_pin, INPUT);
}
void loop()
{
X_Val = analogRead(X_pin);
Y_Val = analogRead(Y_pin);
Dig_Val=digitalRead(SW_pin);
if (Dig_Val<1)
{
if
(dly==5) dly=10;
else
{
if(dly==10) dly=20;
else if (dly==20) dly=5;
} delay (500); // Butona basış tespit edildikten sonra 500 ms bir gecikme ilave edilmesi.
}
if (X_Val<1000 && pos1>teta1min) pos1-=stp;
if (X_Val>3000 && pos1 if (Y_Val<1000 && pos2>teta2min) pos2-=stp;
if (Y_Val>3000 && pos2 myservo1.write(pos1);
myservo2.write(pos2);
delay (dly); // bu gecikme işlemi ile motorların dönüş hızları tayin edilir.
}

IMU sensörü servo

sehitkamilbelkaihl — 2022-06-05 12:24:27 UTC

// Deneyap, ESP32 ve IMU kütüphanelerinin ve dijital servo pinlerinin tanımlanması
#include "deneyap.h"
#include
#include "lsm6dsm.h"
#define SERVOPIN1 D14
#define SERVOPIN2 D9
// Üç boyutlu İvme değişkenlerinin tanımlanması.
typedef struct struct_message {
float accel_x;
float accel_y;
float accel_z;
} struct_message;
LSM6DSM IMU; // Kart üzerindeki ivme ölçerin tanımlanması.
struct_message imuData; // İvme ölçer verileri tek bir blok değişken tanımlanması.
// Servo motorların tanımlanması
Servo myservo1;
Servo myservo2;
// X ve Y eksenlerindeki ivme değişkenlerinin tanımlanması ve ilk değerlerinin atanması.
float accAngleX=0.;
float accAngleY=0.;
// Motorların dönüş açı değişkenlerinin tanımlanması ve ilk değerlerinin atanması.
int pos1 = 90;
int pos2 = 90;
void setup()
{
IMU.begin(); // IMU’yu başlat.
// Servo motorların hangi dijital pinlere bağlı olduklarının belirtilmesi.
myservo1.attach(SERVOPIN1);
myservo2.attach(SERVOPIN2);
}
void loop()
{
// IMU’dan 3 boyutlu ivme verisi alınması:
imuData.accel_x = IMU.readFloatAccelX();
imuData.accel_y = IMU.readFloatAccelY();
imuData.accel_z = IMU.readFloatAccelZ();
// İvme verisinden, X ve Y eğim açılarının trigonometrik olarak belirlenmesi.
accAngleX = atan(imuData.accel_x / sqrt(pow(imuData.accel_y, 2) +
pow(imuData.accel_z, 2)+0.001)) * 180 / PI;
accAngleY = atan(-1 * imuData.accel_y / sqrt(pow(imuData.accel_x, 2) +
pow(imuData.accel_z, 2)+0.001)) * 180 / PI;
// Motorlara belirlenen X ve Y eğim açılarının uygulanması:
pos1=90+int(accAngleX);
pos2=90+int(accAngleY);
myservo1.write(pos1);
myservo2.write(pos2);
delay(10); // Genel döngüye 10 milisaniyelik bir gecikme uygulanması.
}

imu joystik ile servo kontrolü

sehitkamilbelkaihl — 2022-06-05 13:02:37 UTC

// Deneyap, ESP32 ve IMU kütüphanelerinin tanımlanması
#include "deneyap.h"
#include
#include "lsm6dsm.h"
// Dijital servo pin bağlantıları:
#define SERVOPIN1 D14
#define SERVOPIN2 D9
//Joystick eksen analog bağlantı pinleri
#define X_pin A0
#define Y_pin A1
int X_Val, Y_Val; // Joystick eksen değişkenleri
int dly = 5; // Genel döngüdeki gecikme değişkeni
int stp = 1; // Servo motorların dönüş durumunda her bir döngüdeki dönüş
adımı
int X_jys=0, Y_jys=0; // Joystick eksen değişkenlerinden elde edilen motor
//dönüş açıları
// Motor dönüş açı alt ve üst limitleri:
int teta1min=0;
int teta1max=180;
int teta2min=0;
int teta2max=180;
// İvme ölçerden elde edilen X ve Y eksenlerindeki açısal dönüşler:
float accAngleX=0.;
float accAngleY=0.;
typedef struct struct_message { // İvme ölçerden elde edilecek 3 boyutlu açısal
//ivmelerin tek blokta tanımlanması.
float accel_x;
float accel_y;
float accel_z;
} struct_message;
LSM6DSM IMU; // IMU’ nun tanımlanması
struct_message imuData; // İvme ölçer parametrelerinin tek blok bir
//değişkende tanımlanması.
// Servo motorların tanımlanması.
Servo myservo1;
Servo myservo2;
// Motorların dönüş açı değişkenlerinin tanımlanması ve ilk değerlerinin
//atanması.
int pos1 = 90;
int pos2 = 90;
void setup()
{
IMU.begin(); // IMU’yu başlat.
// Servo motorların hangi dijital pinlere bağlı olduklarının belirtilmesi.
myservo1.attach(SERVOPIN1);
myservo2.attach(SERVOPIN2);
pinMode(BUILTIN_KEY, INPUT); // Deneyap kartı üzerindeki butonun giriş
//olarak tanımlanması
// Joystick pinlerinin giriş olarak tanımlanması
pinMode(X_pin, INPUT);
pinMode(Y_pin, INPUT);
// RGB Led pinlerinin çıkış olarak tanımlanması.
pinMode(LEDR, OUTPUT);
pinMode(LEDG, OUTPUT);
pinMode(LEDB, OUTPUT);
digitalWrite(LEDR, LOW); //Kırmızı led açık (aktif)
digitalWrite(LEDG, HIGH); // Yeşil led sönük (pasif)
digitalWrite(LEDB, HIGH); // Mavi led sönük (pasif)
}
void loop()
{
// Joystick X ve Y değişiminin okunması.
X_Val = analogRead(X_pin);
Y_Val = analogRead(Y_pin);
uint8_t buttonState = digitalRead(BUILTIN_KEY); // Buton durumu
// Kart üzerindeki butona her basıldığında, servo motor dönüş hızlarını
//belirleyen dly değişkenine değerler atanması ve buna göre kart üzerindeki RGB
//led’lerinin değiştirilmesi.
if(buttonState == BUTTON_PRESSED) // Butona basılma durumu
{
if (dly==5) {
dly=10;
digitalWrite(LEDR, HIGH);
digitalWrite(LEDG, LOW); //Yeşil led aktif
}
else {
if(dly==10) {
dly=20;
digitalWrite(LEDG, HIGH);
digitalWrite(LEDB, LOW); //Mavi led aktif
}
else if (dly==20) {
dly=5;
digitalWrite(LEDR, LOW); //Kırmızı led aktif
digitalWrite(LEDB, HIGH);
}
}
delay(300); //Butona basış tespit edildikten sonra 300 ms bir gecikme ilave
//edilmesi.
}
// Joystick’ten elde edilen dönüş açılarının limitler dahilinde uygulanması.
if (X_Val<1000 && pos1>teta1min) X_jys-=stp;
if (X_Val>3000 && pos1 if (Y_Val<1000 && pos2>teta2min) Y_jys-=stp;
if (Y_Val>3000 && pos2 // IMU’dan 3 boyutlu ivme verisi alınması:
imuData.accel_x = IMU.readFloatAccelX();
imuData.accel_y = IMU.readFloatAccelY();
imuData.accel_z = IMU.readFloatAccelZ();
// İvme verisinden, X ve Y eğim açılarının trigonometrik olarak belirlenmesi.
accAngleX = atan(imuData.accel_x / sqrt(pow(imuData.accel_y, 2) +
pow(imuData.accel_z, 2)+0.001)) * 180 / PI;
accAngleY = atan(-1 * imuData.accel_y / sqrt(pow(imuData.accel_x, 2) +
pow(imuData.accel_z, 2)+0.001)) * 180 / PI;
// Motor dönüş açıları:
pos1=90+int(accAngleX)+X_jys;
pos2=90+int(accAngleY)+Y_jys;
// Motorlara dönüş komutlarının verilmesi.
myservo1.write(pos1);
myservo2.write(pos2);
delay(dly); // bu gecikme işlemi ile motorların dönüş hızları tayin edilir.
}

Işık Sensörleri ile 2 Servo Motor

sehitkamilbelkaihl — 2022-06-12 09:38:03 UTC

// Deneyap ve ESP32 kütüphanelerinin tanımlanması
#include "deneyap.h"
#include
// Dijital servo pin bağlantıları:
#define SERVOPIN1 D9
#define SERVOPIN2 D14
// Işık sensörleri analog bağlantıları:
#define L1_pin A0
#define L2_pin A1
#define L3_pin A2
// Potansiyometre analog bağlantısı.
#define Pot_pin A3
// Motor dönüş açı alt ve üst limitleri:
int teta1min=0;
int teta1max=180;
int teta2min=0;
int teta2max=180;
//Accelerometre X ve Y eksenleri etrafı açı değişkenleri
int accAngleX=0;
int accAngleY=0;
int coef; // Potansiyometreden elde edilen katsayı değişkeni.
int valin1, valin2, valin3; // Işık sensörlerinden hesaplanan değerlerin
atanacağı değişkenler.
//Servo motor dönüş açı değişkenleri
int pos1 = 0;
int pos2 = 0;
// İvme ölçer verisinde oluşan noise’i (gürültüyü) azaltmak (hatta yok etmek)
için “moving average” algoritmasının uygulanmasında kullanılan değişkenler.
int i=0;
int n=10;
int32_t tot1=0.;
int32_t tot2=0.;
int32_t tot3=0.;
// Servo motorların tanımlanması.
Servo myservo1, myservo2;
void setup()
{
// Servo motorların hangi dijital pinlere bağlı olduklarının belirtilmesi.
myservo1.attach(SERVOPIN1);
myservo2.attach(SERVOPIN2);
}
void loop()
{
coef = analogRead(Pot_pin); //Potansiyometre çıkışının bir katsayı
değişkenine atanması.
// Işık sensörlerinden hesaplanan değerlerin atandığı değişkenler.
valin1 = 1023-analogRead(L1_pin);
valin2 = 1023-analogRead(L2_pin);
valin3 = 1023-analogRead(L3_pin);
// Işık sensörü verisindeki noise’i (gürültüyü) azaltmak için “moving average”
(hareketli ortalama) yöntemi:
if (i i++;
}
tot1=int(((i-1)*tot1+valin1)/i);
tot2=int(((i-1)*tot2+valin2)/i);
tot3=int(((i-1)*tot3+valin3)/i);
// Işık sensörlerinden hesaplana verilerin kullanılması ile hesaplanan servo
motor dönüş açıları:
pos1=90+(tot2-tot1)*((2048-coef)/2048.);
pos2=120+(0.5*(tot1+tot2)-tot3)*((2048-coef)/2048.);
// Motorlara dönüş komutlarının verilmesi.
myservo1.write(pos1);
myservo2.write(pos2);
delay(5); // Genel döngü içerisinde 5 milisaniye gecikme.
}

üç serbestlik dereceli kol

sehitkamilbelkaihl — 2022-06-12 09:55:21 UTC

// Deneyap ve ESP32 kütüphanelerinin tanımlanması
#include "deneyap.h"
#include
// Dijital joysticklerin button pin bağlantıları:
#define SW1_pin D0
#define SW2_pin D1
//Joysticklerin eksen analog bağlantı pinleri
#define X1_pin A0
#define Y1_pin A1
#define X2_pin A2
#define Y2_pin A3
// Dijital servo pin bağlantıları:
#define SERVOPIN1 D9
#define SERVOPIN2 D12
#define SERVOPIN3 D13
#define SERVOPIN4 D14
// Motor dönüş açı alt ve üst limitleri:
//Aşağıdaki teta değerleri 0-3000 arasında kalibre edilmiştir. 3000 değeri 180
dereceye karşılık gelmektedir.
int teta1min=0;
int teta1max=3000;
int teta2min=0;
int teta2max=3000;
int teta3min=0;
int teta3max=3000;
int teta4min=1500;
int teta4max=2000;
// Motor dönüş açıları için değişkenlerin tanımlanması
int valin1, valin2, valin3, valin4;
int valcalc1=1500;//Başlangıç açı değerleri
int valcalc2=1500;
int valcalc3=1500;
int valcalc4=2000;
int jystp=20; // Joysticklerle motor hareketleri sağlandığında her bir döngüde
istenen açı değeri (bu yazılımda 3000 değerinin 360 dereceye karşılık geldiğini
unutmayın.
bool playmode=false; // Robotun tekrarlama (oynama) modu değişkeni. Değer
false olduğunda, öğretme modunda demektir.
int ButtonMode=0; // Buton modu değişkeninin tanımlanması. Sıfır olduğunda,
öğretme modunda demektir
unsigned long CurrentMilliSec = millis(); // milis() fonksiyonu, şu anki saati
milisecond cinsinden vermektedir. Zamanlayıcı olarak bu fonksiyonu kullanmaktayız.
Herhangi bir anda, o anki zamanı bir long değişkenine atayabiliriz.
long PreMilliSec=0; // Bir evvelki zaman (milisaniye cinsinden)
int NumMemory=0; // Hafıza sayacı (Maksimum 50 olabilir. Deneyap kartının
kapasitesine bağlı olarak bu sınır 50’den yukarıda olabilir.)
int PreNumMemory=0; // Bir evvelki moddaki hafıza değeri.
int Step=-1; // Öğretme modunda, servo pozisyonlarını hafızaya alırken
kullanılan bir değişken.
// Servo pozisyonlarının hafızaya alındığı 50’lik seriye sahip değişkenler.
int joint1[50];
int joint2[50];
int joint3[50];
int joint4[50];
int maxarray=49; //Hafıza hatası vermeyi önlemek için 59 değerine sahip bir
limit değişkeni.
int prehiz=10; // Bir önceki hız değerinin atanacağı değişken tanımlaması.
int normalhiz=10; // Normal hız değişkeni
// hiz=1 çok yavaş, 5 yavaş, 10 normal, 15 hızlı, 20+ çok hızlı
int hiz=10; // Şu anki hız değişkeni
int prepos1, prepos2, prepos3, prepos4; // bir evvelki motor konumları.
int pos11,pos21,pos31,pos41; // Motor konumları ile ilgili ikincil (yardımcı)
değişkenler.
Servo myservo1, myservo2, myservo3, myservo4; // Servo motorların
tanımlanması.
void setup()
{
// Servo motorların hangi dijital pinlere bağlı olduklarının belirtilmesi.
myservo1.attach(SERVOPIN1);
myservo2.attach(SERVOPIN2);
myservo3.attach(SERVOPIN3);
myservo4.attach(SERVOPIN4);
// Joystik butonlarının pinlerinin tanımlanması (Normalde HIGH).
pinMode(SW1_pin, INPUT_PULLUP);
pinMode(SW2_pin, INPUT_PULLUP);
// Joystick pinlerinin giriş olarak tanımlanması
pinMode(X1_pin, INPUT);
pinMode(Y1_pin, INPUT);
pinMode(X2_pin, INPUT);
pinMode(Y2_pin, INPUT);
// Joystik butonlarının pinlerinin değerleri normalde HIGH’ a atandı.
digitalWrite(SW1_pin, HIGH);
digitalWrite(SW2_pin, HIGH);
// Tercihe bağlı olarak, Seri port’a “Ready” yazdırılması (baudrate= 9600)
Serial.begin(9600);
Serial.println("Ready.");
}
void loop() {
CurrentMilliSec = millis(); // Şu anki zaman değeri (ms)
Button(); // Kendi yazdığımız Button fonksiyonunun çağırılması.
if (playmode==false && Step==-1){ // Öğretme modunda iken,
ReadAnalog(); // Analog kanalların okunması için kendi yazdığımız
ReadAnalog fonksiyonunun çağırılması.
// Servo motorlara dönüş komutlarının verilmesi:
myservo1.writeMicroseconds(valcalc1);
myservo2.writeMicroseconds(valcalc2);
myservo3.writeMicroseconds(valcalc3);
myservo4.writeMicroseconds(valcalc4);
delay(int(300./hiz)); // Hız kontrolü için genel döngü içerisindeki gecikme
ayarı.
}
else{
playit(); //tekrarlama (veya oynama) modunda iken kendi yazdığımız Play
fonksiyonunun çağırılması.
}
}
Joysticklerin ürettiği analog sinyallerin okunduğu ve motor açı değerlerinin bu
sinyallere göre değiştirildiği fonksiyon aşağıda verilmiştir.
void ReadAnalog() {
// Analog pinlere giren joystick değerlerinin okunması.
valin1 = analogRead(Y1_pin);
valin2 = analogRead(X1_pin);
valin3 = analogRead(X2_pin);
valin4 = analogRead(Y2_pin);
// Belli sınırlar dahilinde, Joystik değerlerine göre servo motor dönüş
açılarının tayini.
if (valin1<1000 && valcalc1>teta1min) valcalc1-=jystp;
if (valin1>3000 && valcalc1 if (valin2<1000 && valcalc2>teta2min) valcalc2+=jystp;
if (valin2>3000 && valcalc2 if (valin3<1000 && valcalc3>teta3min) valcalc3-=jystp;
if (valin3>3000 && valcalc3 if (valin4<1000 && valcalc4>teta4min) valcalc4-=jystp;
if (valin4>3000 && valcalc4}
void Button() { // Butona basılma durumu
if (playmode==false){ // Eğer öğrenme modunda iken,
if (digitalRead(SW1_pin) == 0){
delay(100);
if (digitalRead(SW1_pin) == 1) { // butona tıklanmış ise,
if (ButtonMode == 0)
{
ButtonMode = 1; // İlk buton tek tık tespiti.
PreMilliSec = CurrentMilliSec;
}
else if ((ButtonMode == 1) && (CurrentMilliSec - PreMilliSec < 500))
{
ButtonMode = 2; // İlk buton çift tık tespiti.
}
}
}
if ((ButtonMode == 1) && (CurrentMilliSec - PreMilliSec > 600)) {
saveit(); // İlk buton tek tık tespit edildiğinde, motor konumları hafızaya
alınır.
NumMemory += 1;
if (NumMemory>maxarray) NumMemory= maxarray;
ButtonMode = 0;
Serial.print("Saved: "); Serial.println(NumMemory);
}
else if (ButtonMode == 2 && (CurrentMilliSec - PreMilliSec > 600)) {
if ((NumMemory>1 || PreNumMemory>1) && playmode==false){
Serial.println("Playmode...");
playmode = true; // Çift tık tespit edildiğinde tekrarlama (oynama) moduna
geçilir.
ButtonMode = 0;
if (NumMemory<2) NumMemory=PreNumMemory; else
PreNumMemory=NumMemory; // Eğer hafızaya herhangi bir konum
alınmamış olup ta çift tık yapılırsa, bir önceki oynatılmış konum bilgilerinin
tekrar kullanılması için devreye alınması.
// Motor açı konumlarının hafızaya alınması:
joint1[0]=pos11;
joint2[0]=pos21;
joint3[0]=pos31;
joint4[0]=pos41;
Step=NumMemory-1;
ReadAnalog();
prepos1=valcalc1;
prepos2=valcalc2;
prepos3=valcalc3;
prepos4=valcalc4;
}
}
if (ButtonMode<3 && CurrentMilliSec - PreMilliSec > 1000) {
ButtonMode = 0; // Tekrar öğretme moduna geçiş.
}
}
else if (ButtonMode<3 && digitalRead(SW1_pin) == 0) {
ButtonMode = 3;
Serial.println("Stopping."); // Tekrarlamayı (Oynamayı) durdur.
ReadAnalog();
NumMemory+=1;
// Durulan konumun hafızanın en son basamağına kaydedilmesi.
joint1[NumMemory-1] = valcalc1;
joint2[NumMemory-1] = valcalc2;
joint3[NumMemory-1] = valcalc3;
joint4[NumMemory-1] = valcalc4;
}
// İkinci butona her tık yapıldığında robot hareket hızı değiştirilir. Normal,
yavaş, hızlı olmak üzere 3 ayrı hız, aşağıdaki kod yazılmıştır. Diğer detaylar,
önceki bölümlerdeki kodlarda anlatıldığı gibidir.
if (digitalRead(SW2_pin) == 0) {
if (prehiz==hiz) {
if (hiz==normalhiz) {
hiz=2*normalhiz;
jystp=40;
Serial.println("High speed.");
if (playmode==false) {
delay(300);
}
} else {
if (hiz==(2*normalhiz)) {
hiz=normalhiz/2;
jystp=10;
Serial.println("Low speed.");
if (playmode==false) {
delay(300);
}
}
else if (hiz==(normalhiz/2)) {
hiz=normalhiz;
jystp=20;
Serial.println("Normal speed.");
if (playmode==false) {
delay(300);
}
}
}
}
}
else{
prehiz=hiz;
}
}
Mevcut servo motor açılarının hafızaya kaydedildiği fonksiyon aşağıda verilmiştir.
Detaylar, önceki bölümlerde verilen kodlardaki gibidir.
void saveit() {
joint1[NumMemory] = valcalc1;
joint2[NumMemory] = valcalc2;
joint3[NumMemory] = valcalc3;
joint4[NumMemory] = valcalc4;
prepos1=valcalc1;
prepos2=valcalc2;
prepos3=valcalc3;
prepos4=valcalc4;
if (NumMemory==1){
pos11=joint1[0];
pos21=joint2[0];
pos31=joint3[0];
pos41=joint4[0];
}
}
Hafızaya kaydedilen servo motor konumlarının tekrarlatıldığı fonksiyon aşağıda
verilmiştir. Detaylar, önceki bölümlerde verilen kodlardaki gibidir.
void playit()
{
float pos1=prepos1;
float pos2=prepos2;
float pos3=prepos3;
float pos4=prepos4;
// Motor dönüşlerinin olabildiğince düzgün ve sürekli hissedilmesi için dönüş
açı mesafeleri yukarıda tanımlanmış farklara bölüştürülüp servo motorlara
uygulanmıştır.
float dif1=abs(joint1[Step]-pos1);
float dif2=abs(joint2[Step]-pos2);
float dif3=abs(joint3[Step]-pos3);
float dif4=abs(joint4[Step]-pos4);
float maxdif=0.0;
if (dif1>maxdif) maxdif=dif1;
if (dif2>maxdif) maxdif=dif2;
if (dif3>maxdif) maxdif=dif3;
if (dif4>maxdif) maxdif=dif4;
maxdif=maxdif/hiz;
if (maxdif<1.0) maxdif=1.0;
float stp1=(joint1[Step]-pos1)/maxdif;
float stp2=(joint2[Step]-pos2)/maxdif;
float stp3=(joint3[Step]-pos3)/maxdif;
float stp4=(joint4[Step]-pos4)/maxdif;
for (int i=1;i<=int(maxdif);i++){ // Motor dönüşlerinin olabildiğince düzgün ve
sürekli hissedilmesi için dönüş açı mesafeleri yukarıda tanımlanmış farklara
bölüştürülüp servo motorlara uygulanmıştır.
pos1=pos1+stp1;
pos2=pos2+stp2;
pos3=pos3+stp3;
pos4=pos4+stp4;
if (i==int(maxdif)){
pos1=joint1[Step];
pos2=joint2[Step];
pos3=joint3[Step];
pos4=joint4[Step];
}
myservo1.writeMicroseconds(int(pos1));
myservo2.writeMicroseconds(int(pos2));
myservo3.writeMicroseconds(int(pos3));
myservo4.writeMicroseconds(int(pos4));
prepos1=pos1;
prepos2=pos2;
prepos3=pos3;
prepos4=pos4;
delay(10);
if (ButtonMode<3) Button();
else delay(5);
}
if (ButtonMode==3 && Step==NumMemory-1) { // Oynatma modunda
durdurma işlemi.
NumMemory = 0;
Step=-1;
ButtonMode = 0;
playmode=false;
Serial.println("Stopped.");
}
else{
Step+=1;
if (Step>NumMemory-1) Step=0; // Hafıza aşıldığında, ilk adıma dönüş.
Burada istenirse “Step=NumMemory-1;” de yazılabilir ve hafızada sürekli en
son adımda kalınır.
}
}