Arduino Mega2560 code for two Cameras working simultaneously

MultipleCams

//******************************************************
// Connections : @Yaseen
// RX from MAX3232 to Arduino Serial 1 (TX 1)
// TX from MAX3232 to Arduino Serial 1 (RX 1)
// Vcc from camera to ARduino +5v
// Gnd from camera to Arduino Gnd
//*******************************************************
 
byte ZERO = 0x00;
byte incomingbyte;
long int j=0,k=0,count=0,i=0x0000;
uint8_t MH,ML;
boolean EndFlag=0;
 
void SendResetCmd()
{
    Serial1.write(0x56);
    Serial1.write(ZERO);
    Serial1.write(0x26);
    Serial1.write(ZERO);
}

void SendResetCmd2()
{
    Serial2.write(0x56);
    Serial2.write(ZERO);
    Serial2.write(0x26);
    Serial2.write(ZERO);
}
 
/*************************************/
/* Set ImageSize :
/* <1> 0x22 : 160*120
/* <2> 0x11 : 320*240
/* <3> 0x00 : 640*480
/* <4> 0x1D : 800*600
/* <5> 0x1C : 1024*768
/* <6> 0x1B : 1280*960
/* <7> 0x21 : 1600*1200
/************************************/
void SetImageSizeCmd(byte Size)
{
    Serial1.write(0x56);
    Serial1.write(ZERO);  
    Serial1.write(0x54);
    Serial1.write(0x01);
    Serial1.write(Size);
}
 
/*************************************/
/* Set BaudRate :
/* <1>¡¡0xAE  :   9600
/* <2>¡¡0x2A  :   38400
/* <3>¡¡0x1C  :   57600
/* <4>¡¡0x0D  :   115200
/* <5>¡¡0xAE  :   128000
/* <6>¡¡0x56  :   256000
/*************************************/
void SetBaudRateCmd(byte baudrate)
{
    Serial1.write(0x56);
    Serial1.write(ZERO);
    Serial1.write(0x24);
    Serial1.write(0x03);
    Serial1.write(0x01);
    Serial1.write(baudrate);
}
 
 void SetBaudRateCmd2(byte baudrate)
{
    Serial2.write(0x56);
    Serial2.write(ZERO);
    Serial2.write(0x24);
    Serial2.write(0x03);
    Serial2.write(0x01);
    Serial2.write(baudrate);
}
void SendTakePhotoCmd()
{
    Serial1.write(0x56);
    Serial1.write(ZERO);
    Serial1.write(0x36);
    Serial1.write(0x01);
    Serial1.write(ZERO); 
}
 
void SendTakePhotoCmd2()
{
    Serial2.write(0x56);
    Serial2.write(ZERO);
    Serial2.write(0x36);
    Serial2.write(0x01);
    Serial2.write(ZERO); 
}

void SendReadDataCmd()
{
    MH=i/0x100;
    ML=i%0x100;
    Serial1.write(0x56);
    Serial1.write(ZERO);
    Serial1.write(0x32);
    Serial1.write(0x0c);
    Serial1.write(ZERO);
    Serial1.write(0x0a);
    Serial1.write(ZERO);
    Serial1.write(ZERO);
    Serial1.write(MH);
    Serial1.write(ML);
    Serial1.write(ZERO);
    Serial1.write(ZERO);
    Serial1.write(ZERO);
    Serial1.write(0x20);
    Serial1.write(ZERO);
    Serial1.write(0x0a);
    i+=0x20;
}
 
void SendReadDataCmd2()
{
    MH=i/0x100;
    ML=i%0x100;
    Serial2.write(0x56);
    Serial2.write(ZERO);
    Serial2.write(0x32);
    Serial2.write(0x0c);
    Serial2.write(ZERO);
    Serial2.write(0x0a);
    Serial2.write(ZERO);
    Serial2.write(ZERO);
    Serial2.write(MH);
    Serial2.write(ML);
    Serial2.write(ZERO);
    Serial2.write(ZERO);
    Serial2.write(ZERO);
    Serial2.write(0x20);
    Serial2.write(ZERO);
    Serial2.write(0x0a);
    i+=0x20;
}
void StopTakePhotoCmd()
{
    Serial1.write(0x56);
    Serial1.write(ZERO);
    Serial1.write(0x36);
    Serial1.write(0x01);
    Serial1.write(0x03);
}
void StopTakePhotoCmd2()
{
    Serial2.write(0x56);
    Serial2.write(ZERO);
    Serial2.write(0x36);
    Serial2.write(0x01);
    Serial2.write(0x03);
}
 
void setup()
{
    Serial.begin(38400);
    while (!Serial)
    {
        ; // wait for Serial1 port to connect. Needed for Leonardo only
    }
 
    Serial.println("initialization done.");
    Serial.println("please waiting ....");
 
 
   Serial.println("Initializing Serial for Cam # 1");
   
    Serial1.begin(115200);
    delay(100);
    SendResetCmd();
    delay(2000);
    SetBaudRateCmd(0x2A);
    delay(500);
    // Serial1.begin(115200);
    Serial1.begin(38400);
    delay(100);
    
    Serial.println("Initializing Serial for Cam # 2");
    Serial2.begin(115200);
    delay(100);
    SendResetCmd2();
    delay(2000);
    SetBaudRateCmd2(0x2A);
    delay(500);
    // Serial1.begin(115200);
    Serial2.begin(38400);
    delay(100);
}
 
void DoCameraOneWork()
{
 byte a[32];
    int ii;
 
    SendResetCmd();
    delay(2000);                            //Wait 2-3 second to send take picture command
    SendTakePhotoCmd();
    delay(1000);
    while(Serial1.available()>0)
    {
        incomingbyte=Serial1.read();
    }
 
    //myFile = SD.open("pic.jpg", FILE_WRITE); //The file name should not be too long
   EndFlag = 0;
   i = 0x0000;
    while(!EndFlag)
    {  
        j=0;
        k=0;
        count=0;
        //Serial1.flush();
        SendReadDataCmd();
        delay(20);
        while(Serial1.available()>0)
        {
            incomingbyte=Serial1.read();
            k++;
            delay(1); //250 for regular
            if((k>5)&&(j<32)&&(!EndFlag))
            {
                a[j]=incomingbyte;
                if((a[j-1]==0xFF)&&(a[j]==0xD9))     //tell if the picture is finished
                {
                    EndFlag=1;
                    
                }
                j++;
                count++;
            }
        }
 
        for(j=0;j<count;j++)
        {
            if(a[j]<0x10)  Serial.print("0");
            Serial.print(a[j],HEX);           // observe the image through Serial1 port
            //Serial.print("");
        }
 
        //for(ii=0; ii<count; ii++)
        //myFile.write(a[ii]);
        Serial.println();
    }
 
    //myFile.close();
    Serial.print("Finished writing data to file");
    Serial.println("End of image"); 
    
    Serial.println("----------------------------------");
    Serial.println("End of CAMERA # 1");
}

void DoCameraTwoWork()
{
byte a[32];
    int ii;
 
    SendResetCmd2();
    delay(2000);                            //Wait 2-3 second to send take picture command
    SendTakePhotoCmd2();
    delay(1000);
    while(Serial2.available()>0)
    {
        incomingbyte=Serial2.read();
    }
 
    //myFile = SD.open("pic.jpg", FILE_WRITE); //The file name should not be too long
   EndFlag = 0;
   i = 0x0000;
    while(!EndFlag)
    {
        j=0;
        k=0;
        count=0;
        //Serial1.flush();
        SendReadDataCmd2();
        delay(20);
        while(Serial2.available()>0)
        {
            incomingbyte=Serial2.read();
            k++;
            delay(1); //250 for regular
            if((k>5)&&(j<32)&&(!EndFlag))
            {
                a[j]=incomingbyte;
                if((a[j-1]==0xFF)&&(a[j]==0xD9))     //tell if the picture is finished
                {
                    EndFlag=1;
                    
                }
                j++;
                count++;
            }
        }
 
        for(j=0;j<count;j++)
        {
            if(a[j]<0x10)  Serial.print("0");
            Serial.print(a[j],HEX);           // observe the image through Serial1 port
            //Serial.print("");
        }
 
        Serial.println();
    }
 
    //myFile.close();
    Serial.print("Finished writing data to file");
    Serial.println("End of image");
    
        Serial.println("----------------------------------");
    Serial.println("End of CAMERA # 2");
}
void loop()
{
   Serial.println("Starting cam 2 work");
   DoCameraTwoWork();
   
    delay(4000);
    
    Serial.println("Starting cam 1 work");
   DoCameraOneWork(); 
   
   while(1);
}