Hsieh JenYun / Mbed OS agv20190417

test0417

Dependencies:   ros_lib_indigo

main.cpp@8:31d4dd20cfc0, 2019-04-17 (annotated)

Committer:
Tanakacool
Date:
Wed Apr 17 09:11:00 2019 +0000
Revision:
8:31d4dd20cfc0
Parent:
7:46bb04ae559c 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:f76c26307f9a 1 #include "mbed.h"
Tanakacool 8:31d4dd20cfc0 2 #include "uagv.h"
cathy101 1:6926cb0de2b0 3
Tanakacool 5:221ce6ba655c 4 #define LEFT 0
Tanakacool 5:221ce6ba655c 5 #define RIGHT 1
Tanakacool 5:221ce6ba655c 6
Tanakacool 8:31d4dd20cfc0 7
Tanakacool 8:31d4dd20cfc0 8 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 9 ROS NodeHandle
Tanakacool 8:31d4dd20cfc0 10 *******************************************************************************/
Tanakacool 8:31d4dd20cfc0 11 ros::NodeHandle nh;
Tanakacool 8:31d4dd20cfc0 12
Tanakacool 8:31d4dd20cfc0 13 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 14 Publisher
Tanakacool 8:31d4dd20cfc0 15 *******************************************************************************/
Tanakacool 8:31d4dd20cfc0 16 std_msgs::UInt8 lift_status; // 0 = unexpected result or error, 1 = lift up, 2 = lift down, 3 = halt, 4 = reserved for UAGV onload status
Tanakacool 8:31d4dd20cfc0 17 ros::Publisher lift_status_pub("liftStatus", &lift_status);
Tanakacool 8:31d4dd20cfc0 18
Tanakacool 8:31d4dd20cfc0 19 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 20 Subscriber
Tanakacool 8:31d4dd20cfc0 21 *******************************************************************************/
Tanakacool 8:31d4dd20cfc0 22 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", commandVelocityCallback);
Tanakacool 8:31d4dd20cfc0 23 ros::Subscriber<std_msgs::UInt8> lift_cmd_sub("liftCmd", liftCmdCallback);
Tanakacool 8:31d4dd20cfc0 24
Tanakacool 8:31d4dd20cfc0 25 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 26 Declaration for motor & BT command
Tanakacool 8:31d4dd20cfc0 27 *******************************************************************************/
Tanakacool 8:31d4dd20cfc0 28 float mDesiredDirection, mDesiredVelocity; //從BT來的command：角度-1~1, 速度 -1~1
Tanakacool 8:31d4dd20cfc0 29 float mMaxVelocity = 0.5; //最高速 = 0.5m/s
Tanakacool 8:31d4dd20cfc0 30 float mCurrentVelocity, mCurrentDirection; //目前的速度和角度
Tanakacool 8:31d4dd20cfc0 31 float safeVelocity = 1;
Tanakacool 5:221ce6ba655c 32
Tanakacool 8:31d4dd20cfc0 33 float goal_linear_velocity = 0.0;
Tanakacool 8:31d4dd20cfc0 34 float goal_angular_velocity = 0.0;
Tanakacool 8:31d4dd20cfc0 35
Tanakacool 8:31d4dd20cfc0 36 short VELOCITY_CONSTANT_VAULE = 30000; //cmd:16585~=1(m/s)
Tanakacool 8:31d4dd20cfc0 37 short LIMIT_X_MAX_VELOCITY = 18000; //max cmd:8293~=0.5m/s
Tanakacool 8:31d4dd20cfc0 38 short LIMIT_X_MIN_VELOCITY = 1640; //min value
mbed_official 0:f76c26307f9a 39
Tanakacool 8:31d4dd20cfc0 40 int lin_vel1; //左輪
Tanakacool 8:31d4dd20cfc0 41 int lin_vel2; //右輪
Tanakacool 8:31d4dd20cfc0 42 int lin_vel1_t; //左輪
Tanakacool 8:31d4dd20cfc0 43 int lin_vel2_t; //右輪
Tanakacool 8:31d4dd20cfc0 44 long LencV_0; //左輪
Tanakacool 8:31d4dd20cfc0 45 long RencV_0; //右輪
Tanakacool 8:31d4dd20cfc0 46 long LencV_t; //左輪
Tanakacool 8:31d4dd20cfc0 47 long RencV_t; //右輪
Tanakacool 7:46bb04ae559c 48
Tanakacool 8:31d4dd20cfc0 49 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 50 Declaration for LED
Tanakacool 8:31d4dd20cfc0 51 *******************************************************************************/
Tanakacool 8:31d4dd20cfc0 52 int brightness = 0;
Tanakacool 8:31d4dd20cfc0 53 int fadeAmount = 5;
Tanakacool 8:31d4dd20cfc0 54
Tanakacool 8:31d4dd20cfc0 55 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 56 Declaration for control flags & other parameters
Tanakacool 8:31d4dd20cfc0 57 *******************************************************************************/
Tanakacool 8:31d4dd20cfc0 58 bool i2cinitial = true;
Tanakacool 8:31d4dd20cfc0 59 uint32_t delaytime;
Tanakacool 8:31d4dd20cfc0 60
Tanakacool 7:46bb04ae559c 61 DigitalOut LIFT_UP_PIN(D4);
Tanakacool 7:46bb04ae559c 62 DigitalOut LIFT_DOWN_PIN(D3);
Tanakacool 5:221ce6ba655c 63 //InterruptIn sw2(B1);
Tanakacool 5:221ce6ba655c 64 //I2C i2c( PTB1, PTB0);
Tanakacool 5:221ce6ba655c 65 I2C i2c( D14, D15);
Tanakacool 8:31d4dd20cfc0 66
Tanakacool 8:31d4dd20cfc0 67 //Serial serialPort(USBTX, USBRX);
Tanakacool 5:221ce6ba655c 68 //Serial serialPort( PTE0, PTE1);
Tanakacool 2:3d64970eeb96 69
Tanakacool 4:186241a75818 70 char Rmotobuffer[3];
Tanakacool 4:186241a75818 71 char Lmotobuffer[3];
Tanakacool 3:996d0d74fb11 72
Tanakacool 2:3d64970eeb96 73 int Right_ADDR = 0x14<<1;
Tanakacool 3:996d0d74fb11 74 int Left_ADDR = 0x15<<1;
Tanakacool 8:31d4dd20cfc0 75 Timer t;
Tanakacool 8:31d4dd20cfc0 76 long last_debounce_time=0;
Tanakacool 5:221ce6ba655c 77
Tanakacool 5:221ce6ba655c 78 int main()
Tanakacool 5:221ce6ba655c 79 {
Tanakacool 8:31d4dd20cfc0 80 // Initializing Mbed and public parameters
Tanakacool 8:31d4dd20cfc0 81 t.start();
Tanakacool 5:221ce6ba655c 82 i2c.frequency(100000);
Tanakacool 5:221ce6ba655c 83 wait_ms(100);
Tanakacool 7:46bb04ae559c 84
Tanakacool 8:31d4dd20cfc0 85 // Initializing ROS connection
Tanakacool 8:31d4dd20cfc0 86 nh.getHardware()->setBaud(57600);
Tanakacool 8:31d4dd20cfc0 87 nh.initNode();
Tanakacool 8:31d4dd20cfc0 88 nh.subscribe(cmd_vel_sub);
Tanakacool 8:31d4dd20cfc0 89 nh.advertise(lift_status_pub);
Tanakacool 8:31d4dd20cfc0 90 nh.subscribe(lift_cmd_sub);
Tanakacool 8:31d4dd20cfc0 91 nh.loginfo("Initialized done.");
Tanakacool 8:31d4dd20cfc0 92 while(true){
Tanakacool 8:31d4dd20cfc0 93 if(t.read_ms() - last_debounce_time>(1000/CMD_VEL_PUBLISH_PERIOD)){
Tanakacool 8:31d4dd20cfc0 94 Driving_Left(lin_vel1);
Tanakacool 8:31d4dd20cfc0 95 wait_ms(10);
Tanakacool 8:31d4dd20cfc0 96 Driving_Right(lin_vel2);
Tanakacool 8:31d4dd20cfc0 97 last_debounce_time = t.read_ms();
Tanakacool 8:31d4dd20cfc0 98 }
Tanakacool 8:31d4dd20cfc0 99 nh.spinOnce();
Tanakacool 5:221ce6ba655c 100 }
Tanakacool 5:221ce6ba655c 101 }
Tanakacool 5:221ce6ba655c 102
Tanakacool 8:31d4dd20cfc0 103 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 104 Callback function for cmd_vel msg
Tanakacool 8:31d4dd20cfc0 105 *******************************************************************************/
Tanakacool 8:31d4dd20cfc0 106 void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg)
Tanakacool 8:31d4dd20cfc0 107 {
Tanakacool 8:31d4dd20cfc0 108 goal_linear_velocity = cmd_vel_msg.linear.x;
Tanakacool 8:31d4dd20cfc0 109 goal_angular_velocity = cmd_vel_msg.angular.z;
Tanakacool 8:31d4dd20cfc0 110 controlMotorSpeed(goal_linear_velocity, goal_angular_velocity);
Tanakacool 8:31d4dd20cfc0 111 }
Tanakacool 8:31d4dd20cfc0 112
Tanakacool 8:31d4dd20cfc0 113 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 114 Converter of motor command
Tanakacool 8:31d4dd20cfc0 115 *******************************************************************************/
Tanakacool 5:221ce6ba655c 116 void controlMotorSpeed(float goalLinearVel, float goalAngularVel)
Tanakacool 2:3d64970eeb96 117 {
Tanakacool 5:221ce6ba655c 118 float wheel_speed_cmd[2];
Tanakacool 5:221ce6ba655c 119
Tanakacool 5:221ce6ba655c 120 wheel_speed_cmd[LEFT] = goalLinearVel - (goalAngularVel * WHEEL_SEPARATION / 2);
Tanakacool 5:221ce6ba655c 121 wheel_speed_cmd[RIGHT] = goalLinearVel + (goalAngularVel * WHEEL_SEPARATION / 2);
Tanakacool 5:221ce6ba655c 122
Tanakacool 5:221ce6ba655c 123 lin_vel1 = wheel_speed_cmd[LEFT] * VELOCITY_CONSTANT_VAULE;
Tanakacool 5:221ce6ba655c 124 wait_ms(2);
Tanakacool 5:221ce6ba655c 125
Tanakacool 5:221ce6ba655c 126 if (lin_vel1 > LIMIT_X_MAX_VELOCITY) lin_vel1 = LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 127 else if (lin_vel1 < LIMIT_X_MIN_VELOCITY && lin_vel1 > 0 ) lin_vel1 = LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 128 else if (lin_vel1 < -LIMIT_X_MAX_VELOCITY) lin_vel1 = -LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 129 else if (lin_vel1 > -LIMIT_X_MIN_VELOCITY && lin_vel1 < 0) lin_vel1 = -LIMIT_X_MIN_VELOCITY;
Tanakacool 8:31d4dd20cfc0 130 lin_vel1 = lin_vel1_t * 0.3 + lin_vel1 * 0.7;
Tanakacool 5:221ce6ba655c 131
Tanakacool 5:221ce6ba655c 132 lin_vel2 = wheel_speed_cmd[RIGHT] * VELOCITY_CONSTANT_VAULE;
Tanakacool 5:221ce6ba655c 133 wait_ms(2);
Tanakacool 5:221ce6ba655c 134
Tanakacool 5:221ce6ba655c 135 if (lin_vel2 > LIMIT_X_MAX_VELOCITY) lin_vel2 = LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 136 else if (lin_vel2 < LIMIT_X_MIN_VELOCITY && lin_vel2 > 0 ) lin_vel2 = LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 137 else if (lin_vel2 < -LIMIT_X_MAX_VELOCITY) lin_vel2 = -LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 138 else if (lin_vel2 > -LIMIT_X_MIN_VELOCITY && lin_vel2 < 0) lin_vel2 = -LIMIT_X_MIN_VELOCITY;
Tanakacool 8:31d4dd20cfc0 139 lin_vel2 = lin_vel2_t * 0.3 + lin_vel2 * 0.7;
Tanakacool 5:221ce6ba655c 140 }
Tanakacool 8:31d4dd20cfc0 141
Tanakacool 8:31d4dd20cfc0 142 /*******************************************************************************
Tanakacool 8:31d4dd20cfc0 143 Callback function for lift_cmd msg
Tanakacool 8:31d4dd20cfc0 144 *******************************************************************************/
Tanakacool 8:31d4dd20cfc0 145 void liftCmdCallback(const std_msgs::UInt8& lift_cmd_msg)
Tanakacool 7:46bb04ae559c 146 {
Tanakacool 8:31d4dd20cfc0 147 wait_ms(100);
Tanakacool 8:31d4dd20cfc0 148 LIFT_UP_PIN=0;
Tanakacool 8:31d4dd20cfc0 149 LIFT_DOWN_PIN=0;
Tanakacool 8:31d4dd20cfc0 150 wait_ms(2000);
Tanakacool 8:31d4dd20cfc0 151 switch (lift_cmd_msg.data) {
Tanakacool 7:46bb04ae559c 152 case 1: //up
Tanakacool 7:46bb04ae559c 153 LIFT_UP_PIN=1;
Tanakacool 7:46bb04ae559c 154 LIFT_DOWN_PIN=0;
Tanakacool 8:31d4dd20cfc0 155
Tanakacool 7:46bb04ae559c 156 break;
Tanakacool 7:46bb04ae559c 157 case 2: //down
Tanakacool 7:46bb04ae559c 158 LIFT_UP_PIN=0;
Tanakacool 7:46bb04ae559c 159 LIFT_DOWN_PIN=1;
Tanakacool 8:31d4dd20cfc0 160
Tanakacool 8:31d4dd20cfc0 161 break;
Tanakacool 8:31d4dd20cfc0 162 case 3: //stop
Tanakacool 8:31d4dd20cfc0 163 LIFT_UP_PIN=0;
Tanakacool 8:31d4dd20cfc0 164 LIFT_DOWN_PIN=0;
Tanakacool 8:31d4dd20cfc0 165
Tanakacool 7:46bb04ae559c 166 break;
Tanakacool 8:31d4dd20cfc0 167 default: //stop
Tanakacool 8:31d4dd20cfc0 168 LIFT_UP_PIN=0;
Tanakacool 8:31d4dd20cfc0 169 LIFT_DOWN_PIN=0;
Tanakacool 8:31d4dd20cfc0 170
Tanakacool 8:31d4dd20cfc0 171 break;
Tanakacool 8:31d4dd20cfc0 172 }
Tanakacool 8:31d4dd20cfc0 173
Tanakacool 8:31d4dd20cfc0 174 }
Tanakacool 7:46bb04ae559c 175
Tanakacool 5:221ce6ba655c 176 void Driving_Right(int rightOutPut)
Tanakacool 5:221ce6ba655c 177 {
Tanakacool 5:221ce6ba655c 178 Rmotobuffer[0]= 0x00;
Tanakacool 5:221ce6ba655c 179 Rmotobuffer[1]= rightOutPut>>8;
Tanakacool 5:221ce6ba655c 180 Rmotobuffer[2]= rightOutPut;
Tanakacool 2:3d64970eeb96 181 i2c.start();
Tanakacool 2:3d64970eeb96 182 i2c.write(Right_ADDR);
Tanakacool 2:3d64970eeb96 183 i2c.write(0);
Tanakacool 4:186241a75818 184 i2c.write(Rmotobuffer[1]);
Tanakacool 4:186241a75818 185 i2c.write(Rmotobuffer[2]);
Tanakacool 2:3d64970eeb96 186 i2c.stop();
Tanakacool 2:3d64970eeb96 187 }
Tanakacool 8:31d4dd20cfc0 188
Tanakacool 5:221ce6ba655c 189 void Driving_Left(int leftOutPut)
Tanakacool 2:3d64970eeb96 190 {
Tanakacool 5:221ce6ba655c 191 Lmotobuffer[0]= 0x00;
Tanakacool 5:221ce6ba655c 192 Lmotobuffer[1]= leftOutPut>>8;
Tanakacool 5:221ce6ba655c 193 Lmotobuffer[2]= leftOutPut;
Tanakacool 5:221ce6ba655c 194 i2c.start();
Tanakacool 5:221ce6ba655c 195 i2c.write(Left_ADDR);
Tanakacool 5:221ce6ba655c 196 i2c.write(0);
Tanakacool 5:221ce6ba655c 197 i2c.write(Lmotobuffer[1]);
Tanakacool 5:221ce6ba655c 198 i2c.write(Lmotobuffer[2]);
Tanakacool 5:221ce6ba655c 199 i2c.stop();
Tanakacool 8:31d4dd20cfc0 200
Tanakacool 5:221ce6ba655c 201 }
Tanakacool 3:996d0d74fb11 202