Pipe Inpection Robot / Mbed OS GPG_motor_latest

complete motor

Dependencies:   BufferedSerial motor_sn7544

main.cpp@12:da4fb0d541ca, 2019-08-06 (annotated)

Committer:
kangmingyo
Date:
Tue Aug 06 08:33:42 2019 +0000
Revision:
12:da4fb0d541ca
Parent:
11:2228e8931266
Child:
13:3ac8d2472417
hh; ]; ; ; ; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kangmingyo 0:dde6e4d8c301 1 #include "mbed.h"
kangmingyo 1:1d18a2e8ce45 2 #include "motor.h"
Jeonghoon 7:13dd93a0efe8 3 #include <ros.h>
Jeonghoon 7:13dd93a0efe8 4 #include <std_msgs/Float64.h>
Jeonghoon 7:13dd93a0efe8 5 #include <std_msgs/Int32.h>
Jeonghoon 8:efe5a5b1f10a 6 #include <std_msgs/String.h>
Jeonghoon 11:2228e8931266 7 #include <geometry_msgs/Point.h>
Jeonghoon 7:13dd93a0efe8 8
Jeonghoon 7:13dd93a0efe8 9 std_msgs::Float64 cum_dist;
Jeonghoon 8:efe5a5b1f10a 10 std_msgs::Float64 rela_dist;
Jeonghoon 8:efe5a5b1f10a 11 std_msgs::Float64 flaw_loca;
Jeonghoon 11:2228e8931266 12 std_msgs::Float64 ultra_reflection;
Jeonghoon 11:2228e8931266 13 std_msgs::Float64 curr_vel;
Jeonghoon 7:13dd93a0efe8 14 std_msgs::Int32 curr_rpm;
Jeonghoon 11:2228e8931266 15
Jeonghoon 11:2228e8931266 16
Jeonghoon 11:2228e8931266 17 geometry_msgs::Point flaw_info;
Jeonghoon 7:13dd93a0efe8 18
Jeonghoon 7:13dd93a0efe8 19
Jeonghoon 11:2228e8931266 20 ros::NodeHandle nh;
Jeonghoon 11:2228e8931266 21
Jeonghoon 7:13dd93a0efe8 22 ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
Jeonghoon 7:13dd93a0efe8 23 ros::Publisher rpm_pub("rpm", &curr_rpm);
Jeonghoon 11:2228e8931266 24 ros::Publisher ultra_reflection_pub("ultra_reflection", &ultra_reflection);
Jeonghoon 11:2228e8931266 25 ros::Publisher flaw_info_pub("flaw_info", &flaw_info);
Jeonghoon 11:2228e8931266 26 ros::Publisher curr_vel_pub("curr_vel", &curr_vel);
kangmingyo 0:dde6e4d8c301 27
kangmingyo 12:da4fb0d541ca 28 BusOut bus(D11,D12);
kangmingyo 12:da4fb0d541ca 29 MotorCtl motor1(D3,bus,D4,D5);
kangmingyo 1:1d18a2e8ce45 30 InterruptIn tachoINT1(D4);
kangmingyo 1:1d18a2e8ce45 31 InterruptIn tachoINT2(D5);
Jeonghoon 8:efe5a5b1f10a 32 //Serial pc(USBTX,USBRX,115200);
Jeonghoon 7:13dd93a0efe8 33
kangmingyo 2:a04440f0d001 34 char rx_buffer[10];
kangmingyo 2:a04440f0d001 35 int index=0;
kangmingyo 2:a04440f0d001 36 volatile int flag;
kangmingyo 2:a04440f0d001 37 void update_current(void)
kangmingyo 2:a04440f0d001 38 {
kangmingyo 1:1d18a2e8ce45 39 motor1.UpdateCurrentPosition();
kangmingyo 2:a04440f0d001 40 // pc.printf("Update Position\r\n");
kangmingyo 2:a04440f0d001 41 }
Jeonghoon 8:efe5a5b1f10a 42 //void rx_cb(void)
Jeonghoon 8:efe5a5b1f10a 43 //{
Jeonghoon 8:efe5a5b1f10a 44 // char ch;
Jeonghoon 8:efe5a5b1f10a 45 // ch = pc.getc();
Jeonghoon 8:efe5a5b1f10a 46 // pc.putc(ch);
Jeonghoon 8:efe5a5b1f10a 47 // rx_buffer[index++]=ch;
Jeonghoon 8:efe5a5b1f10a 48 // if(ch==0x0D) {
Jeonghoon 8:efe5a5b1f10a 49 // pc.putc(0x0A);
Jeonghoon 8:efe5a5b1f10a 50 // rx_buffer[--index]='\0';
Jeonghoon 8:efe5a5b1f10a 51 // index=0;
Jeonghoon 8:efe5a5b1f10a 52 // flag=1;
Jeonghoon 8:efe5a5b1f10a 53 // }
Jeonghoon 8:efe5a5b1f10a 54 //}
Jeonghoon 8:efe5a5b1f10a 55 //
Jeonghoon 8:efe5a5b1f10a 56 //void set_speed(void)
Jeonghoon 8:efe5a5b1f10a 57 //{
Jeonghoon 8:efe5a5b1f10a 58 // int speed;
Jeonghoon 8:efe5a5b1f10a 59 // speed = atoi((const char*)rx_buffer);
Jeonghoon 8:efe5a5b1f10a 60 // if(speed>78) {
Jeonghoon 8:efe5a5b1f10a 61 // speed=78;
Jeonghoon 8:efe5a5b1f10a 62 // }
Jeonghoon 8:efe5a5b1f10a 63 // if(speed<-78) {
Jeonghoon 8:efe5a5b1f10a 64 // speed=-78;
Jeonghoon 8:efe5a5b1f10a 65 // }
Jeonghoon 8:efe5a5b1f10a 66 // motor1.setTarget(speed);
Jeonghoon 8:efe5a5b1f10a 67 // motor1.setTarget(60);
Jeonghoon 8:efe5a5b1f10a 68 // pc.printf(" Set speed = %d\r\n", speed);
Jeonghoon 8:efe5a5b1f10a 69 //
Jeonghoon 8:efe5a5b1f10a 70 //}
kangmingyo 0:dde6e4d8c301 71
kangmingyo 2:a04440f0d001 72 int main()
kangmingyo 2:a04440f0d001 73 {
Jeonghoon 7:13dd93a0efe8 74 nh.initNode();
Jeonghoon 11:2228e8931266 75
Jeonghoon 7:13dd93a0efe8 76 nh.advertise(rela_dist_pub);
Jeonghoon 7:13dd93a0efe8 77 nh.advertise(rpm_pub);
Jeonghoon 11:2228e8931266 78 nh.advertise(ultra_reflection_pub);
Jeonghoon 11:2228e8931266 79 nh.advertise(flaw_info_pub);
Jeonghoon 11:2228e8931266 80 nh.advertise(curr_vel_pub);
Jeonghoon 7:13dd93a0efe8 81
kangmingyo 1:1d18a2e8ce45 82 tachoINT1.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 83 tachoINT1.rise(&update_current);
kangmingyo 1:1d18a2e8ce45 84 tachoINT2.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 85 tachoINT2.rise(&update_current);
Jeonghoon 8:efe5a5b1f10a 86 // pc.attach(callback(rx_cb));
kangmingyo 2:a04440f0d001 87 int rpm;
Jeonghoon 11:2228e8931266 88 float velocity;
kangmingyo 4:a72f75611198 89 float reladistance; //m단위
kangmingyo 4:a72f75611198 90 float cumdistance;
Jeonghoon 11:2228e8931266 91 float ultra_reflect = 0;
Jeonghoon 8:efe5a5b1f10a 92 float flaw_location = 0;
Jeonghoon 8:efe5a5b1f10a 93 // char flaw_curr_state[10] = "stable";
Jeonghoon 8:efe5a5b1f10a 94
Jeonghoon 8:efe5a5b1f10a 95 motor1.setTarget(60);
kangmingyo 2:a04440f0d001 96 while(1) {
kangmingyo 2:a04440f0d001 97 flag=0;
kangmingyo 5:e6f9768e1438 98
Jeonghoon 8:efe5a5b1f10a 99 // pc.printf("Enter the value for speed [-78,78]\r\n");
kangmingyo 2:a04440f0d001 100 while(flag!=1) {
Jeonghoon 11:2228e8931266 101
kangmingyo 4:a72f75611198 102 rpm = motor1.getRPM();
kangmingyo 4:a72f75611198 103 cumdistance = motor1.CalculateCumDis(); // 누적거리
kangmingyo 4:a72f75611198 104 reladistance = motor1.CalculateRelaDis(); // 상대거리
Jeonghoon 11:2228e8931266 105 velocity = motor1.CalculateVelocity();
Jeonghoon 11:2228e8931266 106
Jeonghoon 11:2228e8931266 107 // cum_dist.data = cumdistance;
Jeonghoon 11:2228e8931266 108 // cum_dist_pub.publish(&cum_dist);
Jeonghoon 7:13dd93a0efe8 109
Jeonghoon 7:13dd93a0efe8 110 rela_dist.data = reladistance;
Jeonghoon 7:13dd93a0efe8 111 rela_dist_pub.publish(&rela_dist);
Jeonghoon 7:13dd93a0efe8 112
Jeonghoon 7:13dd93a0efe8 113 curr_rpm.data = rpm;
Jeonghoon 11:2228e8931266 114 rpm_pub.publish(&curr_rpm);
Jeonghoon 11:2228e8931266 115
Jeonghoon 11:2228e8931266 116 curr_vel.data = velocity;
Jeonghoon 11:2228e8931266 117 curr_vel_pub.publish(&curr_vel);
Jeonghoon 7:13dd93a0efe8 118
Jeonghoon 11:2228e8931266 119 ultra_reflection.data = ultra_reflect;
Jeonghoon 11:2228e8931266 120 ultra_reflection_pub.publish(&ultra_reflection);
Jeonghoon 11:2228e8931266 121
Jeonghoon 11:2228e8931266 122 flaw_info.x = cumdistance;
Jeonghoon 11:2228e8931266 123 flaw_info.y = flaw_location;
Jeonghoon 11:2228e8931266 124 flaw_info_pub.publish(&flaw_info);
Jeonghoon 8:efe5a5b1f10a 125
Jeonghoon 11:2228e8931266 126 flaw_location += 0.05;
Jeonghoon 8:efe5a5b1f10a 127 if(flaw_location > 1){
Jeonghoon 8:efe5a5b1f10a 128 flaw_location = 0;
Jeonghoon 8:efe5a5b1f10a 129 }
Jeonghoon 11:2228e8931266 130
Jeonghoon 11:2228e8931266 131 ultra_reflect += 0.1;
Jeonghoon 11:2228e8931266 132 if(ultra_reflect > 0.5){
Jeonghoon 11:2228e8931266 133 ultra_reflect = 0;
Jeonghoon 11:2228e8931266 134 }
Jeonghoon 8:efe5a5b1f10a 135 // flaw_state.data = flaw_curr_state;
Jeonghoon 8:efe5a5b1f10a 136 // flaw_state_pub.publish(&flaw_state);
Jeonghoon 8:efe5a5b1f10a 137
Jeonghoon 8:efe5a5b1f10a 138 // printf("Rpm: %f\r\n",rpm);
Jeonghoon 8:efe5a5b1f10a 139 // printf("cumdistance: %f\r\n",cumdistance);
Jeonghoon 8:efe5a5b1f10a 140 // printf("reladistance: %f\r\n",reladistance);
Jeonghoon 8:efe5a5b1f10a 141 nh.spinOnce();
kangmingyo 2:a04440f0d001 142 wait(1);
kangmingyo 1:1d18a2e8ce45 143 }
kangmingyo 3:0c6258635590 144 // motor1.setTarget(60);
Jeonghoon 8:efe5a5b1f10a 145 // set_speed();
kangmingyo 2:a04440f0d001 146 }
kangmingyo 0:dde6e4d8c301 147
Jeonghoon 8:efe5a5b1f10a 148 }