Pipe Inpection Robot / Mbed OS GPG_motor_latest

complete motor

Dependencies:   BufferedSerial motor_sn7544

main.cpp@13:3ac8d2472417, 2019-11-21 (annotated)

Committer:
Jeonghoon
Date:
Thu Nov 21 11:39:20 2019 +0000
Revision:
13:3ac8d2472417
Parent:
12:da4fb0d541ca 
complete motor

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 13:3ac8d2472417 9 BusOut bus(PA_11,PA_12);
Jeonghoon 13:3ac8d2472417 10 MotorCtl motor1(PB_13,bus,PB_14,PB_15);
Jeonghoon 13:3ac8d2472417 11 InterruptIn tachoINT1(PB_14);
Jeonghoon 13:3ac8d2472417 12 InterruptIn tachoINT2(PB_15);
Jeonghoon 13:3ac8d2472417 13
Jeonghoon 13:3ac8d2472417 14
Jeonghoon 7:13dd93a0efe8 15 std_msgs::Float64 cum_dist;
Jeonghoon 8:efe5a5b1f10a 16 std_msgs::Float64 rela_dist;
Jeonghoon 8:efe5a5b1f10a 17 std_msgs::Float64 flaw_loca;
Jeonghoon 11:2228e8931266 18 std_msgs::Float64 ultra_reflection;
Jeonghoon 11:2228e8931266 19 std_msgs::Float64 curr_vel;
Jeonghoon 7:13dd93a0efe8 20 std_msgs::Int32 curr_rpm;
Jeonghoon 11:2228e8931266 21
Jeonghoon 11:2228e8931266 22
Jeonghoon 11:2228e8931266 23 geometry_msgs::Point flaw_info;
Jeonghoon 7:13dd93a0efe8 24
Jeonghoon 13:3ac8d2472417 25 void goalVelCb(const std_msgs::Float64& msg){
Jeonghoon 13:3ac8d2472417 26 motor1.setTarget(msg.data);
Jeonghoon 13:3ac8d2472417 27 }
Jeonghoon 13:3ac8d2472417 28
Jeonghoon 7:13dd93a0efe8 29
Jeonghoon 11:2228e8931266 30 ros::NodeHandle nh;
Jeonghoon 11:2228e8931266 31
Jeonghoon 13:3ac8d2472417 32 ros::Publisher cum_dist_pub("cum_dist", &cum_dist);
Jeonghoon 7:13dd93a0efe8 33 ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
Jeonghoon 7:13dd93a0efe8 34 ros::Publisher rpm_pub("rpm", &curr_rpm);
Jeonghoon 11:2228e8931266 35 ros::Publisher curr_vel_pub("curr_vel", &curr_vel);
Jeonghoon 13:3ac8d2472417 36 ros::Subscriber<std_msgs::Float64> goal_vel_sub("goal_vel", &goalVelCb);
kangmingyo 0:dde6e4d8c301 37
Jeonghoon 7:13dd93a0efe8 38
kangmingyo 2:a04440f0d001 39 int index=0;
kangmingyo 2:a04440f0d001 40 volatile int flag;
Jeonghoon 13:3ac8d2472417 41
kangmingyo 2:a04440f0d001 42 void update_current(void)
kangmingyo 2:a04440f0d001 43 {
Jeonghoon 13:3ac8d2472417 44 motor1.UpdateCurrentPosition();
kangmingyo 2:a04440f0d001 45 }
Jeonghoon 13:3ac8d2472417 46
kangmingyo 0:dde6e4d8c301 47
kangmingyo 2:a04440f0d001 48 int main()
kangmingyo 2:a04440f0d001 49 {
Jeonghoon 7:13dd93a0efe8 50
kangmingyo 1:1d18a2e8ce45 51 tachoINT1.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 52 tachoINT1.rise(&update_current);
kangmingyo 1:1d18a2e8ce45 53 tachoINT2.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 54 tachoINT2.rise(&update_current);
Jeonghoon 13:3ac8d2472417 55
Jeonghoon 13:3ac8d2472417 56 wait(1);
Jeonghoon 13:3ac8d2472417 57 motor1.setTarget(60);
Jeonghoon 13:3ac8d2472417 58
Jeonghoon 13:3ac8d2472417 59
kangmingyo 2:a04440f0d001 60 int rpm;
Jeonghoon 11:2228e8931266 61 float velocity;
kangmingyo 4:a72f75611198 62 float reladistance; //m단위
kangmingyo 4:a72f75611198 63 float cumdistance;
Jeonghoon 11:2228e8931266 64 float ultra_reflect = 0;
Jeonghoon 8:efe5a5b1f10a 65 float flaw_location = 0;
Jeonghoon 13:3ac8d2472417 66
Jeonghoon 13:3ac8d2472417 67 nh.initNode();
Jeonghoon 8:efe5a5b1f10a 68
Jeonghoon 13:3ac8d2472417 69 nh.advertise(cum_dist_pub);
Jeonghoon 13:3ac8d2472417 70 nh.advertise(rela_dist_pub);
Jeonghoon 13:3ac8d2472417 71 nh.advertise(rpm_pub);
Jeonghoon 13:3ac8d2472417 72 nh.subscribe(goal_vel_sub);
Jeonghoon 13:3ac8d2472417 73
Jeonghoon 13:3ac8d2472417 74
Jeonghoon 13:3ac8d2472417 75
kangmingyo 2:a04440f0d001 76 while(1) {
Jeonghoon 11:2228e8931266 77
kangmingyo 4:a72f75611198 78 rpm = motor1.getRPM();
kangmingyo 4:a72f75611198 79 cumdistance = motor1.CalculateCumDis(); // 누적거리
kangmingyo 4:a72f75611198 80 reladistance = motor1.CalculateRelaDis(); // 상대거리
Jeonghoon 11:2228e8931266 81 velocity = motor1.CalculateVelocity();
Jeonghoon 13:3ac8d2472417 82
Jeonghoon 11:2228e8931266 83
Jeonghoon 13:3ac8d2472417 84 cum_dist.data = cumdistance;
Jeonghoon 13:3ac8d2472417 85 cum_dist_pub.publish(&cum_dist);
Jeonghoon 7:13dd93a0efe8 86
Jeonghoon 7:13dd93a0efe8 87 rela_dist.data = reladistance;
Jeonghoon 7:13dd93a0efe8 88 rela_dist_pub.publish(&rela_dist);
Jeonghoon 7:13dd93a0efe8 89
Jeonghoon 7:13dd93a0efe8 90 curr_rpm.data = rpm;
Jeonghoon 11:2228e8931266 91 rpm_pub.publish(&curr_rpm);
Jeonghoon 13:3ac8d2472417 92
Jeonghoon 11:2228e8931266 93
Jeonghoon 8:efe5a5b1f10a 94 nh.spinOnce();
kangmingyo 2:a04440f0d001 95 wait(1);
Jeonghoon 13:3ac8d2472417 96
kangmingyo 2:a04440f0d001 97 }
kangmingyo 0:dde6e4d8c301 98
Jeonghoon 8:efe5a5b1f10a 99 }