Revision 8:31d4dd20cfc0, committed 2019-04-17

Comitter:

Tanakacool

Date:

Wed Apr 17 09:11:00 2019 +0000

Parent:

7:46bb04ae559c

Commit message:

test

Changed in this revision

--- a/main.cpp	Wed Mar 27 05:27:59 2019 +0000
+++ b/main.cpp	Wed Apr 17 09:11:00 2019 +0000
@@ -1,21 +1,70 @@
 #include "mbed.h"
+#include "uagv.h"
 
 #define LEFT 0
 #define RIGHT 1
 
-#define WHEEL_SEPARATION 4
+
+/*******************************************************************************
+  ROS NodeHandle
+*******************************************************************************/
+ros::NodeHandle nh;
+
+/*******************************************************************************
+  Publisher
+*******************************************************************************/
+std_msgs::UInt8 lift_status;                                      // 0 = unexpected result or error, 1 = lift up, 2 = lift down, 3 = halt, 4 = reserved for UAGV onload status
+ros::Publisher lift_status_pub("liftStatus", &lift_status);
+
+/*******************************************************************************
+  Subscriber
+*******************************************************************************/
+ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", commandVelocityCallback);
+ros::Subscriber<std_msgs::UInt8> lift_cmd_sub("liftCmd", liftCmdCallback);
+
+/*******************************************************************************
+  Declaration for motor & BT command
+*******************************************************************************/
+float mDesiredDirection, mDesiredVelocity; //從BT來的command：角度-1~1, 速度 -1~1
+float mMaxVelocity = 0.5; //最高速 = 0.5m/s
+float mCurrentVelocity, mCurrentDirection; //目前的速度和角度
+float safeVelocity = 1;
 
-//DigitalOut led_red(LED_RED);
+float goal_linear_velocity  = 0.0;
+float goal_angular_velocity = 0.0;
+
+short VELOCITY_CONSTANT_VAULE = 30000;    //cmd:16585~=1(m/s)
+short LIMIT_X_MAX_VELOCITY = 18000;      //max cmd:8293~=0.5m/s
+short LIMIT_X_MIN_VELOCITY = 1640;       //min value
 
-DigitalOut led1(LED1);
+int lin_vel1;  //左輪
+int lin_vel2;  //右輪
+int lin_vel1_t;  //左輪
+int lin_vel2_t;  //右輪
+long LencV_0;  //左輪
+long RencV_0;  //右輪
+long LencV_t;  //左輪
+long RencV_t;  //右輪
 
+/*******************************************************************************
+  Declaration for LED
+*******************************************************************************/
+int brightness = 0;
+int fadeAmount = 5;
+
+/*******************************************************************************
+  Declaration for control flags & other parameters
+*******************************************************************************/
+bool i2cinitial = true;
+uint32_t delaytime;
+ 
 DigitalOut LIFT_UP_PIN(D4);
 DigitalOut LIFT_DOWN_PIN(D3);
 //InterruptIn sw2(B1);
 //I2C i2c( PTB1, PTB0);
 I2C i2c( D14, D15);
-
-Serial serialPort(USBTX, USBRX);
+ 
+//Serial serialPort(USBTX, USBRX);
 //Serial serialPort( PTE0, PTE1);
 
 char Rmotobuffer[3];
@@ -23,53 +72,47 @@
 
 int Right_ADDR = 0x14<<1;
 int Left_ADDR  = 0x15<<1;
-
-int lin_vel1;  //左輪
-int lin_vel2;  //右輪
-
-int VELOCITY_CONSTANT_VAULE= 17000;
-int LIMIT_X_MAX_VELOCITY = 8293;      //max cmd:8293~=0.5m/s
-int LIMIT_X_MIN_VELOCITY = 1640;       //min value
-
-/*
-void sw2_release(void)
-{
-    led_red = !led_red;
-    serialPort.printf("On-board button SW2 was released.\n");
-}
-*/
-void Driving_Right(int rightOutPut);
-void Driving_Left(int leftOutPut);
-
-void liftup(int stage);
-
-void controlMotorSpeed(float goalLinearVel, float goalAngularVel);
+Timer t;
+long last_debounce_time=0;
 
 int main()
 {
-
-    //sw2.rise(&sw2_release);
-    serialPort.baud(9600);
-    serialPort.printf("WPI Serial Port Started\n");
-
+    // Initializing Mbed and public parameters
+    t.start();
     i2c.frequency(100000);
     wait_ms(100);
-
-    int motorOutPut=0;
-
-    Driving_Right(motorOutPut);
-    wait_ms(100);
-    Driving_Left(motorOutPut);
     
-    liftup(0);
-
-    while (true) {
-
-
-
+    // Initializing ROS connection
+    nh.getHardware()->setBaud(57600);
+    nh.initNode();
+    nh.subscribe(cmd_vel_sub);
+    nh.advertise(lift_status_pub);
+    nh.subscribe(lift_cmd_sub);
+    nh.loginfo("Initialized done.");
+    while(true){
+        if(t.read_ms() - last_debounce_time>(1000/CMD_VEL_PUBLISH_PERIOD)){
+            Driving_Left(lin_vel1);
+            wait_ms(10);
+            Driving_Right(lin_vel2);
+            last_debounce_time = t.read_ms();
+        }
+        nh.spinOnce();
     }
 }
 
+/*******************************************************************************
+  Callback function for cmd_vel msg
+*******************************************************************************/
+void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg)
+{
+  goal_linear_velocity  = cmd_vel_msg.linear.x;
+  goal_angular_velocity = cmd_vel_msg.angular.z;
+  controlMotorSpeed(goal_linear_velocity, goal_angular_velocity);
+}
+
+/*******************************************************************************
+  Converter of motor command
+*******************************************************************************/
 void controlMotorSpeed(float goalLinearVel, float goalAngularVel)
 {
     float wheel_speed_cmd[2];
@@ -82,43 +125,54 @@
 
     if (lin_vel1 > LIMIT_X_MAX_VELOCITY)       lin_vel1 =  LIMIT_X_MAX_VELOCITY;
     else if (lin_vel1 < LIMIT_X_MIN_VELOCITY && lin_vel1 > 0 ) lin_vel1 = LIMIT_X_MIN_VELOCITY;
-
     else if (lin_vel1 < -LIMIT_X_MAX_VELOCITY) lin_vel1 = -LIMIT_X_MAX_VELOCITY;
     else if (lin_vel1 > -LIMIT_X_MIN_VELOCITY && lin_vel1 < 0) lin_vel1 = -LIMIT_X_MIN_VELOCITY;
+    lin_vel1 = lin_vel1_t * 0.3 + lin_vel1 * 0.7;
 
     lin_vel2 = wheel_speed_cmd[RIGHT] * VELOCITY_CONSTANT_VAULE;
     wait_ms(2);
 
-
     if (lin_vel2 > LIMIT_X_MAX_VELOCITY)       lin_vel2 =  LIMIT_X_MAX_VELOCITY;
     else if (lin_vel2 < LIMIT_X_MIN_VELOCITY && lin_vel2 > 0 ) lin_vel2 = LIMIT_X_MIN_VELOCITY;
-
     else if (lin_vel2 < -LIMIT_X_MAX_VELOCITY) lin_vel2 = -LIMIT_X_MAX_VELOCITY;
     else if (lin_vel2 > -LIMIT_X_MIN_VELOCITY && lin_vel2 < 0) lin_vel2 = -LIMIT_X_MIN_VELOCITY;
-
+    lin_vel2 = lin_vel2_t * 0.3 + lin_vel2 * 0.7;
 }
-void liftup(int stage)
+
+/*******************************************************************************
+  Callback function for lift_cmd msg
+*******************************************************************************/
+void liftCmdCallback(const std_msgs::UInt8& lift_cmd_msg)
 {
-    switch (stage) {
-        case 0:         //stop
-            LIFT_UP_PIN=0;
-            LIFT_DOWN_PIN=0;
-
-            break;
+    wait_ms(100);
+    LIFT_UP_PIN=0;
+    LIFT_DOWN_PIN=0;
+    wait_ms(2000);
+    switch (lift_cmd_msg.data) {
         case 1:         //up
             LIFT_UP_PIN=1;
             LIFT_DOWN_PIN=0;
-
+ 
             break;
         case 2:         //down
             LIFT_UP_PIN=0;
             LIFT_DOWN_PIN=1;
-
+ 
+            break;
+        case 3:         //stop
+            LIFT_UP_PIN=0;
+            LIFT_DOWN_PIN=0;
+            
             break;
+        default:         //stop
+            LIFT_UP_PIN=0;
+            LIFT_DOWN_PIN=0;
+            
+            break; 
+    }
+ 
+}
 
-    }
-
-}
 void Driving_Right(int rightOutPut)
 {
     Rmotobuffer[0]= 0x00;
@@ -131,7 +185,7 @@
     i2c.write(Rmotobuffer[2]);
     i2c.stop();
 }
-
+ 
 void Driving_Left(int leftOutPut)
 {
     Lmotobuffer[0]= 0x00;
@@ -143,6 +197,6 @@
     i2c.write(Lmotobuffer[1]);
     i2c.write(Lmotobuffer[2]);
     i2c.stop();
-
+ 
 }
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_indigo.lib	Wed Apr 17 09:11:00 2019 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/garyservin/code/ros_lib_indigo/#fd24f7ca9688

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uagv.h	Wed Apr 17 09:11:00 2019 +0000
@@ -0,0 +1,55 @@
+#ifndef UAGV_CORE_CONFIG_H_
+#define UAGV_CORE_CONFIG_H_
+
+#include <math.h>
+
+#include <ros.h>
+#include <ros/time.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/UInt8.h>
+#include <geometry_msgs/Twist.h>
+
+#define CMD_VEL_PUBLISH_PERIOD           20.0   //hz
+#define DRIVE_INFORMATION_PUBLISH_PERIOD 30.0   //hz
+
+#define WHEEL_RADIUS                    0.09           // meter
+#define WHEEL_SEPARATION                0.46           // meter (BURGER => 0.16, WAFFLE => 0.287)
+#define ROBOT_RADIUS                    0.294           // meter (BURGER => 0.078, WAFFLE => 0.294)
+#define ENCODER_MIN                     -2147483648     // raw
+#define ENCODER_MAX                     2147483648      // raw
+
+#define LEFT                            0
+#define RIGHT                           1
+
+#define VELOCITY_CONSTANT_VALUE         1263.632956882  // V = r * w = r * RPM * 0.10472
+//   = 0.033 * 0.229 * Goal RPM * 0.10472
+// Goal RPM = V * 1263.632956882
+
+#define MAX_LINEAR_VELOCITY             0.22   // m/s
+#define MAX_ANGULAR_VELOCITY            2.84   // rad/s
+#define VELOCITY_STEP                   0.01   // m/s
+#define VELOCITY_LINEAR_X               0.01   // m/s
+#define VELOCITY_ANGULAR_Z              0.1    // rad/s
+#define SCALE_VELOCITY_LINEAR_X         1
+#define SCALE_VELOCITY_ANGULAR_Z        1
+
+#define TICK2RAD                        0.000628318  // 2 * 3.14159265359 / 10000 = 0.000628318f
+
+#define DEG2RAD(x)                      (x * 0.01745329252)  // *PI/180
+#define RAD2DEG(x)                      (x * 57.2957795131)  // *180/PI
+
+#define TEST_DISTANCE                   0.300     // meter
+#define TEST_RADIAN                     3.14      // 180 degree
+
+#define WAIT_FOR_BUTTON_PRESS           0
+#define WAIT_SECOND                     1
+#define CHECK_BUTTON_RELEASED           2
+
+void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg);
+void liftCmdCallback(const std_msgs::UInt8& lift_cmd_msg);
+void Driving_Right(int rightOutPut);
+void Driving_Left(int leftOutPut);
+void controlMotorSpeed(float goalLinearVel, float goalAngularVel);
+
+#endif // UAGV_CORE_CONFIG_H_
\ No newline at end of file