Pipe Inpection Robot / Mbed OS capstone_finish

capstone_finish

Dependencies:   BufferedSerial motor_sn7544

main.cpp@9:ccd662244071, 2019-11-21 (annotated)

Committer:
Jeonghoon
Date:
Thu Nov 21 10:36:11 2019 +0000
Revision:
9:ccd662244071
Parent:
8:221b2fc093e4
Child:
10:ca4e4062701a
complete ov7670;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mio 0:f3f80a0695ff 1 //
mio 0:f3f80a0695ff 2 // OV7670 + FIFO AL422B camera board test
mio 0:f3f80a0695ff 3 //
mio 0:f3f80a0695ff 4 #include "mbed.h"
mio 0:f3f80a0695ff 5 #include "ov7670.h"
Jeonghoon 3:2a3664dc6634 6 #include <ros.h>
Jeonghoon 3:2a3664dc6634 7 #include <ros/time.h>
Jeonghoon 3:2a3664dc6634 8 #include <std_msgs/UInt8MultiArray.h>
Jeonghoon 9:ccd662244071 9 #include <std_msgs/Byte.h>
kangmingyo 8:221b2fc093e4 10 #include <std_msgs/Bool.h>
kangmingyo 2:e98408458d2b 11
Jeonghoon 3:2a3664dc6634 12 std_msgs::UInt8MultiArray img;
Jeonghoon 9:ccd662244071 13 //std_msgs::Byte img;
kangmingyo 8:221b2fc093e4 14 std_msgs::Bool sync;
Jeonghoon 3:2a3664dc6634 15 ros::NodeHandle nh;
Jeonghoon 3:2a3664dc6634 16 ros::Publisher img_pub("image_data", &img);
kangmingyo 8:221b2fc093e4 17 ros::Publisher sync_pub("sync", &sync);
kangmingyo 2:e98408458d2b 18
mio 0:f3f80a0695ff 19
kangmingyo 7:152fba230106 20
mio 0:f3f80a0695ff 21 OV7670 camera(
kangmingyo 2:e98408458d2b 22 D14,D15, // SDA,SCL(I2C / SCCB)
kangmingyo 4:7b63cf3d205f 23 D9,D8,D11, // VSYNC,HREF,WEN(FIFO)
mio 0:f3f80a0695ff 24 //p18,p17,p16,p15,p11,p12,p14,p13, // D7-D0
kangmingyo 7:152fba230106 25 PortB,0x00FF,
kangmingyo 7:152fba230106 26 D13,D12,D6,D2) ; // RRST,OE,RCLK,WRST
mio 0:f3f80a0695ff 27
Jeonghoon 3:2a3664dc6634 28 //Serial pc(USBTX,USBRX,115200) ;
kangmingyo 7:152fba230106 29
Jeonghoon 9:ccd662244071 30 #define NUMBYTE 480
mio 0:f3f80a0695ff 31
kangmingyo 2:e98408458d2b 32 //#undef QQVGA
kangmingyo 2:e98408458d2b 33 #define QQVGA
mio 1:509676f3be32 34
mio 0:f3f80a0695ff 35 #ifdef QQVGA
kangmingyo 7:152fba230106 36 # define SIZEX (160)
kangmingyo 7:152fba230106 37 # define SIZEY (120)
Jeonghoon 9:ccd662244071 38
mio 0:f3f80a0695ff 39 #else
mio 0:f3f80a0695ff 40 # define SIZEX (320)
mio 0:f3f80a0695ff 41 # define SIZEY (240)
mio 0:f3f80a0695ff 42 #endif
mio 0:f3f80a0695ff 43
Jeonghoon 3:2a3664dc6634 44
Jeonghoon 3:2a3664dc6634 45 uint8_t fdata[SIZEX*SIZEY];
Jeonghoon 9:ccd662244071 46 uint8_t fdata_ros[NUMBYTE];
Jeonghoon 9:ccd662244071 47 int loop_num;
kangmingyo 8:221b2fc093e4 48
Jeonghoon 3:2a3664dc6634 49
Jeonghoon 3:2a3664dc6634 50 size_t size = SIZEX*SIZEY;
kangmingyo 2:e98408458d2b 51
kangmingyo 7:152fba230106 52 int main()
Jeonghoon 9:ccd662244071 53 {
Jeonghoon 9:ccd662244071 54 nh.initNode();
Jeonghoon 9:ccd662244071 55 loop_num = size/NUMBYTE;
Jeonghoon 9:ccd662244071 56 sync.data=true;
kangmingyo 7:152fba230106 57 wait_ms(1);
Jeonghoon 3:2a3664dc6634 58 // pc.baud(115200) ;
mio 0:f3f80a0695ff 59 camera.Reset() ;
Jeonghoon 3:2a3664dc6634 60 nh.advertise(img_pub);
kangmingyo 8:221b2fc093e4 61 nh.advertise(sync_pub);
mio 0:f3f80a0695ff 62
mio 0:f3f80a0695ff 63 #ifdef QQVGA
kangmingyo 2:e98408458d2b 64 // camera.InitQQVGA565(true,false) ;
kangmingyo 6:fe8b32cb9357 65 camera.InitQQVGAYUV(false,false);
kangmingyo 2:e98408458d2b 66
kangmingyo 7:152fba230106 67 #else
kangmingyo 2:e98408458d2b 68 //camera.InitQVGA565(true,false) ;
mio 0:f3f80a0695ff 69 #endif
kangmingyo 7:152fba230106 70
kangmingyo 7:152fba230106 71
mio 0:f3f80a0695ff 72 // CAPTURE and SEND LOOP
kangmingyo 7:152fba230106 73
kangmingyo 8:221b2fc093e4 74
kangmingyo 7:152fba230106 75 while(1) {
kangmingyo 7:152fba230106 76
kangmingyo 7:152fba230106 77
mio 0:f3f80a0695ff 78 camera.CaptureNext() ;
mio 0:f3f80a0695ff 79 while(camera.CaptureDone() == false) ;
Jeonghoon 3:2a3664dc6634 80
mio 0:f3f80a0695ff 81 camera.ReadStart() ;
kangmingyo 8:221b2fc093e4 82
Jeonghoon 9:ccd662244071 83 sync.data=true;
Jeonghoon 9:ccd662244071 84
Jeonghoon 3:2a3664dc6634 85 for (int y = 0; y < SIZEX*SIZEY ; y++) {
Jeonghoon 3:2a3664dc6634 86 camera.ReadOneByte();
Jeonghoon 3:2a3664dc6634 87 fdata[y]=camera.ReadOneByte();
Jeonghoon 9:ccd662244071 88
mio 0:f3f80a0695ff 89 }
Jeonghoon 9:ccd662244071 90
kangmingyo 8:221b2fc093e4 91
kangmingyo 7:152fba230106 92 camera.ReadStop() ;
Jeonghoon 9:ccd662244071 93
Jeonghoon 9:ccd662244071 94 for(int x=0; x< loop_num; x++) {
Jeonghoon 9:ccd662244071 95 for(int y=0; y < NUMBYTE; y++) {
Jeonghoon 9:ccd662244071 96 fdata_ros[y]=fdata[(NUMBYTE*(x))+y];
kangmingyo 7:152fba230106 97 }
Jeonghoon 9:ccd662244071 98
kangmingyo 8:221b2fc093e4 99 sync_pub.publish(&sync);
Jeonghoon 9:ccd662244071 100 img.data_length = NUMBYTE;
kangmingyo 7:152fba230106 101 img.data = &fdata_ros[0];
kangmingyo 7:152fba230106 102 img_pub.publish(&img);
kangmingyo 7:152fba230106 103 nh.spinOnce();
kangmingyo 8:221b2fc093e4 104
Jeonghoon 9:ccd662244071 105 sync.data=false;
Jeonghoon 9:ccd662244071 106 wait_us(3125); // 25000us 12500
Jeonghoon 9:ccd662244071 107
Jeonghoon 9:ccd662244071 108 }
kangmingyo 7:152fba230106 109
kangmingyo 7:152fba230106 110
Jeonghoon 3:2a3664dc6634 111 }
mio 0:f3f80a0695ff 112 }