Weber Yang / Mbed 2 deprecated AGV_0220

test

Dependencies:   mbed ros_lib_kinetic

main.cpp@0:6a62ef9495ec, 2018-04-12 (annotated)

Committer:
WeberYang
Date:
Thu Apr 12 00:55:59 2018 +0000
Revision:
0:6a62ef9495ec
betaTesting

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WeberYang 0:6a62ef9495ec 1 #include "mbed.h"
WeberYang 0:6a62ef9495ec 2 #include <ros.h>
WeberYang 0:6a62ef9495ec 3 #include <ros/time.h>
WeberYang 0:6a62ef9495ec 4 #include <std_msgs/Empty.h>
WeberYang 0:6a62ef9495ec 5 #include <std_msgs/String.h>
WeberYang 0:6a62ef9495ec 6 #include<geometry_msgs/Twist.h> //set buffer larger than 50byte
WeberYang 0:6a62ef9495ec 7 #include <std_msgs/Float32.h>
WeberYang 0:6a62ef9495ec 8 #include <math.h>
WeberYang 0:6a62ef9495ec 9 #include <stdio.h>
WeberYang 0:6a62ef9495ec 10 #include <string.h>
WeberYang 0:6a62ef9495ec 11 #include <iostream>
WeberYang 0:6a62ef9495ec 12 #define Start 0xAA
WeberYang 0:6a62ef9495ec 13 #define Address 0x7F
WeberYang 0:6a62ef9495ec 14 #define ReturnType 0x00
WeberYang 0:6a62ef9495ec 15 #define Clean 0x00
WeberYang 0:6a62ef9495ec 16 #define Reserve 0x00
WeberYang 0:6a62ef9495ec 17 #define End 0x55
WeberYang 0:6a62ef9495ec 18
WeberYang 0:6a62ef9495ec 19 Serial RS232 (PB_10, PB_11,115200); // This one works
WeberYang 0:6a62ef9495ec 20 std_msgs::String str_msg;
WeberYang 0:6a62ef9495ec 21 std_msgs::Float32 VelAngular_L;
WeberYang 0:6a62ef9495ec 22 std_msgs::Float32 VelAngular_R;
WeberYang 0:6a62ef9495ec 23 ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
WeberYang 0:6a62ef9495ec 24 ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
WeberYang 0:6a62ef9495ec 25
WeberYang 0:6a62ef9495ec 26 ros::NodeHandle nh;
WeberYang 0:6a62ef9495ec 27 geometry_msgs::Twist vel;
WeberYang 0:6a62ef9495ec 28 //float left;
WeberYang 0:6a62ef9495ec 29 //float right;
WeberYang 0:6a62ef9495ec 30 float Lrpm,Rrpm;
WeberYang 0:6a62ef9495ec 31 float ticks_since_target;
WeberYang 0:6a62ef9495ec 32 double timeout_ticks;
WeberYang 0:6a62ef9495ec 33
WeberYang 0:6a62ef9495ec 34 double w;
WeberYang 0:6a62ef9495ec 35 double rate;
WeberYang 0:6a62ef9495ec 36 double Dimeter;
WeberYang 0:6a62ef9495ec 37 float dx,dy,dr;
WeberYang 0:6a62ef9495ec 38 void init_variables();
WeberYang 0:6a62ef9495ec 39 void Sendmessage(float Rrpm,float Lrpm);
WeberYang 0:6a62ef9495ec 40
WeberYang 0:6a62ef9495ec 41 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
WeberYang 0:6a62ef9495ec 42 {
WeberYang 0:6a62ef9495ec 43 unsigned int i, j;
WeberYang 0:6a62ef9495ec 44 //#define wPolynom 0xA001
WeberYang 0:6a62ef9495ec 45 unsigned int wCrc = 0xffff;
WeberYang 0:6a62ef9495ec 46 unsigned int wPolynom = 0xA001;
WeberYang 0:6a62ef9495ec 47 /*---------------------------------------------------------------------------------*/
WeberYang 0:6a62ef9495ec 48 for (i = 0; i < iBufLen; i++)
WeberYang 0:6a62ef9495ec 49 {
WeberYang 0:6a62ef9495ec 50 wCrc ^= cBuffer[i];
WeberYang 0:6a62ef9495ec 51 for (j = 0; j < 8; j++)
WeberYang 0:6a62ef9495ec 52 {
WeberYang 0:6a62ef9495ec 53 if (wCrc &0x0001)
WeberYang 0:6a62ef9495ec 54 {
WeberYang 0:6a62ef9495ec 55 wCrc = (wCrc >> 1) ^ wPolynom;
WeberYang 0:6a62ef9495ec 56 }
WeberYang 0:6a62ef9495ec 57 else
WeberYang 0:6a62ef9495ec 58 {
WeberYang 0:6a62ef9495ec 59 wCrc = wCrc >> 1;
WeberYang 0:6a62ef9495ec 60 }
WeberYang 0:6a62ef9495ec 61 }
WeberYang 0:6a62ef9495ec 62 }
WeberYang 0:6a62ef9495ec 63 return wCrc;
WeberYang 0:6a62ef9495ec 64 }
WeberYang 0:6a62ef9495ec 65
WeberYang 0:6a62ef9495ec 66 /*
WeberYang 0:6a62ef9495ec 67 起始碼(1Byte): AA
WeberYang 0:6a62ef9495ec 68 地址碼(1Byte): 01
WeberYang 0:6a62ef9495ec 69 返回數據類型(1Byte):從站驅動器不返回數據 00
WeberYang 0:6a62ef9495ec 70 故障清除(1Byte)默認發送: 00
WeberYang 0:6a62ef9495ec 71 保留位(1Byte) 00
WeberYang 0:6a62ef9495ec 72 A 馬達過載控制(1Byte)驅動器使能狀態 01
WeberYang 0:6a62ef9495ec 73 B 馬達控制(1Byte) 01
WeberYang 0:6a62ef9495ec 74 A 運行方向(1Byte)00:正轉,01反轉 00
WeberYang 0:6a62ef9495ec 75 B 運行方向(1Byte)00:正轉,01反轉 00
WeberYang 0:6a62ef9495ec 76 運行轉速(2Byte)是100–3000r/min, 12 34
WeberYang 0:6a62ef9495ec 77 運行轉速(2Byte)是100–3000r/min, 12 34
WeberYang 0:6a62ef9495ec 78 第 1 個 Byte:速度值高 8 位;
WeberYang 0:6a62ef9495ec 79 第 2 個 Byte:速度值低 8 位;
WeberYang 0:6a62ef9495ec 80 無效(2Byte)00:無效數據位。 00
WeberYang 0:6a62ef9495ec 81 結束碼(1Byte)55:通訊結束編碼識別。 55
WeberYang 0:6a62ef9495ec 82 校驗碼(2Byte)CRC16
WeberYang 0:6a62ef9495ec 83 */
WeberYang 0:6a62ef9495ec 84
WeberYang 0:6a62ef9495ec 85 // void spinOnce();
WeberYang 0:6a62ef9495ec 86 // void twistCallback(const geometry_msgs::Twist &twist_aux);
WeberYang 0:6a62ef9495ec 87
WeberYang 0:6a62ef9495ec 88
WeberYang 0:6a62ef9495ec 89 void init_variables()
WeberYang 0:6a62ef9495ec 90 {
WeberYang 0:6a62ef9495ec 91 dx = dy = dr =0;
WeberYang 0:6a62ef9495ec 92 w = 0.2 ;
WeberYang 0:6a62ef9495ec 93 rate = 50;
WeberYang 0:6a62ef9495ec 94 timeout_ticks = 2;
WeberYang 0:6a62ef9495ec 95 Dimeter = 0.15;
WeberYang 0:6a62ef9495ec 96 }
WeberYang 0:6a62ef9495ec 97 void Sendmessage(float Rrpm,float Lrpm)
WeberYang 0:6a62ef9495ec 98 {
WeberYang 0:6a62ef9495ec 99 //RS232.printf("Wr = %.1f\n",Rrpm);
WeberYang 0:6a62ef9495ec 100 //RS232.printf("Wl = %.1f\n",Lrpm);
WeberYang 0:6a62ef9495ec 101 unsigned char sendData[16];
WeberYang 0:6a62ef9495ec 102 unsigned int tmpCRC;
WeberYang 0:6a62ef9495ec 103 int motor1,motor2;
WeberYang 0:6a62ef9495ec 104
WeberYang 0:6a62ef9495ec 105 sendData[0] = Start;
WeberYang 0:6a62ef9495ec 106 sendData[1] = Address;
WeberYang 0:6a62ef9495ec 107 sendData[2] = ReturnType;
WeberYang 0:6a62ef9495ec 108 sendData[3] = Clean;
WeberYang 0:6a62ef9495ec 109 sendData[4] = Reserve;
WeberYang 0:6a62ef9495ec 110 sendData[5] = 0x01;//motor1Sevro ON
WeberYang 0:6a62ef9495ec 111 sendData[6] = 0x01;//motor2Sevro ON
WeberYang 0:6a62ef9495ec 112 if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
WeberYang 0:6a62ef9495ec 113 if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
WeberYang 0:6a62ef9495ec 114 motor1 = abs(Rrpm);
WeberYang 0:6a62ef9495ec 115 motor2 = abs(Lrpm);
WeberYang 0:6a62ef9495ec 116
WeberYang 0:6a62ef9495ec 117 sendData[9] = (motor1>>8);//motor1speedH
WeberYang 0:6a62ef9495ec 118 sendData[10] = (motor1 & 0xFF);//motor1speedL
WeberYang 0:6a62ef9495ec 119 sendData[11] = (motor2>>8);//motor2speedH
WeberYang 0:6a62ef9495ec 120 sendData[12] = (motor2 & 0xFF);//motor2speedL
WeberYang 0:6a62ef9495ec 121 sendData[13] = End;
WeberYang 0:6a62ef9495ec 122 tmpCRC = CRC_Verify(sendData, 14);
WeberYang 0:6a62ef9495ec 123 sendData[14] = (tmpCRC & 0xFF);
WeberYang 0:6a62ef9495ec 124 sendData[15] = (tmpCRC>>8);
WeberYang 0:6a62ef9495ec 125 int i;
WeberYang 0:6a62ef9495ec 126 for (i=0;i<16;i++)
WeberYang 0:6a62ef9495ec 127 {
WeberYang 0:6a62ef9495ec 128 RS232.printf("%c",sendData[i]);
WeberYang 0:6a62ef9495ec 129 }
WeberYang 0:6a62ef9495ec 130 RS232.printf("\r\n");
WeberYang 0:6a62ef9495ec 131 }
WeberYang 0:6a62ef9495ec 132 void TwistToMotors()
WeberYang 0:6a62ef9495ec 133 {
WeberYang 0:6a62ef9495ec 134 float right,left;
WeberYang 0:6a62ef9495ec 135 double vel_data[2];
WeberYang 0:6a62ef9495ec 136 right = ( 1.0 * dx ) + (dr * w /2);
WeberYang 0:6a62ef9495ec 137 left = ( 1.0 * dx ) - (dr * w /2);
WeberYang 0:6a62ef9495ec 138 vel_data[0] = right*rate/Dimeter/60*1000;
WeberYang 0:6a62ef9495ec 139 vel_data[1] = left*rate/Dimeter/60*1000;
WeberYang 0:6a62ef9495ec 140 Sendmessage(vel_data[0],vel_data[1]);
WeberYang 0:6a62ef9495ec 141 VelAngular_R.data = vel_data[0];
WeberYang 0:6a62ef9495ec 142 VelAngular_L.data = vel_data[1];
WeberYang 0:6a62ef9495ec 143 //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
WeberYang 0:6a62ef9495ec 144 //}
WeberYang 0:6a62ef9495ec 145 //else{
WeberYang 0:6a62ef9495ec 146 pub_rmotor.publish( &VelAngular_R );
WeberYang 0:6a62ef9495ec 147 pub_lmotor.publish( &VelAngular_L );
WeberYang 0:6a62ef9495ec 148 //}
WeberYang 0:6a62ef9495ec 149 //RS232.printf("Wr = %.1f\n",vel_data[0]);
WeberYang 0:6a62ef9495ec 150 //RS232.printf("Wl = %.1f\n",vel_data[1]);
WeberYang 0:6a62ef9495ec 151 ticks_since_target += 1;
WeberYang 0:6a62ef9495ec 152
WeberYang 0:6a62ef9495ec 153 }
WeberYang 0:6a62ef9495ec 154
WeberYang 0:6a62ef9495ec 155 void messageCb(const geometry_msgs::Twist &msg)
WeberYang 0:6a62ef9495ec 156 {
WeberYang 0:6a62ef9495ec 157 ticks_since_target = 0;
WeberYang 0:6a62ef9495ec 158 dx = msg.linear.x;
WeberYang 0:6a62ef9495ec 159 dy = msg.linear.y;
WeberYang 0:6a62ef9495ec 160 dr = msg.angular.z;
WeberYang 0:6a62ef9495ec 161 //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr);
WeberYang 0:6a62ef9495ec 162 TwistToMotors();
WeberYang 0:6a62ef9495ec 163 }
WeberYang 0:6a62ef9495ec 164
WeberYang 0:6a62ef9495ec 165 void ACT( const std_msgs::String& ACT) {
WeberYang 0:6a62ef9495ec 166 char sendData[16];
WeberYang 0:6a62ef9495ec 167 //sendData = ACT;
WeberYang 0:6a62ef9495ec 168 int motor1,motor2;
WeberYang 0:6a62ef9495ec 169
WeberYang 0:6a62ef9495ec 170 sendData[0] = Start;
WeberYang 0:6a62ef9495ec 171 sendData[1] = Address;
WeberYang 0:6a62ef9495ec 172 sendData[2] = ReturnType;
WeberYang 0:6a62ef9495ec 173 sendData[3] = Clean;
WeberYang 0:6a62ef9495ec 174 sendData[4] = Reserve;
WeberYang 0:6a62ef9495ec 175 sendData[5] = 0x00;//motor1Sevro OFF
WeberYang 0:6a62ef9495ec 176 sendData[6] = 0x00;//motor2Sevro OFF
WeberYang 0:6a62ef9495ec 177 motor1 = 0;
WeberYang 0:6a62ef9495ec 178 motor2 = 0;
WeberYang 0:6a62ef9495ec 179 sendData[9] = (motor1>>8);//motor1speedH
WeberYang 0:6a62ef9495ec 180 sendData[10] = (motor1 & 0xFF);//motor1speedL
WeberYang 0:6a62ef9495ec 181 sendData[11] = (motor2>>8);//motor2speedH
WeberYang 0:6a62ef9495ec 182 sendData[12] = (motor2 & 0xFF);//motor2speedL
WeberYang 0:6a62ef9495ec 183 sendData[13] = End;
WeberYang 0:6a62ef9495ec 184
WeberYang 0:6a62ef9495ec 185
WeberYang 0:6a62ef9495ec 186 sendData[14] = Start;
WeberYang 0:6a62ef9495ec 187 sendData[15] = Start;
WeberYang 0:6a62ef9495ec 188 int i;
WeberYang 0:6a62ef9495ec 189 for (i=0;i<16;i++)
WeberYang 0:6a62ef9495ec 190 {
WeberYang 0:6a62ef9495ec 191 RS232.printf("%c",sendData[i]);
WeberYang 0:6a62ef9495ec 192 }
WeberYang 0:6a62ef9495ec 193 RS232.printf("\r\n");
WeberYang 0:6a62ef9495ec 194 }
WeberYang 0:6a62ef9495ec 195 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
WeberYang 0:6a62ef9495ec 196 ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT );
WeberYang 0:6a62ef9495ec 197
WeberYang 0:6a62ef9495ec 198 Timer t;
WeberYang 0:6a62ef9495ec 199 int main(int argc, char **argv)
WeberYang 0:6a62ef9495ec 200 {
WeberYang 0:6a62ef9495ec 201 long vel_timer = 0;
WeberYang 0:6a62ef9495ec 202 init_variables();
WeberYang 0:6a62ef9495ec 203 t.start();
WeberYang 0:6a62ef9495ec 204 nh.initNode();
WeberYang 0:6a62ef9495ec 205 nh.advertise(pub_lmotor);
WeberYang 0:6a62ef9495ec 206 nh.advertise(pub_rmotor);
WeberYang 0:6a62ef9495ec 207 nh.subscribe(cmd_vel_sub);
WeberYang 0:6a62ef9495ec 208 nh.subscribe(sub_action);
WeberYang 0:6a62ef9495ec 209
WeberYang 0:6a62ef9495ec 210 while (1)
WeberYang 0:6a62ef9495ec 211 {
WeberYang 0:6a62ef9495ec 212 if (t.read_ms() > vel_timer)
WeberYang 0:6a62ef9495ec 213 {
WeberYang 0:6a62ef9495ec 214 vel_timer = t.read_ms() + 500;
WeberYang 0:6a62ef9495ec 215 }
WeberYang 0:6a62ef9495ec 216 nh.spinOnce();
WeberYang 0:6a62ef9495ec 217
WeberYang 0:6a62ef9495ec 218 wait_ms(10);
WeberYang 0:6a62ef9495ec 219 }
WeberYang 0:6a62ef9495ec 220 }