Diff: main.cpp

Revision:

1:c0a3e4589a8f

Parent:

0:83b2c6a67cce

Child:

2:b728a6564520

--- a/main.cpp	Thu Jul 08 03:46:29 2021 +0000
+++ b/main.cpp	Thu Jul 08 11:48:58 2021 +0000
@@ -13,11 +13,11 @@
 DigitalOut myled2(LED2);
 DigitalOut myled3(LED3);
 DigitalOut myled4(LED4);
-float arm_num=0,arm_deg=0;
+float arm_num=0,arm_rad=0;
 void cArmCallback(const geometry_msgs::Point &arm_state)
 {
-    arm_num = arm_state.x;
-    arm_deg = arm_state.y;
+    arm_num = arm_state.x;        //右回転＝０、左回転＝１
+    arm_rad = arm_state.y;        //目標角度
 }
 ros::NodeHandle  nh;
 ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
@@ -26,12 +26,11 @@
 #ifndef M_PI
 #define M_PI 3.14159265359f
 #endif
-#define DELTA_T 0.01    //制御周期
-#define DUTY_MAX 0.07     //duty比の最大値
-#define OMEGA_MAX 0.6    //速度制御を利用した角度制御での角速度の最大値
+#define DELTA_T 0.01        //制御周期
+#define DUTY_MAX 0.07       //duty比の最大値
+#define OMEGA_MAX 0.6       //速度制御を利用した角度制御での角速度の最大値
 #define NUM_DATA 270
 
-Serial pc(USBTX, USBRX);
 ////角度制御(アブソリュートエンコーダ)/////
 DigitalOut myled1(LED1);
 Timer timer;
@@ -52,8 +51,9 @@
 double servoDeg(double servo_value);
 double calServoDeg(double servo_value);     //360°の度数法にする
 int id_c1 = 0;
-float deg_c_grab = 9000;
-float deg_c_open = 3500;
+float deg_c_grab = 10000;
+float deg_c_open = 7500;
+int hand = 0;
 
 
 ////データ記録まわり/////
@@ -66,7 +66,7 @@
 {
     myled1 = 0;     //アブソリュートエンコーダ用if文中で点灯
     /////////       ROS       /////////
-    nh.getHardware()->setBaud(115200);
+    nh.getHardware()->setBaud(921600);
     nh.initNode();
     nh.subscribe(sub);
 
@@ -81,33 +81,29 @@
     while (1) {
         ////    ROS     ////
         nh.spinOnce();
-        if(arm_num==1&&arm_deg==60) {       //LEDは動作確認用
+        target_rad = arm_rad;
+        if(arm_num==1&&arm_rad==-(M_PI/6)) {       //LEDは動作確認用
             myled2 = !myled2;
         }
-        if(arm_num == 1) {
-            myled3 = 1;
-        }
-        if(arm_deg == 60) {
-            myled4 = 1;
-        }
-        
+
         ////タッチセンサ////
         if(ts_c1 == 0) {      //タッチセンサ反応したらハンドルを掴む
             wait_us(50);
             if(ts_c1 == 0) {
-                servo_c.set_degree(0,servoDeg(4500));   //servo_c掴む
+                servo_c.set_degree(0,servoDeg(10000));   //servo_c掴む
+                hand = 1;
             }
         }
 
         ////アブソリュートエンコーダ////
         if(timer.read()>DELTA_T) {
             if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
-                myled1=1;   //LED2 点灯
+                myled1=!myled1;   //LED2 点灯
                 motor.AcOmega(target_rad);   //速度式角度制御
             } else { //目標角度に達したら停止
                 myled1=0;   //LED2 消灯
                 motor.stop();
-                servo_c.set_degree(0,servoDeg(1000));   //servo_c離す
+                servo_c.set_degree(0,servoDeg(7500));   //servo_c離す
             }
             amt.rewriteCount();
             amt.calOmega();