Pipe Inpection Robot / Mbed OS capstone_finish

capstone_finish

Dependencies:   BufferedSerial motor_sn7544

main.cpp@8:221b2fc093e4, 2019-09-06 (annotated)

Committer:
kangmingyo
Date:
Fri Sep 06 02:09:38 2019 +0000
Revision:
8:221b2fc093e4
Parent:
7:152fba230106
Child:
9:ccd662244071
final

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