Diff: main.cpp

Revision:

6:4e03a22f2abb

Parent:

5:221ce6ba655c

--- a/main.cpp	Tue Mar 26 01:16:55 2019 +0000
+++ b/main.cpp	Wed Apr 17 06:02:13 2019 +0000
@@ -1,18 +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];
@@ -20,49 +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 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);
-
-    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];
@@ -75,20 +125,52 @@
 
     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;
+}
 
+/*******************************************************************************
+  Callback function for lift_cmd msg
+*******************************************************************************/
+void liftCmdCallback(const std_msgs::UInt8& lift_cmd_msg)
+{
+    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)
@@ -103,7 +185,7 @@
     i2c.write(Rmotobuffer[2]);
     i2c.stop();
 }
-
+ 
 void Driving_Left(int leftOutPut)
 {
     Lmotobuffer[0]= 0x00;
@@ -115,5 +197,6 @@
     i2c.write(Lmotobuffer[1]);
     i2c.write(Lmotobuffer[2]);
     i2c.stop();
+ 
 }