Pipe Inpection Robot / Mbed OS GPG_motor_latest

complete motor

Dependencies:   BufferedSerial motor_sn7544

main.cpp@11:2228e8931266, 2019-07-11 (annotated)

Committer:
Jeonghoon
Date:
Thu Jul 11 13:30:27 2019 +0000
Revision:
11:2228e8931266
Parent:
8:efe5a5b1f10a
Child:
12:da4fb0d541ca
190711; ;

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