Diff: main.cpp

Revision:

2:b728a6564520

Parent:

1:c0a3e4589a8f

Child:

3:78b608ba0286

--- a/main.cpp	Thu Jul 08 11:48:58 2021 +0000
+++ b/main.cpp	Thu Jul 08 12:55:18 2021 +0000
@@ -5,22 +5,9 @@
 #include "KondoServo.h"
 #include "MotorController.h"
 #include <stdlib.h>
-
-/////////////////               ROS               /////////////////
 #include <ros.h>
 #include <std_msgs/String.h>
 #include <geometry_msgs/Point.h>
-DigitalOut myled2(LED2);
-DigitalOut myled3(LED3);
-DigitalOut myled4(LED4);
-float arm_num=0,arm_rad=0;
-void cArmCallback(const geometry_msgs::Point &arm_state)
-{
-    arm_num = arm_state.x;        //右回転＝０、左回転＝１
-    arm_rad = arm_state.y;        //目標角度
-}
-ros::NodeHandle  nh;
-ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
 
 /////////////////               宣言部分                  //////////////////////
 #ifndef M_PI
@@ -43,6 +30,7 @@
 double target_rad=0;
 double target_deg = 0;
 double radToDeg(double x);
+double degToRad(double x);
 
 ////先端アーム(サーボ)/////
 DigitalIn ts_c1(p6, PullUp);
@@ -55,13 +43,25 @@
 float deg_c_open = 7500;
 int hand = 0;
 
-
 ////データ記録まわり/////
 int angle_count=0;
 int omega_count=0;
 double now_deg=0;    //度数法
 double now_count=0;
 
+/////////////////               ROS               /////////////////
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+float arm_num=0,arm_deg=0,arm_rad=0;
+void cArmCallback(const geometry_msgs::Point &arm_state)
+{
+    arm_num = arm_state.x;        //右回転＝1、左回転＝2
+    arm_deg = arm_state.y;        //目標角度
+}
+ros::NodeHandle  nh;
+ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
+
 int main()
 {
     myled1 = 0;     //アブソリュートエンコーダ用if文中で点灯
@@ -75,15 +75,17 @@
 
     motor.setEquation(0.0523,0.0148,0.0459,-0.0341);    //Excel"omega_controll"の図より
 
-//    target_rad = -(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）           //////////////////////////
-//    target_deg = radToDeg(target_rad);
+    target_rad = -(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）           //////////////////////////
+    target_deg = radToDeg(target_rad);
 
     while (1) {
         ////    ROS     ////
         nh.spinOnce();
-        target_rad = arm_rad;
-        if(arm_num==1&&arm_rad==-(M_PI/6)) {       //LEDは動作確認用
+        if(arm_num==1&&arm_deg==30) {       //LEDは動作確認用
             myled2 = !myled2;
+            myled3 = !myled3;
+            arm_rad = degToRad(arm_deg);
+//            target_rad = arm_rad;
         }
 
         ////タッチセンサ////
@@ -98,11 +100,12 @@
         ////アブソリュートエンコーダ////
         if(timer.read()>DELTA_T) {
             if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
-                myled1=!myled1;   //LED2 点灯
+                myled1=1;   //LED2 点灯
                 motor.AcOmega(target_rad);   //速度式角度制御
             } else { //目標角度に達したら停止
                 myled1=0;   //LED2 消灯
                 motor.stop();
+                //wait(100);    //table停止用時間
                 servo_c.set_degree(0,servoDeg(7500));   //servo_c離す
             }
             amt.rewriteCount();
@@ -127,4 +130,8 @@
 double radToDeg(double x)
 {
     return x*180/M_PI;
+}
+double degToRad(double x)
+{
+    return x*M_PI/180;
 }
\ No newline at end of file