![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
using NucleoF103
Dependencies: mbed ros_lib_kinetic
Revision 0:576de68cbb45, committed 2018-04-12
- Comitter:
- WeberYang
- Date:
- Thu Apr 12 00:53:17 2018 +0000
- Commit message:
- SMT_weber
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Apr 12 00:53:17 2018 +0000 @@ -0,0 +1,220 @@ +#include "mbed.h" +#include <ros.h> +#include <ros/time.h> +#include <std_msgs/Empty.h> +#include <std_msgs/String.h> +#include<geometry_msgs/Twist.h> //set buffer larger than 50byte +#include <std_msgs/Float32.h> +#include <math.h> +#include <stdio.h> +#include <string.h> +#include <iostream> +#define Start 0xAA +#define Address 0x7F +#define ReturnType 0x00 +#define Clean 0x00 +#define Reserve 0x00 +#define End 0x55 + + Serial RS232 (PB_10, PB_11,115200); // This one works + std_msgs::String str_msg; + std_msgs::Float32 VelAngular_L; + std_msgs::Float32 VelAngular_R; + ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L); + ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R); + + ros::NodeHandle nh; + geometry_msgs::Twist vel; + //float left; + //float right; + float Lrpm,Rrpm; + float ticks_since_target; + double timeout_ticks; + + double w; + double rate; + double Dimeter; + float dx,dy,dr; + void init_variables(); +void Sendmessage(float Rrpm,float Lrpm); + +unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen) +{ + unsigned int i, j; + //#define wPolynom 0xA001 + unsigned int wCrc = 0xffff; + unsigned int wPolynom = 0xA001; + /*---------------------------------------------------------------------------------*/ + for (i = 0; i < iBufLen; i++) + { + wCrc ^= cBuffer[i]; + for (j = 0; j < 8; j++) + { + if (wCrc &0x0001) + { + wCrc = (wCrc >> 1) ^ wPolynom; + } + else + { + wCrc = wCrc >> 1; + } + } + } + return wCrc; +} + + /* + 起始碼(1Byte): AA +地址碼(1Byte): 01 +返回數據類型(1Byte):從站驅動器不返回數據 00 +故障清除(1Byte)默認發送: 00 +保留位(1Byte) 00 +A 馬達過載控制(1Byte)驅動器使能狀態 01 +B 馬達控制(1Byte) 01 +A 運行方向(1Byte)00:正轉,01反轉 00 +B 運行方向(1Byte)00:正轉,01反轉 00 +運行轉速(2Byte)是100–3000r/min, 12 34 +運行轉速(2Byte)是100–3000r/min, 12 34 +第 1 個 Byte:速度值高 8 位; +第 2 個 Byte:速度值低 8 位; +無效(2Byte)00:無效數據位。 00 +結束碼(1Byte)55:通訊結束編碼識別。 55 +校驗碼(2Byte)CRC16 + */ + + // void spinOnce(); + // void twistCallback(const geometry_msgs::Twist &twist_aux); + + +void init_variables() +{ + dx = dy = dr =0; + w = 0.2 ; + rate = 50; + timeout_ticks = 2; + Dimeter = 0.15; +} +void Sendmessage(float Rrpm,float Lrpm) +{ + //RS232.printf("Wr = %.1f\n",Rrpm); + //RS232.printf("Wl = %.1f\n",Lrpm); +unsigned char sendData[16]; +unsigned int tmpCRC; +int motor1,motor2; + + sendData[0] = Start; + sendData[1] = Address; + sendData[2] = ReturnType; + sendData[3] = Clean; + sendData[4] = Reserve; + sendData[5] = 0x01;//motor1Sevro ON + sendData[6] = 0x01;//motor2Sevro ON +if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;} +if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;} + motor1 = abs(Rrpm); + motor2 = abs(Lrpm); + + sendData[9] = (motor1>>8);//motor1speedH + sendData[10] = (motor1 & 0xFF);//motor1speedL + sendData[11] = (motor2>>8);//motor2speedH + sendData[12] = (motor2 & 0xFF);//motor2speedL + sendData[13] = End; + tmpCRC = CRC_Verify(sendData, 14); + sendData[14] = (tmpCRC & 0xFF); + sendData[15] = (tmpCRC>>8); + int i; + for (i=0;i<16;i++) + { + RS232.printf("%c",sendData[i]); + } + RS232.printf("\r\n"); +} +void TwistToMotors() +{ + float right,left; + double vel_data[2]; + right = ( 1.0 * dx ) + (dr * w /2); + left = ( 1.0 * dx ) - (dr * w /2); + vel_data[0] = right*rate/Dimeter/60*1000; + vel_data[1] = left*rate/Dimeter/60*1000; + Sendmessage(vel_data[0],vel_data[1]); + VelAngular_R.data = vel_data[0]; + VelAngular_L.data = vel_data[1]; + //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){ + //} + //else{ + pub_rmotor.publish( &VelAngular_R ); + pub_lmotor.publish( &VelAngular_L ); + //} + //RS232.printf("Wr = %.1f\n",vel_data[0]); + //RS232.printf("Wl = %.1f\n",vel_data[1]); + ticks_since_target += 1; + +} + +void messageCb(const geometry_msgs::Twist &msg) +{ + ticks_since_target = 0; + dx = msg.linear.x; + dy = msg.linear.y; + dr = msg.angular.z; + //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr); + TwistToMotors(); +} + +void ACT( const std_msgs::String& ACT) { +char sendData[16]; +//sendData = ACT; +int motor1,motor2; + + sendData[0] = Start; + sendData[1] = Address; + sendData[2] = ReturnType; + sendData[3] = Clean; + sendData[4] = Reserve; + sendData[5] = 0x00;//motor1Sevro OFF + sendData[6] = 0x00;//motor2Sevro OFF + motor1 = 0; + motor2 = 0; + sendData[9] = (motor1>>8);//motor1speedH + sendData[10] = (motor1 & 0xFF);//motor1speedL + sendData[11] = (motor2>>8);//motor2speedH + sendData[12] = (motor2 & 0xFF);//motor2speedL + sendData[13] = End; + + + sendData[14] = Start; + sendData[15] = Start; + int i; + for (i=0;i<16;i++) + { + RS232.printf("%c",sendData[i]); + } + RS232.printf("\r\n"); +} +ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb); +ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT ); + + Timer t; +int main(int argc, char **argv) +{ + long vel_timer = 0; + init_variables(); + t.start(); + nh.initNode(); + nh.advertise(pub_lmotor); + nh.advertise(pub_rmotor); + nh.subscribe(cmd_vel_sub); + nh.subscribe(sub_action); + + while (1) + { + if (t.read_ms() > vel_timer) + { + vel_timer = t.read_ms() + 500; + } + nh.spinOnce(); + + wait_ms(10); + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 12 00:53:17 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/7130f322cb7e \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Thu Apr 12 00:53:17 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f