File content as of revision 10:ca4e4062701a:

//
// OV7670 + FIFO AL422B camera board test
//
#include "mbed.h"
#include "ov7670.h"
#include "motor.h"
#include "ros.h"
#include <ros/time.h>
#include <std_msgs/UInt8MultiArray.h>
#include <std_msgs/Byte.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>


BusOut bus(PC_0,PC_1);
MotorCtl motor1(PB_13,bus,PB_14,PB_15);
InterruptIn tachoINT1(PB_14);
InterruptIn tachoINT2(PB_15);


void goalVelCb(const std_msgs::Float64& msg){
    motor1.setTarget(msg.data);
}




std_msgs::Float64 cum_dist;
std_msgs::Float64 rela_dist;
std_msgs::Float64 curr_vel;
std_msgs::Int32 curr_rpm;

std_msgs::UInt8MultiArray img;
//std_msgs::Byte img;
std_msgs::Bool sync;

ros::NodeHandle nh_motor;
ros::NodeHandle_cam nh_cam;
ros::Publisher img_pub("image_data", &img);
ros::Publisher sync_pub("sync", &sync);

ros::Publisher cum_dist_pub("cum_dist", &cum_dist);
ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
ros::Publisher rpm_pub("rpm", &curr_rpm);
ros::Publisher curr_vel_pub("curr_vel", &curr_vel);
ros::Subscriber<std_msgs::Float64> goal_vel_sub("goal_vel", &goalVelCb);




OV7670 camera(
    D14,D15,       // SDA,SCL(I2C / SCCB)
    D9,D8,D11,   // VSYNC,HREF,WEN(FIFO)
    //p18,p17,p16,p15,p11,p12,p14,p13, // D7-D0
    PortB,0x00FF,
    D13,D12,D6,D2) ; // RRST,OE,RCLK,WRST

//Serial pc(USBTX,USBRX,115200) ;

#define NUMBYTE 480

//#undef QQVGA
#define QQVGA

#ifdef QQVGA
# define SIZEX (160)
# define SIZEY (120)

#else
# define SIZEX (320)
# define SIZEY (240)
#endif


uint8_t fdata[SIZEX*SIZEY];
uint8_t fdata_ros[NUMBYTE];
int loop_num;
size_t size = SIZEX*SIZEY;

int index=0;
volatile int flag;

void update_current(void)
{
    motor1.UpdateCurrentPosition();  
}



int main()
{   

    int rpm;
    float velocity;
    float reladistance; //m단위
    float cumdistance;
    
    tachoINT1.fall(&update_current);
    tachoINT1.rise(&update_current);
    tachoINT2.fall(&update_current);
    tachoINT2.rise(&update_current);
    
    wait(1);
    motor1.setTarget(10);
    
    nh_cam.initNode();
    nh_motor.initNode();
    loop_num = size/NUMBYTE;
    sync.data=true;
    wait_ms(1);
//    pc.baud(115200) ;
    camera.Reset() ;
    nh_cam.advertise(img_pub);
    nh_cam.advertise(sync_pub);
    
    nh_motor.advertise(cum_dist_pub);
    nh_motor.advertise(rela_dist_pub);
    nh_motor.advertise(rpm_pub);
    nh_motor.subscribe(goal_vel_sub);
#ifdef QQVGA
//    camera.InitQQVGA565(true,false) ;
    camera.InitQQVGAYUV(false,false);

#else
    //camera.InitQVGA565(true,false) ;
#endif


    // CAPTURE and SEND LOOP

    
    while(1) {

            
        camera.CaptureNext() ;
        while(camera.CaptureDone() == false) ;

        camera.ReadStart() ;
        
        //motor
        rpm = motor1.getRPM();
        cumdistance = motor1.CalculateCumDis(); //  누적거리
        reladistance = motor1.CalculateRelaDis(); // 상대거리
        velocity = motor1.CalculateVelocity();        
        //motor end
        sync.data=true;
        
        for (int y = 0; y < SIZEX*SIZEY ; y++) {
            camera.ReadOneByte();
            fdata[y]=camera.ReadOneByte();
                
        }
        

        camera.ReadStop() ;

        for(int x=0; x< loop_num; x++) {
            for(int y=0; y < NUMBYTE; y++) {
                fdata_ros[y]=fdata[(NUMBYTE*(x))+y];
            }
            
            sync_pub.publish(&sync);
            img.data_length = NUMBYTE;
            img.data = &fdata_ros[0];
            img_pub.publish(&img);
            
            //motor
            cum_dist.data = cumdistance;
            cum_dist_pub.publish(&cum_dist);
            
            rela_dist.data = reladistance;
            rela_dist_pub.publish(&rela_dist);
            
            curr_rpm.data = rpm;
            rpm_pub.publish(&curr_rpm);               
            //motor end
            
            nh_cam.spinOnce();
            nh_motor.spinOnce();
            sync.data=false;
            wait_us(4687.5); // 3125 us , 1843200 baudrate for camera
                           // 950 ~ 1000 us for motor
        }

       
    }
}