Hsieh JenYun
/
agv20190417
test0417
uagv.h@6:4e03a22f2abb, 2019-04-17 (annotated)
- Committer:
- rudy_wang
- Date:
- Wed Apr 17 06:02:13 2019 +0000
- Revision:
- 6:4e03a22f2abb
11
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rudy_wang | 6:4e03a22f2abb | 1 | #ifndef UAGV_CORE_CONFIG_H_ |
rudy_wang | 6:4e03a22f2abb | 2 | #define UAGV_CORE_CONFIG_H_ |
rudy_wang | 6:4e03a22f2abb | 3 | |
rudy_wang | 6:4e03a22f2abb | 4 | #include <math.h> |
rudy_wang | 6:4e03a22f2abb | 5 | |
rudy_wang | 6:4e03a22f2abb | 6 | #include <ros.h> |
rudy_wang | 6:4e03a22f2abb | 7 | #include <ros/time.h> |
rudy_wang | 6:4e03a22f2abb | 8 | #include <std_msgs/Bool.h> |
rudy_wang | 6:4e03a22f2abb | 9 | #include <std_msgs/Empty.h> |
rudy_wang | 6:4e03a22f2abb | 10 | #include <std_msgs/UInt8.h> |
rudy_wang | 6:4e03a22f2abb | 11 | #include <geometry_msgs/Twist.h> |
rudy_wang | 6:4e03a22f2abb | 12 | |
rudy_wang | 6:4e03a22f2abb | 13 | #define CMD_VEL_PUBLISH_PERIOD 20.0 //hz |
rudy_wang | 6:4e03a22f2abb | 14 | #define DRIVE_INFORMATION_PUBLISH_PERIOD 30.0 //hz |
rudy_wang | 6:4e03a22f2abb | 15 | |
rudy_wang | 6:4e03a22f2abb | 16 | #define WHEEL_RADIUS 0.09 // meter |
rudy_wang | 6:4e03a22f2abb | 17 | #define WHEEL_SEPARATION 0.46 // meter (BURGER => 0.16, WAFFLE => 0.287) |
rudy_wang | 6:4e03a22f2abb | 18 | #define ROBOT_RADIUS 0.294 // meter (BURGER => 0.078, WAFFLE => 0.294) |
rudy_wang | 6:4e03a22f2abb | 19 | #define ENCODER_MIN -2147483648 // raw |
rudy_wang | 6:4e03a22f2abb | 20 | #define ENCODER_MAX 2147483648 // raw |
rudy_wang | 6:4e03a22f2abb | 21 | |
rudy_wang | 6:4e03a22f2abb | 22 | #define LEFT 0 |
rudy_wang | 6:4e03a22f2abb | 23 | #define RIGHT 1 |
rudy_wang | 6:4e03a22f2abb | 24 | |
rudy_wang | 6:4e03a22f2abb | 25 | #define VELOCITY_CONSTANT_VALUE 1263.632956882 // V = r * w = r * RPM * 0.10472 |
rudy_wang | 6:4e03a22f2abb | 26 | // = 0.033 * 0.229 * Goal RPM * 0.10472 |
rudy_wang | 6:4e03a22f2abb | 27 | // Goal RPM = V * 1263.632956882 |
rudy_wang | 6:4e03a22f2abb | 28 | |
rudy_wang | 6:4e03a22f2abb | 29 | #define MAX_LINEAR_VELOCITY 0.22 // m/s |
rudy_wang | 6:4e03a22f2abb | 30 | #define MAX_ANGULAR_VELOCITY 2.84 // rad/s |
rudy_wang | 6:4e03a22f2abb | 31 | #define VELOCITY_STEP 0.01 // m/s |
rudy_wang | 6:4e03a22f2abb | 32 | #define VELOCITY_LINEAR_X 0.01 // m/s |
rudy_wang | 6:4e03a22f2abb | 33 | #define VELOCITY_ANGULAR_Z 0.1 // rad/s |
rudy_wang | 6:4e03a22f2abb | 34 | #define SCALE_VELOCITY_LINEAR_X 1 |
rudy_wang | 6:4e03a22f2abb | 35 | #define SCALE_VELOCITY_ANGULAR_Z 1 |
rudy_wang | 6:4e03a22f2abb | 36 | |
rudy_wang | 6:4e03a22f2abb | 37 | #define TICK2RAD 0.000628318 // 2 * 3.14159265359 / 10000 = 0.000628318f |
rudy_wang | 6:4e03a22f2abb | 38 | |
rudy_wang | 6:4e03a22f2abb | 39 | #define DEG2RAD(x) (x * 0.01745329252) // *PI/180 |
rudy_wang | 6:4e03a22f2abb | 40 | #define RAD2DEG(x) (x * 57.2957795131) // *180/PI |
rudy_wang | 6:4e03a22f2abb | 41 | |
rudy_wang | 6:4e03a22f2abb | 42 | #define TEST_DISTANCE 0.300 // meter |
rudy_wang | 6:4e03a22f2abb | 43 | #define TEST_RADIAN 3.14 // 180 degree |
rudy_wang | 6:4e03a22f2abb | 44 | |
rudy_wang | 6:4e03a22f2abb | 45 | #define WAIT_FOR_BUTTON_PRESS 0 |
rudy_wang | 6:4e03a22f2abb | 46 | #define WAIT_SECOND 1 |
rudy_wang | 6:4e03a22f2abb | 47 | #define CHECK_BUTTON_RELEASED 2 |
rudy_wang | 6:4e03a22f2abb | 48 | |
rudy_wang | 6:4e03a22f2abb | 49 | void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg); |
rudy_wang | 6:4e03a22f2abb | 50 | void liftCmdCallback(const std_msgs::UInt8& lift_cmd_msg); |
rudy_wang | 6:4e03a22f2abb | 51 | void Driving_Right(int rightOutPut); |
rudy_wang | 6:4e03a22f2abb | 52 | void Driving_Left(int leftOutPut); |
rudy_wang | 6:4e03a22f2abb | 53 | void controlMotorSpeed(float goalLinearVel, float goalAngularVel); |
rudy_wang | 6:4e03a22f2abb | 54 | |
rudy_wang | 6:4e03a22f2abb | 55 | #endif // UAGV_CORE_CONFIG_H_ |