ERSBI Menggunakan Kamera CMUCAM 4 Untuk Mendeteksi Gawang Kuning Dan Bola Orange

Berikut ini adalah program Arduino yang digunakan untuk mendeteksi bola , mengejar bola , membawa bola ke arah gawang dan menendangnya……….
program ini adalah program awalan sebelum saya mengikuti kontes ESRB di KRI Nasional
Bagi yang ingin belajar monggo dipahami.. wkwkwkwk……..
mudah kok 😀
Besok2 saya akan coba upload program yang sudah ada referee box nya

/***************************************************************************//**
* @file
* Color Tracking Template Code
*
* @version @n 1.0
* @date @n 8/14/2012
*
* @authors @n Kwabena W. Agyeman
* @copyright @n (c) 2012 Kwabena W. Agyeman
* @n All rights reserved – Please see the end of the file for the terms of use
*
* @par Update History:
* @n v1.0 – Initial Release – 8/14/2012
*******************************************************************************/

#include <CMUcam4.h>
#include <CMUcom4.h>
#include <LiquidCrystal.h>
#include <Wire.h>
#include <CMPS10.h>
#include <Servo.h>
CMPS10 compass;
Servo myservo;

///////bola orange///////
#define RED_MIN 189
#define RED_MAX 233
#define GREEN_MIN 120
#define GREEN_MAX 161
#define BLUE_MIN 70
#define BLUE_MAX 108

//////////gawang kuning///////
#define RED_MIN_K 201
#define RED_MAX_K 226
#define GREEN_MIN_K 212
#define GREEN_MAX_K 229
#define BLUE_MIN_K 117
#define BLUE_MAX_K 149

//////////gawang kuning siang hari///////
#define RED_MIN_KS 161
#define RED_MAX_KS 225
#define GREEN_MIN_KS 177
#define GREEN_MAX_KS 229
#define BLUE_MIN_KS 64
#define BLUE_MAX_KS 137
#define LED_BLINK 5 // 5 Hz
#define WAIT_TIME 5000 // 5 seconds

int pwm1 = 12; // Arduino PWM output pin 5; connect to IBT-2 pin 1 (RPWM)
int dir1 = 13; // Arduino PWM output pin 6; connect to IBT-2 pin 2 (LPWM)

int pwm2 = 10; // Arduino PWM output pin 5; connect to IBT-2 pin 1 (RPWM)
int dir2 = 11; // Arduino PWM output pin 6; connect to IBT-2 pin 2 (LPWM)
////////konstanta Servo dan lebar pulsanya///////
#define CMUCAM4_PAN_SERVO 0
#define CMUCAM4_TILT_SERVO 1
#define CMUCAM4_MIN_PULSE_LENGTH 750
#define CMUCAM4_MAX_PULSE_LENGTH 2250

//////////konstanta P dan D untuk PID nya/////////
#define CMUCAM4_MIN_P_GAIN 0
#define CMUCAM4_MIN_D_GAIN 0
#define CMUCAM4_MAX_P_GAIN 1000
#define CMUCAM4_MAX_D_GAIN 1000

int servoX, servoY, nilaiServoX, nilaiServoY;

String dataTerima;

CMUcam4 cam(CMUCOM4_SERIAL1);
CMUcam4_tracking_data_t data;
//LiquidCrystal lcd(32, 30, 28, 26, 24, 22);
LiquidCrystal lcd(22, 24, 5, 2, 4, 3);
int detekbola = 0 ;
int detekgawang = 0 ;
int cw1 = 0;
int ccw1 = 0;
int putaran = 0 ;
int luas = 0 ;
int tunggu = 0 ;
float wait = 0 ;
int cmps_map = 0;
int cmps_int = 0;

void setup()
{
//Serial.begin(9600);
myservo.attach(36);
myservo.write(120);
// myservo.write(60);
// myservo.attach(3);
cam.begin();
Serial.begin(57600);
lcd.begin(8, 2);
// Wait for auto gain and auto white balance to run.

cam.LEDOn(LED_BLINK);

lcd.setCursor(0,0);
lcd.print(“prepare”);
Serial.println(“prepare”);

//pinoutput
pinMode(pwm1, OUTPUT);
pinMode(dir1, OUTPUT);
pinMode(pwm2, OUTPUT);
pinMode(dir2, OUTPUT);

pinMode(3,OUTPUT);

//maju();

delay(WAIT_TIME);

// Turn auto gain and auto white balance off.
cam.autoGainControl(false);
cam.autoWhiteBalance(false);
// cam.colorTracking(true); // Go to YUV mode! False foir RGB mode!
// cam.monitorSignal(false); // Operate in PAL mode. False for NTSC mode.

cam.LEDOn(CMUCAM4_LED_ON);

detekbola = 1;

// cam.automaticPan(true, false);
// cam.automaticTilt(true, false);
//cam.pollMode(true);

berhenti();

}

void loop()
{

lcd.clear();

if (detekgawang == 1 && detekbola == 0)
cam.trackColor(RED_MIN_K, RED_MAX_K, GREEN_MIN_K, GREEN_MAX_K, BLUE_MIN_K, BLUE_MAX_K);

if (detekgawang == 0 && detekbola == 1)
cam.trackColor(RED_MIN, RED_MAX, GREEN_MIN, GREEN_MAX, BLUE_MIN, BLUE_MAX);

if (detekbola == 1 && detekgawang == 0)
{
servoX = map(data.my,0,118,140,110);
myservo.write(servoX);
CariBola();
}

if (detekbola == 0 && detekgawang == 1)
{
myservo.write(160);
CariGawang();
}
if (detekbola == 0 && detekgawang == 0)
{
kompas_manual();
}
if (detekbola == 2 && detekgawang == 2)
{
cobalurus();
}

for(int x=0;x<=3;x++)
{
cam.getTypeTDataPacket(&data); // Get a tracking packet.
luas = (data.x2 – data.x1)*(data.y2 – data.y1);
// Process the packet data safely here.
Serial.print(data.mx);
Serial.print(“-“);
Serial.println(data.my);
Serial.print(“-“);
Serial.println(luas);

lcd.setCursor(0,0);
lcd.print(data.mx);
lcd.print(“+”);
lcd.print(data.my);

lcd.setCursor(3,1);
lcd.print(luas);

}
}

void berhenti(){
digitalWrite(pwm1, LOW);
analogWrite(dir1, 0);
digitalWrite(pwm2, HIGH);
analogWrite(dir2, 255);

lcd.setCursor(0,1);
lcd.print(“St”);

Serial.println(“Berhenti”);

}

void mundur(){
digitalWrite(pwm1, LOW);
analogWrite(dir1, 150); // Motor1 Mundur
digitalWrite(pwm2, LOW);
analogWrite(dir2, 150); // Motor2 Mundur

lcd.setCursor(0,1);
lcd.print(“Bk”);

Serial.println(“Mundur”);

}

void maju(){
digitalWrite(pwm1, HIGH);
analogWrite(dir1, 100); // Motor1 Maju
digitalWrite(pwm2, HIGH);
analogWrite(dir2, 100); // Motor2 Maju

lcd.setCursor(0,1);
lcd.print(“Go”);

Serial.println(“Maju”);

}

void putarCCW(){
digitalWrite(pwm1, LOW);
analogWrite(dir1, 170); // Motor1 Mundur
digitalWrite(pwm2, HIGH);
analogWrite(dir2, 90); // Motor2 Maju

lcd.setCursor(0,1);
lcd.print(“ki”);

Serial.println(“Kiri”);

}

void putarCW(){
digitalWrite(pwm1, HIGH);
analogWrite(dir1, 90); // Motor1 Maju
digitalWrite(pwm2, LOW);
analogWrite(dir2, 170); // Motor2 Mundur

lcd.setCursor(0,1);
lcd.print(“Ka”);

Serial.println(“Kanan”);

}

void kick()
{
digitalWrite(3,HIGH);
lcd.setCursor(0,1);
lcd.print(“Te “);

Serial.println(“Tendang”);
}

void CariBola()
{

if (data.mx <50 && data.mx !=0 ){
putarCCW();
} else
if (data.mx>110 && data.mx !=0 ){
putarCW();
} else
if (data.my<80 && data.my != 0 ){
maju();}
else
if (data.my>80 && data.my != 0 ){
// mundur();
carigawang();
Serial.println(“Cari Gawang”);
} else
{
berhenti();
}
}

void CariGawang()
{
if (data.mx < 65 && data.my != 0 && luas < 9000 ){
putarCCW();
}
else
if (data.mx >= 85 && data.my != 0 && luas < 9000){
putarCW();
}else
if (data.mx != 0 && data.my >= 30 && luas < 9000 ){
maju();}
else
if (data.mx != 0 && data.my < 30 && luas < 9000)
{
berhenti();
lcd.setCursor(0,1);
lcd.print(“te”);
}else
if (data.mx != 0 && data.my != 0 && luas > 9000)
{
berhenti();
lcd.setCursor(0,1);
lcd.print(“te”);
}
else
{
berhenti();
}

}
void caribola()
{
detekgawang = 0;
detekbola = 1;
delay(50);
}

void carigawang()
{
detekgawang = 1;
detekbola = 0;
delay(50);
}

void kompas()
{
lcd.setCursor(0,0);lcd.print(“DATA = “);lcd.print(cmps_int);lcd.print(” | “);lcd.print(cmps_map);

if (cmps_map > 110 && cmps_map < 255)
{
putarCCW();
}
else if (cmps_map < 93)
{
putarCCW();
}
else if (cmps_map > 93 & cmps_map < 110)
{
berhenti();
manual2();
}
else
berhenti();

}

void manual()
{
detekgawang = 0;
detekbola = 0;
delay(50);
}

void manual2()
{
detekgawang = 2;
detekbola = 2;
delay(50);
}

void cobalurus()
{
myservo.write(150);
delay(100);
maju();
delay(4000);
berhenti();
kick();
delay(1000);
tahan();
// carigawang();
caribola();

}

void tahan()
{
digitalWrite(3,LOW);
lcd.setCursor(0,1);
lcd.print(“tahan “);
}

void kompas_manual()
{
putarCCW();
delay(3000);
berhenti();
delay(200);
manual2();
}

Tinggalkan Balasan

Isikan data di bawah atau klik salah satu ikon untuk log in:

Logo WordPress.com

You are commenting using your WordPress.com account. Logout /  Ubah )

Foto Google

You are commenting using your Google account. Logout /  Ubah )

Gambar Twitter

You are commenting using your Twitter account. Logout /  Ubah )

Foto Facebook

You are commenting using your Facebook account. Logout /  Ubah )

Connecting to %s