NTHU機器人學 Team3 / Mbed 2 deprecated ROS_remote_car_2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_indigo

Fork of ROS_remote_car by NTHU機器人學 Team3

Files at this revision

API Documentation at this revision

Revision 2:a6604a2563df, committed 2018-06-15

Comitter:
KCHuang
Date:
Fri Jun 15 02:19:26 2018 +0000
Parent:
1:d24c3384bc59
Commit message:
77777

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Wed Apr 18 09:48:11 2018 +0000
+++ b/main.cpp	Fri Jun 15 02:19:26 2018 +0000
@@ -10,6 +10,8 @@
 #define Ts 0.01f    //period of timer1 (s)
 #define Servo_Period 20
 #define pi 3.14159265f
+#define U1TX D10
+#define U1RX D2
 //****************************************************************************** End of Define
  
 //****************************************************************************** I/O
@@ -31,7 +33,7 @@
 //LED
 DigitalOut led1(A4);
 DigitalOut led2(A5);
- 
+//Bluetooth
 //Timer Setting
 Ticker timer;
 
@@ -47,7 +49,7 @@
  
 //****************************************************************************** Variables
 // Servo
-float servo_duty = 0.03; // 0.025~0.113(-90~+90) 0.069->0 degree
+float servo_duty = 0.06; // 0.025~0.113(-90~+90) 0.069->0 degree
 // motor right
 int8_t HallA_state_R = 0;
 int8_t HallB_state_R = 0;
@@ -73,11 +75,17 @@
 float ctrl_output_L = 0.0f;
 float pwm2_duty = 0.0f;
 //servo
-int i = 0;
+float i = 0;
+int catch_ball = 0;
 //****************************************************************************** End of Variables
-
+//****************************************************************************** Bluetooth
+class HaseHardware : public MbedHardware
+{
+    public:
+        HaseHardware() : MbedHardware(U1TX, U1RX, 115200){};    
+};
 //****************************************************************************** ros related function
-ros:: NodeHandle nh;
+ros:: NodeHandle_<HaseHardware> nh;
 
 geometry_msgs::Twist vel_msg;
 ros::Publisher vel_feedback("feedback_wheel_angularVel", &vel_msg);
@@ -87,7 +95,7 @@
 {
     v_right = msg.y; //right wheel speed m/s 
     v_left = msg.x; //left wheel speed m/s
-    
+    catch_ball = msg.z;
     rotation_ref_R = -v_right/0.03f*60.0f/(2*pi); //  m/s to RPM
     rotation_ref_L = v_left/0.03f*60.0f/(2*pi); //  m/s to RPM
     //Limit the speed
@@ -151,7 +159,7 @@
     // Code for PI controller //
     rotation_err_R = rotation_ref_R - rotation_R;
     rotation_ierr_R += (rotation_err_R*Ts);
-    ctrl_output_R = 0.01f*rotation_err_R+ 0.2f*rotation_ierr_R; 
+    ctrl_output_R = 0.009f*rotation_err_R+ 0.2f*rotation_ierr_R; 
     ///////////////////////////
     
     if(ctrl_output_R >= 0.5f)ctrl_output_R = 0.5f;
@@ -166,41 +174,30 @@
     // Code for PI controller //
     rotation_err_L = rotation_ref_L - rotation_L;
     rotation_ierr_L += (rotation_err_L*Ts);
-    ctrl_output_L = 0.01f*rotation_err_L + 0.2f*rotation_ierr_L;
+    ctrl_output_L = 0.009f*rotation_err_L + 0.2f*rotation_ierr_L;
     ///////////////////////////      
     if(ctrl_output_L >= 0.5f)ctrl_output_L = 0.5f;
     else if(ctrl_output_L <= -0.5f)ctrl_output_L = -0.5f;
     pwm2_duty = ctrl_output_L + 0.5f;
     pwm2.write(pwm2_duty);
     TIM1->CCER |= 0x40;
-    /*
-    if(rotation_ierr_R > 100 || rotation_ierr_R < -100)
-    {
-        rotation_ierr_R = 0;
-    }
-    if(rotation_ierr_L > 100 || rotation_ierr_L < -100)
+   
+    //Servo
+    if(catch_ball == 1)
     {
-        rotation_ierr_L = 0;
-    } */   
-    //Servo
-    if(i==100) 
-    {
-        if(servo_duty < 0.07f)
+        if(servo_duty > 0.038f)
         {
-            servo_duty = servo_duty+0.04f/6;
-        }
-        else
-        {
-            servo_duty = 0.07f;            
+            servo_duty = servo_duty+i;
+            servo.write(servo_duty);
+            i-=0.0001f;
+            wait_ms(100);
         }
-        servo.write(servo_duty);
-        i=0;
     }
-    else
+    if(catch_ball == -1)
     {
-        i++;
+        servo_duty = 0.06f;
     }
-
+    servo.write(servo_duty);
 }
 //****************************************************************************** End of timer_interrupt