void setup()
{
    uint8_t vid,pid;
    uint8_t temp;
    wiring_init();
    arducam(smOV5642,CAM1_CS,-1,-1,-1);
     // Check if the ArduCAM SPI bus is OK
    arducam_write_reg(ARDUCHIP_TEST1, 0x55, CAM1_CS);
    temp = arducam_read_reg(ARDUCHIP_TEST1, CAM1_CS);
    //printf("temp=%x\n",temp);  //  debug
    if(temp != 0x55) {
        printf("SPI interface error!\n");
        //exit(EXIT_FAILURE);
    }
     else{
    	   printf("SPI interface OK!\n");
    	}
    	
    // Change MCU mode
    arducam_write_reg(ARDUCHIP_MODE, 0x00, CAM1_CS);
    
    // Check if the camera module type is OV5642
    arducam_i2c_word_read(OV5642_CHIPID_HIGH, &vid);
    arducam_i2c_word_read(OV5642_CHIPID_LOW, &pid);
    if((vid != 0x56) || (pid != 0x42)) {
        printf("Can't find OV5642 module!\n");
        exit(EXIT_FAILURE);
    } else {
        printf("OV5642 detected\n");
    }
}
void setup()
{
  uint8_t vid,pid;
  uint8_t temp; 

  UTFT();
  arducam(smOV5642);
  printf("ArduCAM Start!\n");



  //Check if the ArduCAM SPI bus is OK
  arducam_write_reg(ARDUCHIP_TEST1, 0x55);
  temp = arducam_read_reg(ARDUCHIP_TEST1);
  if(temp != 0x55)
  {
  	printf("SPI interface Error!\n");
  	exit(EXIT_FAILURE);
  }
  
  //Change MCU mode
  arducam_write_reg(ARDUCHIP_MODE, 0x00);

  InitLCD();
  
  //Check if the camera module type is OV5642
  arducam_i2c_word_read(OV5642_CHIPID_HIGH, &vid);
  arducam_i2c_word_read(OV5642_CHIPID_LOW, &pid);
  if((vid != 0x56) || (pid != 0x42)) {
  	printf("Can't find OV5642 module!\n");
  	exit(EXIT_FAILURE);
  } else {
  	printf("OV5642 detected\n");
  }
  	
  //Change to BMP capture mode and initialize the OV5642 module	  	
  arducam_set_format(fmtBMP);

  arducam_init();
}