main.cpp

00001 //
00002 // OV7670 + FIFO AL422B camera board test
00003 //
00004 #include "mbed.h"
00005 #include "ov7670.h"
00006 #include "motor.h"
00007 #include "ros.h"
00008 #include <ros/time.h>
00009 #include <std_msgs/UInt8MultiArray.h>
00010 #include <std_msgs/Byte.h>
00011 #include <std_msgs/Bool.h>
00012 #include <std_msgs/Float64.h>
00013 #include <std_msgs/Int32.h>
00014 #include <std_msgs/String.h>
00015 
00016 
00017 BusOut bus(PC_0,PC_1);
00018 MotorCtl motor1(PB_13,bus,PB_14,PB_15);
00019 InterruptIn tachoINT1(PB_14);
00020 InterruptIn tachoINT2(PB_15);
00021 
00022 
00023 void goalVelCb(const std_msgs::Float64& msg){
00024     motor1.setTarget(msg.data);
00025 }
00026 
00027 
00028 
00029 
00030 std_msgs::Float64 cum_dist;
00031 std_msgs::Float64 rela_dist;
00032 std_msgs::Float64 curr_vel;
00033 std_msgs::Int32 curr_rpm;
00034 
00035 std_msgs::UInt8MultiArray img;
00036 //std_msgs::Byte img;
00037 std_msgs::Bool sync;
00038 
00039 ros::NodeHandle nh_motor;
00040 ros::NodeHandle_cam nh_cam;
00041 ros::Publisher img_pub("image_data", &img);
00042 ros::Publisher sync_pub("sync", &sync);
00043 
00044 ros::Publisher cum_dist_pub("cum_dist", &cum_dist);
00045 ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
00046 ros::Publisher rpm_pub("rpm", &curr_rpm);
00047 ros::Publisher curr_vel_pub("curr_vel", &curr_vel);
00048 ros::Subscriber<std_msgs::Float64> goal_vel_sub("goal_vel", &goalVelCb);
00049 
00050 
00051 
00052 
00053 OV7670 camera(
00054     D14,D15,       // SDA,SCL(I2C / SCCB)
00055     D9,D8,D11,   // VSYNC,HREF,WEN(FIFO)
00056     //p18,p17,p16,p15,p11,p12,p14,p13, // D7-D0
00057     PortB,0x00FF,
00058     D13,D12,D6,D2) ; // RRST,OE,RCLK,WRST
00059 
00060 //Serial pc(USBTX,USBRX,115200) ;
00061 
00062 #define NUMBYTE 480
00063 
00064 //#undef QQVGA
00065 #define QQVGA
00066 
00067 #ifdef QQVGA
00068 # define SIZEX (160)
00069 # define SIZEY (120)
00070 
00071 #else
00072 # define SIZEX (320)
00073 # define SIZEY (240)
00074 #endif
00075 
00076 
00077 uint8_t fdata[SIZEX*SIZEY];
00078 uint8_t fdata_ros[NUMBYTE];
00079 int loop_num;
00080 size_t size = SIZEX*SIZEY;
00081 
00082 int index=0;
00083 volatile int flag;
00084 
00085 void update_current(void)
00086 {
00087     motor1.UpdateCurrentPosition();  
00088 }
00089 
00090 
00091 
00092 int main()
00093 {   
00094 
00095     int rpm;
00096     float velocity;
00097     float reladistance; //m단위
00098     float cumdistance;
00099     
00100     tachoINT1.fall(&update_current);
00101     tachoINT1.rise(&update_current);
00102     tachoINT2.fall(&update_current);
00103     tachoINT2.rise(&update_current);
00104     
00105     wait(1);
00106     motor1.setTarget(10);
00107     
00108     nh_cam.initNode();
00109     nh_motor.initNode();
00110     loop_num = size/NUMBYTE;
00111     sync.data=true;
00112     wait_ms(1);
00113 //    pc.baud(115200) ;
00114     camera.Reset() ;
00115     nh_cam.advertise(img_pub);
00116     nh_cam.advertise(sync_pub);
00117     
00118     nh_motor.advertise(cum_dist_pub);
00119     nh_motor.advertise(rela_dist_pub);
00120     nh_motor.advertise(rpm_pub);
00121     nh_motor.subscribe(goal_vel_sub);
00122 #ifdef QQVGA
00123 //    camera.InitQQVGA565(true,false) ;
00124     camera.InitQQVGAYUV(false,false);
00125 
00126 #else
00127     //camera.InitQVGA565(true,false) ;
00128 #endif
00129 
00130 
00131     // CAPTURE and SEND LOOP
00132 
00133     
00134     while(1) {
00135 
00136             
00137         camera.CaptureNext() ;
00138         while(camera.CaptureDone() == false) ;
00139 
00140         camera.ReadStart() ;
00141         
00142         //motor
00143         rpm = motor1.getRPM();
00144         cumdistance = motor1.CalculateCumDis(); //  누적거리
00145         reladistance = motor1.CalculateRelaDis(); // 상대거리
00146         velocity = motor1.CalculateVelocity();        
00147         //motor end
00148         sync.data=true;
00149         
00150         for (int y = 0; y < SIZEX*SIZEY ; y++) {
00151             camera.ReadOneByte();
00152             fdata[y]=camera.ReadOneByte();
00153                 
00154         }
00155         
00156 
00157         camera.ReadStop() ;
00158 
00159         for(int x=0; x< loop_num; x++) {
00160             for(int y=0; y < NUMBYTE; y++) {
00161                 fdata_ros[y]=fdata[(NUMBYTE*(x))+y];
00162             }
00163             
00164             sync_pub.publish(&sync);
00165             img.data_length = NUMBYTE;
00166             img.data = &fdata_ros[0];
00167             img_pub.publish(&img);
00168             
00169             //motor
00170             cum_dist.data = cumdistance;
00171             cum_dist_pub.publish(&cum_dist);
00172             
00173             rela_dist.data = reladistance;
00174             rela_dist_pub.publish(&rela_dist);
00175             
00176             curr_rpm.data = rpm;
00177             rpm_pub.publish(&curr_rpm);               
00178             //motor end
00179             
00180             nh_cam.spinOnce();
00181             nh_motor.spinOnce();
00182             sync.data=false;
00183             wait_us(4687.5); // 3125 us , 1843200 baudrate for camera
00184                            // 950 ~ 1000 us for motor
00185         }
00186 
00187        
00188     }
00189 }