Diff: main.cpp

Revision:

4:3abefa13c2e3

Parent:

3:78b608ba0286

Child:

5:4fba3e91741f

--- a/main.cpp	Thu Jul 08 18:20:31 2021 +0000
+++ b/main.cpp	Sat Jul 10 06:19:53 2021 +0000
@@ -37,16 +37,15 @@
 DigitalIn ts_c2(p7, PullUp);
 KondoServo servo_c(p13, p14);
 double servoDeg(double servo_value);
-double calServoDeg(double servo_value);     //360°の度数法にする
 int id_c1 = 0;
 float deg_c_grab = 10000;
 float deg_c_open = 7500;
-int hand = 0;
+int hand = 0;   //タッチセンサがONになった状態:1，OFF:0
 
 ////データ記録まわり/////
 int angle_count=0;
 int omega_count=0;
-double now_deg=0;    //度数法
+double now_deg=0;
 double now_count=0;
 
 /////////////////               ROS               /////////////////
@@ -81,49 +80,46 @@
     while (1) {
         ////    ROS     ////
         nh.spinOnce();
-        if(arm_num==1&&arm_deg==30) {       //LEDは動作確認用
-            myled2 = !myled2;
-            arm_rad = degToRad(arm_deg);
-            target_rad = -30;
-            target_deg = radToDeg(target_rad);
+        if(arm_num==1&&arm_deg==30) {       //動作確認用
+            myled2 = 1;
         }
+        arm_rad = degToRad(arm_deg);
+        target_rad = -arm_rad;
+        target_deg = -arm_deg;
+
 
         ////タッチセンサ////
         if(ts_c1 == 0) {      //タッチセンサ反応したらハンドルを掴む
             wait_us(50);
             if(ts_c1 == 0) {
-                servo_c.set_degree(0,servoDeg(10000));   //servo_c掴む
+                servo_c.set_degree(0,servoDeg(deg_c_grab));   //servo_c掴む
                 hand = 1;
             }
-        } else {
-            hand = 0;
         }
 
         ////アブソリュートエンコーダ////
         if(timer.read()>DELTA_T) {
             if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
-                myled1=1;   //LED1 点灯
-                if(hand == 1) {
-                    myled4=1;   //LED4 点灯
+                myled1 = 1;   //LED1 点灯
+                if(hand == 1) {     //タッチセンサが反応していたとき(ハンドルを掴んでいたとき)
                     motor.AcOmega(target_rad);   //速度式角度制御
                 } else if(hand == 0) {
-                    myled4=0;
+                    myled4=1;   //LED4 点灯
                 }
             } else { //目標角度に達したら停止
                 myled1=0;   //LED1 消灯
                 motor.stop();
-                myled4=0;   //LED4 点灯
+                hand = 0;
                 //wait(100);    //table停止用時間
-                servo_c.set_degree(0,servoDeg(7500));   //servo_c離す
+                servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
             }
             amt.rewriteCount();
             amt.calOmega();
             amt.getOmega();
-            now_deg = amt.getDeg();    //度数法
+            now_deg = amt.getDeg();
             now_count=amt.getCount();
-            motor.AcOmega(target_rad);
+//            motor.AcOmega(target_rad);
 
-//            pc.printf("now_deg = %f,%f\r\n", now_deg,now_count);       //現在角度取得 (確認以外ではコメントアウトしないとモーター回らない)////////////////////
             timer.reset();
         }
         wait_us(10);