Pipe Inpection Robot / Mbed OS GPG_motor_latest

complete motor

Dependencies:   BufferedSerial motor_sn7544

main.cpp@8:efe5a5b1f10a, 2019-07-10 (annotated)

Committer:
Jeonghoon
Date:
Wed Jul 10 03:51:11 2019 +0000
Revision:
8:efe5a5b1f10a
Parent:
7:13dd93a0efe8
Child:
11:2228e8931266
190710_non_err;

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