Pipe Inpection Robot / Mbed OS GPG_motor_latest

complete motor

Dependencies:   BufferedSerial motor_sn7544

main.cpp@7:13dd93a0efe8, 2019-06-27 (annotated)

Committer:
Jeonghoon
Date:
Thu Jun 27 04:58:29 2019 +0000
Revision:
7:13dd93a0efe8
Parent:
5:e6f9768e1438
Child:
8:efe5a5b1f10a
combine motor with ROS

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/Float32.h>
Jeonghoon 7:13dd93a0efe8 6 #include <std_msgs/Int32.h>
Jeonghoon 7:13dd93a0efe8 7
Jeonghoon 7:13dd93a0efe8 8 std_msgs::Float64 cum_dist;
Jeonghoon 7:13dd93a0efe8 9 std_msgs::Float32 rela_dist;
Jeonghoon 7:13dd93a0efe8 10 std_msgs::Int32 curr_rpm;
Jeonghoon 7:13dd93a0efe8 11
Jeonghoon 7:13dd93a0efe8 12
Jeonghoon 7:13dd93a0efe8 13 ros::NodeHandle nh ;
Jeonghoon 7:13dd93a0efe8 14 ros::Publisher cum_dist_pub("cum_dist", &cum_dist);
Jeonghoon 7:13dd93a0efe8 15 ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
Jeonghoon 7:13dd93a0efe8 16 ros::Publisher rpm_pub("rpm", &curr_rpm);
kangmingyo 0:dde6e4d8c301 17
kangmingyo 1:1d18a2e8ce45 18 MotorCtl motor1(D3,D12,D4,D5);
kangmingyo 1:1d18a2e8ce45 19 InterruptIn tachoINT1(D4);
kangmingyo 1:1d18a2e8ce45 20 InterruptIn tachoINT2(D5);
kangmingyo 0:dde6e4d8c301 21 Serial pc(USBTX,USBRX,115200);
Jeonghoon 7:13dd93a0efe8 22
kangmingyo 2:a04440f0d001 23 char rx_buffer[10];
kangmingyo 2:a04440f0d001 24 int index=0;
kangmingyo 2:a04440f0d001 25 volatile int flag;
kangmingyo 2:a04440f0d001 26 void update_current(void)
kangmingyo 2:a04440f0d001 27 {
kangmingyo 1:1d18a2e8ce45 28 motor1.UpdateCurrentPosition();
kangmingyo 2:a04440f0d001 29 // pc.printf("Update Position\r\n");
kangmingyo 2:a04440f0d001 30 }
kangmingyo 5:e6f9768e1438 31 void rx_cb(void)
kangmingyo 5:e6f9768e1438 32 {
kangmingyo 2:a04440f0d001 33 char ch;
kangmingyo 2:a04440f0d001 34 ch = pc.getc();
kangmingyo 2:a04440f0d001 35 pc.putc(ch);
kangmingyo 2:a04440f0d001 36 rx_buffer[index++]=ch;
kangmingyo 5:e6f9768e1438 37 if(ch==0x0D) {
kangmingyo 2:a04440f0d001 38 pc.putc(0x0A);
kangmingyo 2:a04440f0d001 39 rx_buffer[--index]='\0';
kangmingyo 2:a04440f0d001 40 index=0;
kangmingyo 2:a04440f0d001 41 flag=1;
kangmingyo 2:a04440f0d001 42 }
kangmingyo 1:1d18a2e8ce45 43 }
kangmingyo 1:1d18a2e8ce45 44
kangmingyo 3:0c6258635590 45 void set_speed(void)
kangmingyo 2:a04440f0d001 46 {
kangmingyo 2:a04440f0d001 47 int speed;
kangmingyo 2:a04440f0d001 48 speed = atoi((const char*)rx_buffer);
kangmingyo 5:e6f9768e1438 49 if(speed>78) {
kangmingyo 5:e6f9768e1438 50 speed=78;
kangmingyo 5:e6f9768e1438 51 }
kangmingyo 5:e6f9768e1438 52 if(speed<-78) {
kangmingyo 5:e6f9768e1438 53 speed=-78;
kangmingyo 5:e6f9768e1438 54 }
kangmingyo 2:a04440f0d001 55 motor1.setTarget(speed);
kangmingyo 5:e6f9768e1438 56 // motor1.setTarget(60);
kangmingyo 2:a04440f0d001 57 pc.printf(" Set speed = %d\r\n", speed);
kangmingyo 5:e6f9768e1438 58
kangmingyo 2:a04440f0d001 59 }
kangmingyo 0:dde6e4d8c301 60
kangmingyo 2:a04440f0d001 61 int main()
kangmingyo 2:a04440f0d001 62 {
Jeonghoon 7:13dd93a0efe8 63 nh.initNode();
Jeonghoon 7:13dd93a0efe8 64 nh.advertise(cum_dist_pub);
Jeonghoon 7:13dd93a0efe8 65 nh.advertise(rela_dist_pub);
Jeonghoon 7:13dd93a0efe8 66 nh.advertise(rpm_pub);
Jeonghoon 7:13dd93a0efe8 67
kangmingyo 1:1d18a2e8ce45 68 tachoINT1.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 69 tachoINT1.rise(&update_current);
kangmingyo 1:1d18a2e8ce45 70 tachoINT2.fall(&update_current);
kangmingyo 1:1d18a2e8ce45 71 tachoINT2.rise(&update_current);
kangmingyo 2:a04440f0d001 72 pc.attach(callback(rx_cb));
kangmingyo 2:a04440f0d001 73 int rpm;
kangmingyo 4:a72f75611198 74 float reladistance; //m단위
kangmingyo 4:a72f75611198 75 float cumdistance;
kangmingyo 2:a04440f0d001 76
kangmingyo 2:a04440f0d001 77 while(1) {
kangmingyo 2:a04440f0d001 78 flag=0;
kangmingyo 5:e6f9768e1438 79
kangmingyo 2:a04440f0d001 80 pc.printf("Enter the value for speed [-78,78]\r\n");
kangmingyo 2:a04440f0d001 81 while(flag!=1) {
kangmingyo 4:a72f75611198 82 rpm = motor1.getRPM();
kangmingyo 4:a72f75611198 83 cumdistance = motor1.CalculateCumDis(); // 누적거리
kangmingyo 4:a72f75611198 84 reladistance = motor1.CalculateRelaDis(); // 상대거리
Jeonghoon 7:13dd93a0efe8 85
Jeonghoon 7:13dd93a0efe8 86 cum_dist.data = cumdistance;
Jeonghoon 7:13dd93a0efe8 87 cum_dist_pub.publish(&cum_dist);
Jeonghoon 7:13dd93a0efe8 88
Jeonghoon 7:13dd93a0efe8 89 rela_dist.data = reladistance;
Jeonghoon 7:13dd93a0efe8 90 rela_dist_pub.publish(&rela_dist);
Jeonghoon 7:13dd93a0efe8 91
Jeonghoon 7:13dd93a0efe8 92 curr_rpm.data = rpm;
Jeonghoon 7:13dd93a0efe8 93 rpm_pub.publish(&curr_rpm);
Jeonghoon 7:13dd93a0efe8 94
Jeonghoon 7:13dd93a0efe8 95 printf("Rpm: %f\r\n",rpm);
kangmingyo 4:a72f75611198 96 printf("cumdistance: %f\r\n",cumdistance);
kangmingyo 4:a72f75611198 97 printf("reladistance: %f\r\n",reladistance);
kangmingyo 2:a04440f0d001 98 wait(1);
kangmingyo 1:1d18a2e8ce45 99 }
kangmingyo 3:0c6258635590 100 // motor1.setTarget(60);
kangmingyo 2:a04440f0d001 101 set_speed();
kangmingyo 2:a04440f0d001 102 }
kangmingyo 0:dde6e4d8c301 103
kangmingyo 2:a04440f0d001 104 }