Pipe Inpection Robot
V1
Dependencies: BufferedSerial sn7544
Fork of
GPG_motor
by 
Kang mingyo
Revision 13:681f3d1e9077, committed 2019-08-07
- Comitter:
- kangmingyo
- Date:
- Wed Aug 07 13:40:22 2019 +0000
- Parent:
- 11:2228e8931266
- Commit message:
- 0;
Changed in this revision
--- a/main.cpp Thu Jul 11 13:30:27 2019 +0000
+++ b/main.cpp Wed Aug 07 13:40:22 2019 +0000
@@ -6,29 +6,13 @@
#include <std_msgs/String.h>
#include <geometry_msgs/Point.h>
-std_msgs::Float64 cum_dist;
-std_msgs::Float64 rela_dist;
-std_msgs::Float64 flaw_loca;
-std_msgs::Float64 ultra_reflection;
-std_msgs::Float64 curr_vel;
-std_msgs::Int32 curr_rpm;
-
-
-geometry_msgs::Point flaw_info;
-ros::NodeHandle nh;
-
-ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
-ros::Publisher rpm_pub("rpm", &curr_rpm);
-ros::Publisher ultra_reflection_pub("ultra_reflection", &ultra_reflection);
-ros::Publisher flaw_info_pub("flaw_info", &flaw_info);
-ros::Publisher curr_vel_pub("curr_vel", &curr_vel);
-
-MotorCtl motor1(D3,D12,D4,D5);
+BusOut bus(D11,D12);
+MotorCtl motor1(D3,bus,D4,D5);
InterruptIn tachoINT1(D4);
InterruptIn tachoINT2(D5);
-//Serial pc(USBTX,USBRX,115200);
+RawSerial pc(USBTX,USBRX,115200);
char rx_buffer[10];
int index=0;
@@ -38,110 +22,56 @@
motor1.UpdateCurrentPosition();
// pc.printf("Update Position\r\n");
}
-//void rx_cb(void)
-//{
-// char ch;
-// ch = pc.getc();
-// pc.putc(ch);
-// rx_buffer[index++]=ch;
-// if(ch==0x0D) {
-// pc.putc(0x0A);
-// rx_buffer[--index]='\0';
-// index=0;
-// flag=1;
-// }
-//}
-//
-//void set_speed(void)
-//{
-// int speed;
-// speed = atoi((const char*)rx_buffer);
-// if(speed>78) {
-// speed=78;
-// }
-// if(speed<-78) {
-// speed=-78;
-// }
-// motor1.setTarget(speed);
-// motor1.setTarget(60);
-// pc.printf(" Set speed = %d\r\n", speed);
-//
-//}
+void rx_cb(void)
+{
+ char ch;
+ ch = pc.getc();
+ pc.putc(ch);
+ rx_buffer[index++]=ch;
+ if(ch==0x0D) {
+ pc.putc(0x0A);
+ rx_buffer[--index]='\0';
+ index=0;
+ flag=1;
+ }
+}
+
+void set_speed(void)
+{
+ int speed;
+ speed = atoi((const char*)rx_buffer);
+
+ motor1.setTarget(speed);
+
+ pc.printf(" Set speed = %d\r\n", speed);
+
+}
int main()
{
- nh.initNode();
- nh.advertise(rela_dist_pub);
- nh.advertise(rpm_pub);
- nh.advertise(ultra_reflection_pub);
- nh.advertise(flaw_info_pub);
- nh.advertise(curr_vel_pub);
-
+ pc.attach(callback(rx_cb));
+ int rpm;
tachoINT1.fall(&update_current);
tachoINT1.rise(&update_current);
tachoINT2.fall(&update_current);
tachoINT2.rise(&update_current);
-// pc.attach(callback(rx_cb));
- int rpm;
- float velocity;
- float reladistance; //m단위
- float cumdistance;
- float ultra_reflect = 0;
- float flaw_location = 0;
+
// char flaw_curr_state[10] = "stable";
+ wait(1);
+ motor1.setTarget(60);
- motor1.setTarget(60);
while(1) {
flag=0;
-// pc.printf("Enter the value for speed [-78,78]\r\n");
- while(flag!=1) {
-
+ pc.printf("Enter the value for speed [-78,78]\r\n");
+ while(flag!=1){
rpm = motor1.getRPM();
- cumdistance = motor1.CalculateCumDis(); // 누적거리
- reladistance = motor1.CalculateRelaDis(); // 상대거리
- velocity = motor1.CalculateVelocity();
-
-// cum_dist.data = cumdistance;
-// cum_dist_pub.publish(&cum_dist);
-
- rela_dist.data = reladistance;
- rela_dist_pub.publish(&rela_dist);
-
- curr_rpm.data = rpm;
- rpm_pub.publish(&curr_rpm);
-
- curr_vel.data = velocity;
- curr_vel_pub.publish(&curr_vel);
-
- ultra_reflection.data = ultra_reflect;
- ultra_reflection_pub.publish(&ultra_reflection);
-
- flaw_info.x = cumdistance;
- flaw_info.y = flaw_location;
- flaw_info_pub.publish(&flaw_info);
-
- flaw_location += 0.05;
- if(flaw_location > 1){
- flaw_location = 0;
- }
-
- ultra_reflect += 0.1;
- if(ultra_reflect > 0.5){
- ultra_reflect = 0;
- }
-// flaw_state.data = flaw_curr_state;
-// flaw_state_pub.publish(&flaw_state);
-
-// printf("Rpm: %f\r\n",rpm);
-// printf("cumdistance: %f\r\n",cumdistance);
-// printf("reladistance: %f\r\n",reladistance);
- nh.spinOnce();
+// pc.printf("RPM: %d\r\n",rpm);
wait(1);
}
// motor1.setTarget(60);
-// set_speed();
+ set_speed();
}
}
\ No newline at end of file
--- a/mbed-os.lib Thu Jul 11 13:30:27 2019 +0000 +++ b/mbed-os.lib Wed Aug 07 13:40:22 2019 +0000 @@ -1,1 +1,1 @@ -https://github.com/ARMmbed/mbed-os/#51d55508e8400b60af467005646c4e2164738d48 +https://github.com/ARMmbed/mbed-os/#949cb49ab0a144da0e3b04b6af46db0cd2a20d75
--- a/motor.lib Thu Jul 11 13:30:27 2019 +0000 +++ b/motor.lib Wed Aug 07 13:40:22 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/Jeonghoon/code/motor/#4fa7fadc583d +https://os.mbed.com/teams/Pipe-Inpection-Robot/code/sn7544/#40d0087ec663