Pastikan terlebih dahulu posisi motor Servo pada setiap lengan. Tes dahulu posisi lurus body, datar lengan kiri, vertikal lengan kanan dan posisi tutup jari.
#include <Servo.h>
Servo body,kiri,kanan,jari;
void setup() {
Serial.begin(9600);
body.attach(8); //posisi body 90 lurus,0 arah kanan
kiri.attach(9); //tuas kiri 180 tuas datar, 90 tuas vertikal
kanan.attach(10); //tuas kanan 50 vertikal, 95 turun maju
jari.attach(11); //170 jari tutup, 90 jari terbuka
}
void posisi(int badan,int kiwo,int tengen,int telapak){
body.write(badan);
kiri.write(kiwo);
kanan.write(tengen);
jari.write(telapak);
}
void ambil0(){
posisi(0,95,50,90); //lengan normal jari buka
delay(1000);
posisi(0,90,95,170); //maju japit
delay(1000);
posisi(0,95,50,170); //lengan normal jari jepit
delay(1000);
posisi(0,180,40,170); //angkat japit
delay(1000);
}
void ambil90(){
posisi(90,95,50,90); //turun jari buka
delay(500);
posisi(90,90,95,170); //maju jepit
delay(500);
posisi(90,95,50,170); //turun jari jepit
delay(500);
posisi(90,180,40,170); //angkat japit
delay(500);
}
void taruh0(){
posisi(0,110,50,170); //turun jari buka
delay(500);
posisi(0,110,75,90); //maju japit
delay(500);
posisi(0,110,50,90); //turun jari jepit
delay(500);
posisi(0,180,40,90); //angkat japit
delay(500);
}
void taruh90(){
posisi(90,95,50,170); //lengan normal jepit
delay(500);
posisi(90,90,95,90); //lengan maju buka
delay(500);
posisi(90,95,50,90); //lengan normal buka
delay(500);
posisi(90,180,40,90); //angkat buka
delay(500);
}
void angkat90jepit(){
posisi(90,180,40,170); // angkat jepit putar 90
delay(500);
}
void angkat0buka(){
posisi(0,180,40,90); //angkat buka putar 0
delay(500);
}
void angkat90buka(){
posisi(90,180,40,90); // angkat buka putar 90
delay(500);
}
void angkat0jepit(){
posisi(0,180,40,170); //angkat jepit putar 0
delay(500);
}
void loop(){
angkat90buka();
ambil90();
angkat0jepit();
taruh0();
}
#include <Servo.h>
Servo body,kiri,kanan,jari;
void setup() {
Serial.begin(9600);
body.attach(8); //posisi body 90 lurus,0 arah kanan
kiri.attach(9); //tuas kiri 180 tuas datar, 90 tuas vertikal
kanan.attach(10); //tuas kanan 50 vertikal, 95 turun maju
jari.attach(11); //170 jari tutup, 90 jari terbuka
}
void posisi(int badan,int kiwo,int tengen,int telapak){
body.write(badan);
kiri.write(kiwo);
kanan.write(tengen);
jari.write(telapak);
}
void ambil0(){
posisi(0,95,50,90); //lengan normal jari buka
delay(1000);
posisi(0,90,95,170); //maju japit
delay(1000);
posisi(0,95,50,170); //lengan normal jari jepit
delay(1000);
posisi(0,180,40,170); //angkat japit
delay(1000);
}
void ambil90(){
posisi(90,95,50,90); //turun jari buka
delay(500);
posisi(90,90,95,170); //maju jepit
delay(500);
posisi(90,95,50,170); //turun jari jepit
delay(500);
posisi(90,180,40,170); //angkat japit
delay(500);
}
void taruh0(){
posisi(0,110,50,170); //turun jari buka
delay(500);
posisi(0,110,75,90); //maju japit
delay(500);
posisi(0,110,50,90); //turun jari jepit
delay(500);
posisi(0,180,40,90); //angkat japit
delay(500);
}
void taruh90(){
posisi(90,95,50,170); //lengan normal jepit
delay(500);
posisi(90,90,95,90); //lengan maju buka
delay(500);
posisi(90,95,50,90); //lengan normal buka
delay(500);
posisi(90,180,40,90); //angkat buka
delay(500);
}
void angkat90jepit(){
posisi(90,180,40,170); // angkat jepit putar 90
delay(500);
}
void angkat0buka(){
posisi(0,180,40,90); //angkat buka putar 0
delay(500);
}
void angkat90buka(){
posisi(90,180,40,90); // angkat buka putar 90
delay(500);
}
void angkat0jepit(){
posisi(0,180,40,170); //angkat jepit putar 0
delay(500);
}
void loop(){
angkat90buka();
ambil90();
angkat0jepit();
taruh0();
}
0 komentar:
Posting Komentar