Weber Yang
AGV using ros with TROY motor
Dependencies: mbed ros_lib_kinetic
Diff: main.cpp
- Revision:
- 0:85456b971234
- Child:
- 2:fcf8a7739eed
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 12 00:59:24 2018 +0000
@@ -0,0 +1,541 @@
+#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;
+
+// ============= =============
+ float motor_rpm_r, motor_rpm_l;
+// ============= =============
+
+ ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
+ ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
+ int seq = 0; // monitor times of callbacks
+ int old_seq = 0; // monitor times of the command
+
+
+ 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.302;//0.2 ;//m
+ rate = 20;//50;
+ timeout_ticks = 2;
+ Dimeter = 0.127;//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");
+}
+
+
+int myabs( int a ){
+ if ( a < 0 ){
+ return -a;
+ }
+ return a;
+ }
+
+
+
+void TwistToMotors()
+{
+ old_seq = seq;
+ seq = seq + 1;
+ //int lower_bound = 100;
+ //int upper_bound = 300;
+ float right,left;
+// float motor_rpm_r, motor_rpm_l;
+ //double vel_data[2];
+ float vel_data[2];
+ float motor_rpm_vx, motor_rpm_theta, motor_rpm_v;
+
+ // prevent agv receive weird 1.0 command from cmd_vel
+
+ right = ( 1.0 * dx ) + (dr * w /2);
+ left = ( 1.0 * dx ) - (dr * w /2);
+ motor_rpm_v = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416);
+
+ if((motor_rpm_v!=0) && (myabs(motor_rpm_v)<100)){
+ if (motor_rpm_v > 0) motor_rpm_v=100;
+ else motor_rpm_v = -100;
+ }
+
+ motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416);
+
+
+ //motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
+ //motor_rpm_l = left*rate/(Dimeter/2)*60/(2*3.1416);
+
+ motor_rpm_r = motor_rpm_v + motor_rpm_theta;
+ motor_rpm_l = motor_rpm_v - motor_rpm_theta;
+
+
+ if (myabs(motor_rpm_r)<100 || myabs(motor_rpm_l)<100){
+
+ if(dx == 0){
+ if(dr > 0){
+ motor_rpm_r = 100;
+ motor_rpm_l = -100;
+ }else if( dr < 0 ){
+ motor_rpm_r = -100;
+ motor_rpm_l = 100;
+ }else{
+ motor_rpm_r = 0;
+ motor_rpm_l = 0;
+ }
+ }
+ else if( dx > 0 ){
+ if( myabs(motor_rpm_r) < 100 ){
+ motor_rpm_r = 100;
+ motor_rpm_l=motor_rpm_l+(100-motor_rpm_r);
+ }
+ if ( myabs(motor_rpm_l) < 100 ){
+ motor_rpm_l = 100;
+ motor_rpm_r=motor_rpm_r+(100-motor_rpm_l);
+ }
+ }else{
+ if ( myabs(motor_rpm_r) < 100 ){
+ motor_rpm_r = -100;
+ motor_rpm_l=motor_rpm_l+(-100-motor_rpm_r);
+ }
+
+ if (myabs(motor_rpm_l) < 100 ){
+ motor_rpm_l = -100;
+ motor_rpm_r=motor_rpm_r+(-100-motor_rpm_l);
+ }
+ }
+ }
+
+ /*
+ if ( myabs(motor_rpm_r) == myabs(motor_rpm_l) && myabs(motor_rpm_r) < 100){
+ int s = myabs(motor_rpm_r);
+
+ motor_rpm_r = motor_rpm_r / s * 101;
+ motor_rpm_l = motor_rpm_l / s * 101;
+
+ }
+*/
+
+/*
+ if ( myabs(vel_data[0]) < lower_bound )
+ { // if right wheel rpm < lower_bound
+ int a = 0;
+ a = lower_bound - myabs( vel_data[0] );
+
+ if (vel_data[0] > 0)
+ {
+ vel_data[0] = lower_bound;
+ }
+ else
+ {
+ vel_data[0] = -lower_bound;
+ }
+
+
+ if (vel_data[1] > 0)
+ {
+ vel_data[1] = vel_data[1] + a;
+ }
+ else
+ {
+ vel_data[1] = vel_data[1] - a;
+ } //
+
+ }
+
+ else if ( myabs(vel_data[0]) > upper_bound )
+ { // if right wheel rpm > upper_bound
+ int a = 0;
+ a = myabs( vel_data[0] ) - upper_bound;
+
+ if (vel_data[0] > 0)
+ {
+ vel_data[0] = upper_bound;
+ }
+ else
+ {
+ vel_data[0] = -upper_bound;
+ }
+
+
+ if (vel_data[1] > 0)
+ {
+ vel_data[1] = vel_data[1] - a;
+ }
+ else
+ {
+ vel_data[1] = vel_data[1] + a;
+ }
+ }
+
+
+ if ( myabs(vel_data[1]) < lower_bound )
+ { // if left wheel rpm < lower_bound
+ int a = 0;
+ a = lower_bound - myabs( vel_data[1] );
+
+ if (vel_data[1] > 0)
+ {
+ vel_data[1] = lower_bound;
+ }
+ else
+ {
+ vel_data[1] = -lower_bound;
+ }
+
+
+ if (vel_data[0] > 0)
+ {
+ vel_data[0] = vel_data[0] + a;
+ }
+ else
+ {
+ vel_data[0] = vel_data[0] - a;
+ } //
+ }
+
+
+ else if (myabs(vel_data[1]) > upper_bound )
+ { // if left wheel rpm > upper_bound
+ int a = 0;
+ a = myabs( vel_data[1] ) - upper_bound;
+
+ if (vel_data[1] > 0)
+ {
+ vel_data[1] = upper_bound;
+ }
+ else
+ {
+ vel_data[1] = -upper_bound;
+ }
+
+
+ if (vel_data[0] > 0)
+ {
+ vel_data[0] = vel_data[0] - a;
+ }
+ else
+ {
+ vel_data[0] = vel_data[0] + a;
+ }
+
+ }
+
+ */
+
+ /*
+ if ( myabs(motor_rpm_r) < 100 || myabs(motor_rpm_l) < 100 ){
+ float a = 0.0;
+
+ if ( myabs(motor_rpm_r) < myabs(motor_rpm_l) ){
+
+ if ( motor_rpm_r < 0 ){
+ a = -100 - motor_rpm_r;
+ }else if( motor_rpm_r > 0) {
+ a = 100 - motor_rpm_r;
+ }else {
+ a = 0;
+ }
+
+ motor_rpm_r = motor_rpm_r + a;
+ motor_rpm_l = motor_rpm_l + a;
+
+
+
+ }else if ( myabs(motor_rpm_r) > myabs(motor_rpm_l) ){
+
+ if ( motor_rpm_l < 0 ){
+ a = -100 - motor_rpm_l;
+ }else if( motor_rpm_l > 0) {
+ a = 100 - motor_rpm_l;
+ }else {
+ a = 0;
+ }
+
+ motor_rpm_l = motor_rpm_l + a;
+ motor_rpm_r = motor_rpm_r + a;
+
+ }
+
+ // go straight < 100
+ if ( motor_rpm_r == motor_rpm_l ){
+
+ if ( motor_rpm_l < 0 ){
+ motor_rpm_l = -100;
+ motor_rpm_r = -100;
+
+
+ }else if( motor_rpm_l > 0) {
+ motor_rpm_l = 100;
+ motor_rpm_r = 100;
+ }else {
+ motor_rpm_l = 0;
+ motor_rpm_r = 0;
+ }
+
+
+ }
+
+ // turn < 100
+
+ if ( motor_rpm_r == -motor_rpm_l ){
+
+ if ( motor_rpm_l < 0 ){
+ motor_rpm_l = -100;
+ motor_rpm_r = 100;
+ }else if( motor_rpm_l > 0) {
+ motor_rpm_l = 100;
+ motor_rpm_r = -100;
+ }else {
+ motor_rpm_l = 0;
+ motor_rpm_r = 0;
+ }
+
+ }
+ //
+
+ }
+
+
+
+ if ( myabs(motor_rpm_r) > 600 ){
+
+ if ( motor_rpm_r < 0 ){
+ motor_rpm_r = -100 * log( float(myabs(motor_rpm_r)) );
+ }else{
+ motor_rpm_r = 100 * log( float(myabs(motor_rpm_r)) );
+ }
+
+
+
+ }
+
+ if ( myabs(motor_rpm_l) > 600) {
+
+ if ( motor_rpm_l < 0 ){
+ motor_rpm_l = -100 * log( float(myabs(motor_rpm_l)) );
+ }else{
+ motor_rpm_l = 100 * log( float(myabs(motor_rpm_l)) );
+ }
+ }
+
+ */
+ vel_data[0] = motor_rpm_r;
+ vel_data[1] = motor_rpm_l;
+
+
+
+
+
+//===================================================================
+ //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 (true)
+ {
+ //if (t.read_ms() > vel_timer)
+ //{
+ // vel_timer = t.read_ms() + 50;
+ // TwistToMotors();
+ // }
+
+
+ nh.spinOnce();
+
+
+ if (seq +50 < old_seq){
+ motor_rpm_r = 0;
+ motor_rpm_l = 0;
+ }
+
+ Sendmessage(motor_rpm_r,motor_rpm_l);
+
+ old_seq = old_seq + 1;
+
+ wait_ms(10);
+
+ }
+}