Với những kiến thức chuyên môn hiện tại và thời gian có hạn đề tài đã hoàn thành với những yêu cầu đặt ra ban đầu, với đề tài này có thể mở rộng thêm nhƣ:
- Hiện tại Robot chƣa có nhận biết một ngƣời cố định ta có thể phát triển thêm. - Trên Camera Kinect có một động cơ ở chân đế ta có thể tận dụng để lập trình
cho Robot tự động điều chỉnh theo chiều cao của con ngƣời.
- Khối lƣợng của Robot còn nặng vì phải sử dụng laptop điều khiển chúng ta có thể thay thế Laptop bằng Rasberry Pi để lập trình cho Robot khi đó Robot sẽ nhỏ gọn hơn.
78
TÀI LIỆU THAM KHẢO
Tiếng Việt
[1]Nguyễn Hồng Đức và Nguyễn Văn Đức(2012), “ Robot Tự Hành Tránh Vật Cản Sử Dụng Thiết Bị Kinect”, Đồ Án Tốt Nghiệp Đại Học, Trƣờng ĐH Bách Khoa Tp.Hcm.[2] Nguyễn Tấn Lập và Nguyễn Văn Nho(2016), “Điều khiển robot từ xa dùng ROS”, Đồ Án Tốt Nghiệp Đại Học, Trƣờng ĐH Sƣ Phạm Kỹ Thuật Tp.HCM. [3] Nhóm 11 Lớp K55M(2016), “Báo Cáo Tìm Hiểu Cảm Biến Kinect”, Trƣờng ĐH Công Nghệ.
Tiếng Anh
[4] Lentin Joseph(2015), “Mastering ROS for Robotics Programming”. 481 page [5] B. Ilias, S.A. Abdul Shukor, S. Yaacob, A.H. Adom and M.H. Mohd Razali(2014), “A Nurse Following Robot With High Speed Kinect Sensor”, ARPN Journal of Engineering and Applied Sciences.
[6] Matteo Munaro, Filippo Basso and Emanuele Menegatti(2014), “Tracking people within groups with RGB-D data”, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.
[7] J ¨ org St ¨ uckler and Sven Behnke(2014), “Following Human Guidance to Cooperatively Carry a Large Object”, 2011 11th IEEE-RAS International Conference on Humanoid Robots.
79
PHỤ LỤC
MÃ NGUỒN CHƢƠNG TRÌNH
Chƣơng trình trên Arduino Due:
//Libraries for controlling the encoder #include <Encoder.h>
#include <EnableInterrupt.h> #include <DueTimer.h> #include <PID_v1.h> #include <math.h>
//Librabries for interfacing with ros #include <ros.h>
#include <std_msgs/Float64.h>
#define PWM_l_a 8 // chan phat xung cho encoder #define PWM_l_b 9
#define PWM_r_a 10 #define PWM_r_b 11
Encoder leftEncoder (22, 23); Encoder rightEncoder (24, 25); volatile long oldLeft, newLeft = 0; volatile long oldRight, newRight = 0; volatile long diffLeft = 0, diffRight = 0; double leftVelocity = 0, rightVelocity = 0;
double consKp = 25, consKi = 1000, consKd = 0.25; double leftSetPoint = 0, leftPIDOutput;
double rightSetPoint = 0, rightPIDOutput;
PID PID_leftMotor(&leftVelocity, &leftPIDOutput, &leftSetPoint, consKp, consKi, consKd, DIRECT);
PID PID_rightMotor(&rightVelocity, &rightPIDOutput, &rightSetPoint, consKp, consKi, consKd, DIRECT);
//Variables for ROS communication ros::NodeHandle nh;
std_msgs::Float64 leftWheelVelocity; std_msgs::Float64 rightWheelVelocity;
ros::Publisher leftWheelSpeed ("leftWheelSpeed", &leftWheelVelocity); ros::Publisher rightWheelSpeed ("rightWheelSpeed", &rightWheelVelocity);
80 /////////////////////////////////////////////////////////////////////////////////////////////////////
void leftSetpointSubscriber_cb (const std_msgs::Float64& leftSetpointData) { leftSetPoint = leftSetpointData.data;
return; }
/////////////////////////////////////////////////////////////////////////////////////////////////////
void rightSetpointSubscriber_cb (const std_msgs::Float64& rightSetpointData) { rightSetPoint = rightSetpointData.data; return; } ///////////////////////////////////////////////////////////////////////////////////////////////////// ros::Subscriber<std_msgs::Float64> leftSetpointSubscriber ("leftSetpointSubscriber",&leftSetpointSubscriber_cb); ros::Subscriber<std_msgs::Float64> rightSetpointSubscriber ("rightSetpointSubscriber",&rightSetpointSubscriber_cb); ///////////////////////////////////////////////////////////////////////////////////////////////////// void timer_3_interrupt_for_left_motor () { newLeft = leftEncoder.read ();
diffLeft = newLeft - oldLeft;
leftVelocity = double(diffLeft) / (4 * 12 * 64) * 10; oldLeft = newLeft; PID_leftMotor.Compute (); return; } //////////////////////////////////////////newLeft/////////////////////////////////////////////////////////// void timer_4_interrupt_for_right_motor () { newRight = rightEncoder.read (); diffRight = newRight - oldRight;
rightVelocity = double (diffRight) / (4 * 12 * 64) * 10; oldRight = newRight; PID_rightMotor.Compute(); return; } ///////////////////////////////////////////////////////////////////////////////////////////////////// void timer_5_interrupt_for_sending_to_server () {
81 leftWheelVelocity.data = leftVelocity * 2 * M_PI;
rightWheelVelocity.data = rightVelocity * 2 * M_PI; //Publish the left and right Velocity
leftWheelSpeed.publish (&leftWheelVelocity); rightWheelSpeed.publish (&rightWheelVelocity); return; } ///////////////////////////////////////////////////////////////////////////////////////////////////// void setup () {
// setup the pwm input reader pinMode(PWM_l_a, OUTPUT); pinMode(PWM_l_b, OUTPUT); pinMode(PWM_r_a, OUTPUT); pinMode(PWM_r_b, OUTPUT);
//Setup parameter for timer 3 for left motor
Timer3.attachInterrupt (timer_3_interrupt_for_left_motor); Timer3.start (100000); //100ms
//Setup parameter for timer
Timer4.attachInterrupt (timer_4_interrupt_for_right_motor); Timer4.start (100000); //100ms
//Setup timer for sending data to ROS server
Timer5.attachInterrupt (timer_5_interrupt_for_sending_to_server); Timer5.start (100000); //100ms
//Setup PID parameters
PID_leftMotor.SetMode (AUTOMATIC); PID_rightMotor.SetMode (AUTOMATIC);
PID_leftMotor.SetTunings (consKp, consKi, consKd); PID_leftMotor.SetOutputLimits (-255, 255);
PID_rightMotor.SetTunings (consKp, consKi, consKd); PID_rightMotor.SetOutputLimits (-255, 255);
//Setup ros environment nh.initNode ();
nh.advertise (leftWheelSpeed); nh.advertise (rightWheelSpeed); nh.subscribe (leftSetpointSubscriber); nh.subscribe (rightSetpointSubscriber);
82 }
///////////////////////////////////////////////////////////////////////////////////////////////////// void loop () {
//Update the PID value
//Update the value of the current setpoint of the left and right motor // For left motor
if (leftPIDOutput < 0) { digitalWrite(PWM_l_b, LOW); analogWrite(PWM_l_a, abs(leftPIDOutput)); } else { if (leftPIDOutput > 0) { digitalWrite(PWM_l_a, LOW); analogWrite(PWM_l_b, leftPIDOutput); } }
//For right motor
if (rightPIDOutput < 0) { digitalWrite(PWM_r_b, LOW); analogWrite(PWM_r_a, abs(rightPIDOutput)); } else { if (rightPIDOutput > 0) { digitalWrite(PWM_r_a, LOW); analogWrite(PWM_r_b, rightPIDOutput); } }
//Spin ROS for updating system nh.spinOnce ();
}
Chƣơng trình trên hệ điều hành ROS
Mã nguồn các chƣơng trình chạy trên hệ điều hành ROS đƣơc lƣu trữ trong CD đính kèm.