Arduino-hoz OV7670 kamerával program?

Van programom, de valamiért nem fut. Arduino Mega-m van.

A program ez:

int data_p[] = {0,1,2,3,4,5,6,7};

int xclock_p = 8;

int pclock_p = 9;

int href_p = 10;

int vsynch_p = 11;

int lastPclock = 0;

int pclock;

int href;

int vsynch;

byte pixel;

void setup() {

Serial.begin(9600);

Serial.println("started");

for (int i = 0; i < 8; i++) {

pinMode(data_p[i], INPUT);

}

pinMode(xclock_p, OUTPUT);

pinMode(pclock_p, INPUT);

pinMode(href_p, INPUT);

pinMode(vsynch_p, INPUT);

}

int control = 0;

void loop() {

if (control >= 307200) {

return;

}

digitalWrite(xclock_p, HIGH);

vsynch = digitalRead(vsynch_p);

href = digitalRead(href_p);

pclock = digitalRead(pclock_p);

if (vsynch == HIGH) return;

if (href == LOW) return;

if (!(lastPclock == HIGH && pclock == LOW)) {

lastPclock = pclock;

return;

} else {

lastPclock = pclock;

}

pixel = readByteFromDataInput();

char s[10];

sprintf(s, "%d ", pixel);

Serial.println(s);

if (control % 640 == 0) {

Serial.println("");

}

control++;

if (control >= 307200) {

Serial.println("finished");

}

}

byte readByteFromDataInput() {

byte result = 0;

for (int i = 0; i < 8; i++) {

result = result << 1 | digitalRead(data_p[i]);

}

return result;

}