Pipe Inpection Robot / Mbed OS capstone_finish

capstone_finish

Dependencies:   BufferedSerial motor_sn7544

main.cpp@3:2a3664dc6634, 2019-08-13 (annotated)

Committer:
Jeonghoon
Date:
Tue Aug 13 05:53:22 2019 +0000
Revision:
3:2a3664dc6634
Parent:
2:e98408458d2b
Child:
4:7b63cf3d205f
with ROS;

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