Pipe Inpection Robot / Mbed OS capstone_finish

capstone_finish

Dependencies:   BufferedSerial motor_sn7544

main.cpp@10:ca4e4062701a, 2019-11-26 (annotated)

Committer:
Jeonghoon
Date:
Tue Nov 26 15:30:27 2019 +0000
Revision:
10:ca4e4062701a
Parent:
9:ccd662244071 
capstone finish

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 10:ca4e4062701a 6 #include "motor.h"
Jeonghoon 10:ca4e4062701a 7 #include "ros.h"
Jeonghoon 3:2a3664dc6634 8 #include <ros/time.h>
Jeonghoon 3:2a3664dc6634 9 #include <std_msgs/UInt8MultiArray.h>
Jeonghoon 9:ccd662244071 10 #include <std_msgs/Byte.h>
kangmingyo 8:221b2fc093e4 11 #include <std_msgs/Bool.h>
Jeonghoon 10:ca4e4062701a 12 #include <std_msgs/Float64.h>
Jeonghoon 10:ca4e4062701a 13 #include <std_msgs/Int32.h>
Jeonghoon 10:ca4e4062701a 14 #include <std_msgs/String.h>
Jeonghoon 10:ca4e4062701a 15
Jeonghoon 10:ca4e4062701a 16
Jeonghoon 10:ca4e4062701a 17 BusOut bus(PC_0,PC_1);
Jeonghoon 10:ca4e4062701a 18 MotorCtl motor1(PB_13,bus,PB_14,PB_15);
Jeonghoon 10:ca4e4062701a 19 InterruptIn tachoINT1(PB_14);
Jeonghoon 10:ca4e4062701a 20 InterruptIn tachoINT2(PB_15);
Jeonghoon 10:ca4e4062701a 21
Jeonghoon 10:ca4e4062701a 22
Jeonghoon 10:ca4e4062701a 23 void goalVelCb(const std_msgs::Float64& msg){
Jeonghoon 10:ca4e4062701a 24 motor1.setTarget(msg.data);
Jeonghoon 10:ca4e4062701a 25 }
Jeonghoon 10:ca4e4062701a 26
Jeonghoon 10:ca4e4062701a 27
Jeonghoon 10:ca4e4062701a 28
Jeonghoon 10:ca4e4062701a 29
Jeonghoon 10:ca4e4062701a 30 std_msgs::Float64 cum_dist;
Jeonghoon 10:ca4e4062701a 31 std_msgs::Float64 rela_dist;
Jeonghoon 10:ca4e4062701a 32 std_msgs::Float64 curr_vel;
Jeonghoon 10:ca4e4062701a 33 std_msgs::Int32 curr_rpm;
kangmingyo 2:e98408458d2b 34
Jeonghoon 3:2a3664dc6634 35 std_msgs::UInt8MultiArray img;
Jeonghoon 9:ccd662244071 36 //std_msgs::Byte img;
kangmingyo 8:221b2fc093e4 37 std_msgs::Bool sync;
Jeonghoon 10:ca4e4062701a 38
Jeonghoon 10:ca4e4062701a 39 ros::NodeHandle nh_motor;
Jeonghoon 10:ca4e4062701a 40 ros::NodeHandle_cam nh_cam;
Jeonghoon 3:2a3664dc6634 41 ros::Publisher img_pub("image_data", &img);
kangmingyo 8:221b2fc093e4 42 ros::Publisher sync_pub("sync", &sync);
kangmingyo 2:e98408458d2b 43
Jeonghoon 10:ca4e4062701a 44 ros::Publisher cum_dist_pub("cum_dist", &cum_dist);
Jeonghoon 10:ca4e4062701a 45 ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
Jeonghoon 10:ca4e4062701a 46 ros::Publisher rpm_pub("rpm", &curr_rpm);
Jeonghoon 10:ca4e4062701a 47 ros::Publisher curr_vel_pub("curr_vel", &curr_vel);
Jeonghoon 10:ca4e4062701a 48 ros::Subscriber<std_msgs::Float64> goal_vel_sub("goal_vel", &goalVelCb);
Jeonghoon 10:ca4e4062701a 49
Jeonghoon 10:ca4e4062701a 50
mio 0:f3f80a0695ff 51
kangmingyo 7:152fba230106 52
mio 0:f3f80a0695ff 53 OV7670 camera(
kangmingyo 2:e98408458d2b 54 D14,D15, // SDA,SCL(I2C / SCCB)
kangmingyo 4:7b63cf3d205f 55 D9,D8,D11, // VSYNC,HREF,WEN(FIFO)
mio 0:f3f80a0695ff 56 //p18,p17,p16,p15,p11,p12,p14,p13, // D7-D0
kangmingyo 7:152fba230106 57 PortB,0x00FF,
kangmingyo 7:152fba230106 58 D13,D12,D6,D2) ; // RRST,OE,RCLK,WRST
mio 0:f3f80a0695ff 59
Jeonghoon 3:2a3664dc6634 60 //Serial pc(USBTX,USBRX,115200) ;
kangmingyo 7:152fba230106 61
Jeonghoon 9:ccd662244071 62 #define NUMBYTE 480
mio 0:f3f80a0695ff 63
kangmingyo 2:e98408458d2b 64 //#undef QQVGA
kangmingyo 2:e98408458d2b 65 #define QQVGA
mio 1:509676f3be32 66
mio 0:f3f80a0695ff 67 #ifdef QQVGA
kangmingyo 7:152fba230106 68 # define SIZEX (160)
kangmingyo 7:152fba230106 69 # define SIZEY (120)
Jeonghoon 9:ccd662244071 70
mio 0:f3f80a0695ff 71 #else
mio 0:f3f80a0695ff 72 # define SIZEX (320)
mio 0:f3f80a0695ff 73 # define SIZEY (240)
mio 0:f3f80a0695ff 74 #endif
mio 0:f3f80a0695ff 75
Jeonghoon 3:2a3664dc6634 76
Jeonghoon 3:2a3664dc6634 77 uint8_t fdata[SIZEX*SIZEY];
Jeonghoon 9:ccd662244071 78 uint8_t fdata_ros[NUMBYTE];
Jeonghoon 9:ccd662244071 79 int loop_num;
Jeonghoon 10:ca4e4062701a 80 size_t size = SIZEX*SIZEY;
Jeonghoon 10:ca4e4062701a 81
Jeonghoon 10:ca4e4062701a 82 int index=0;
Jeonghoon 10:ca4e4062701a 83 volatile int flag;
Jeonghoon 10:ca4e4062701a 84
Jeonghoon 10:ca4e4062701a 85 void update_current(void)
Jeonghoon 10:ca4e4062701a 86 {
Jeonghoon 10:ca4e4062701a 87 motor1.UpdateCurrentPosition();
Jeonghoon 10:ca4e4062701a 88 }
kangmingyo 8:221b2fc093e4 89
Jeonghoon 3:2a3664dc6634 90
kangmingyo 2:e98408458d2b 91
kangmingyo 7:152fba230106 92 int main()
Jeonghoon 9:ccd662244071 93 {
Jeonghoon 10:ca4e4062701a 94
Jeonghoon 10:ca4e4062701a 95 int rpm;
Jeonghoon 10:ca4e4062701a 96 float velocity;
Jeonghoon 10:ca4e4062701a 97 float reladistance; //m단위
Jeonghoon 10:ca4e4062701a 98 float cumdistance;
Jeonghoon 10:ca4e4062701a 99
Jeonghoon 10:ca4e4062701a 100 tachoINT1.fall(&update_current);
Jeonghoon 10:ca4e4062701a 101 tachoINT1.rise(&update_current);
Jeonghoon 10:ca4e4062701a 102 tachoINT2.fall(&update_current);
Jeonghoon 10:ca4e4062701a 103 tachoINT2.rise(&update_current);
Jeonghoon 10:ca4e4062701a 104
Jeonghoon 10:ca4e4062701a 105 wait(1);
Jeonghoon 10:ca4e4062701a 106 motor1.setTarget(10);
Jeonghoon 10:ca4e4062701a 107
Jeonghoon 10:ca4e4062701a 108 nh_cam.initNode();
Jeonghoon 10:ca4e4062701a 109 nh_motor.initNode();
Jeonghoon 9:ccd662244071 110 loop_num = size/NUMBYTE;
Jeonghoon 9:ccd662244071 111 sync.data=true;
kangmingyo 7:152fba230106 112 wait_ms(1);
Jeonghoon 3:2a3664dc6634 113 // pc.baud(115200) ;
mio 0:f3f80a0695ff 114 camera.Reset() ;
Jeonghoon 10:ca4e4062701a 115 nh_cam.advertise(img_pub);
Jeonghoon 10:ca4e4062701a 116 nh_cam.advertise(sync_pub);
Jeonghoon 10:ca4e4062701a 117
Jeonghoon 10:ca4e4062701a 118 nh_motor.advertise(cum_dist_pub);
Jeonghoon 10:ca4e4062701a 119 nh_motor.advertise(rela_dist_pub);
Jeonghoon 10:ca4e4062701a 120 nh_motor.advertise(rpm_pub);
Jeonghoon 10:ca4e4062701a 121 nh_motor.subscribe(goal_vel_sub);
mio 0:f3f80a0695ff 122 #ifdef QQVGA
kangmingyo 2:e98408458d2b 123 // camera.InitQQVGA565(true,false) ;
kangmingyo 6:fe8b32cb9357 124 camera.InitQQVGAYUV(false,false);
kangmingyo 2:e98408458d2b 125
kangmingyo 7:152fba230106 126 #else
kangmingyo 2:e98408458d2b 127 //camera.InitQVGA565(true,false) ;
mio 0:f3f80a0695ff 128 #endif
kangmingyo 7:152fba230106 129
kangmingyo 7:152fba230106 130
mio 0:f3f80a0695ff 131 // CAPTURE and SEND LOOP
kangmingyo 7:152fba230106 132
kangmingyo 8:221b2fc093e4 133
kangmingyo 7:152fba230106 134 while(1) {
kangmingyo 7:152fba230106 135
kangmingyo 7:152fba230106 136
mio 0:f3f80a0695ff 137 camera.CaptureNext() ;
mio 0:f3f80a0695ff 138 while(camera.CaptureDone() == false) ;
Jeonghoon 3:2a3664dc6634 139
mio 0:f3f80a0695ff 140 camera.ReadStart() ;
kangmingyo 8:221b2fc093e4 141
Jeonghoon 10:ca4e4062701a 142 //motor
Jeonghoon 10:ca4e4062701a 143 rpm = motor1.getRPM();
Jeonghoon 10:ca4e4062701a 144 cumdistance = motor1.CalculateCumDis(); // 누적거리
Jeonghoon 10:ca4e4062701a 145 reladistance = motor1.CalculateRelaDis(); // 상대거리
Jeonghoon 10:ca4e4062701a 146 velocity = motor1.CalculateVelocity();
Jeonghoon 10:ca4e4062701a 147 //motor end
Jeonghoon 9:ccd662244071 148 sync.data=true;
Jeonghoon 9:ccd662244071 149
Jeonghoon 3:2a3664dc6634 150 for (int y = 0; y < SIZEX*SIZEY ; y++) {
Jeonghoon 3:2a3664dc6634 151 camera.ReadOneByte();
Jeonghoon 3:2a3664dc6634 152 fdata[y]=camera.ReadOneByte();
Jeonghoon 9:ccd662244071 153
mio 0:f3f80a0695ff 154 }
Jeonghoon 9:ccd662244071 155
kangmingyo 8:221b2fc093e4 156
kangmingyo 7:152fba230106 157 camera.ReadStop() ;
Jeonghoon 9:ccd662244071 158
Jeonghoon 9:ccd662244071 159 for(int x=0; x< loop_num; x++) {
Jeonghoon 9:ccd662244071 160 for(int y=0; y < NUMBYTE; y++) {
Jeonghoon 9:ccd662244071 161 fdata_ros[y]=fdata[(NUMBYTE*(x))+y];
kangmingyo 7:152fba230106 162 }
Jeonghoon 9:ccd662244071 163
kangmingyo 8:221b2fc093e4 164 sync_pub.publish(&sync);
Jeonghoon 9:ccd662244071 165 img.data_length = NUMBYTE;
kangmingyo 7:152fba230106 166 img.data = &fdata_ros[0];
kangmingyo 7:152fba230106 167 img_pub.publish(&img);
Jeonghoon 10:ca4e4062701a 168
Jeonghoon 10:ca4e4062701a 169 //motor
Jeonghoon 10:ca4e4062701a 170 cum_dist.data = cumdistance;
Jeonghoon 10:ca4e4062701a 171 cum_dist_pub.publish(&cum_dist);
kangmingyo 8:221b2fc093e4 172
Jeonghoon 10:ca4e4062701a 173 rela_dist.data = reladistance;
Jeonghoon 10:ca4e4062701a 174 rela_dist_pub.publish(&rela_dist);
Jeonghoon 10:ca4e4062701a 175
Jeonghoon 10:ca4e4062701a 176 curr_rpm.data = rpm;
Jeonghoon 10:ca4e4062701a 177 rpm_pub.publish(&curr_rpm);
Jeonghoon 10:ca4e4062701a 178 //motor end
Jeonghoon 10:ca4e4062701a 179
Jeonghoon 10:ca4e4062701a 180 nh_cam.spinOnce();
Jeonghoon 10:ca4e4062701a 181 nh_motor.spinOnce();
Jeonghoon 9:ccd662244071 182 sync.data=false;
Jeonghoon 10:ca4e4062701a 183 wait_us(4687.5); // 3125 us , 1843200 baudrate for camera
Jeonghoon 10:ca4e4062701a 184 // 950 ~ 1000 us for motor
Jeonghoon 9:ccd662244071 185 }
kangmingyo 7:152fba230106 186
kangmingyo 7:152fba230106 187
Jeonghoon 3:2a3664dc6634 188 }
mio 0:f3f80a0695ff 189 }