//This successfully communicates with the OV7670, based on measurements of output clocks.
		
#define Nop() asm( "nop" )
void i2c_wait(unsigned int cnt)
{
    while(--cnt)
    {
        Nop();
        Nop();
    }
}
		
	i2c_wait(2000000);
    StartI2C1();//Send the Start Bit
    // PCLK divided by 16, enable DCW, enable manually scaling
    IdleI2C1();//Wait to complete
    MasterWriteI2C1(0x42); //Write address
    IdleI2C1();
    MasterWriteI2C1(0x11); //Write reg
    IdleI2C1();
    MasterWriteI2C1(0x3F); //Write value
    StopI2C1();
    
    i2c_wait(2000000);   
    StartI2C1();
    // Set RGB
    IdleI2C1();//Wait to complete
    MasterWriteI2C1(0x42); //Write address
    IdleI2C1();
    MasterWriteI2C1(0x12); //Write reg
    IdleI2C1();
    MasterWriteI2C1(0x14); //Write value
    StopI2C1();
    
    i2c_wait(2000000);   
    StartI2C1();
    // RGB565 format
    IdleI2C1();//Wait to complete
    MasterWriteI2C1(0x42); //Write address
    IdleI2C1();
    MasterWriteI2C1(0x40); //Write reg
    IdleI2C1();
    MasterWriteI2C1(0xD0); //Write value