Revision 4:e20532e09d42, committed 2021-09-19

Comitter:

oshin1030

Date:

Sun Sep 19 13:33:57 2021 +0000

Parent:

3:ab266b418d90

Commit message:

9/19

Changed in this revision

diff -r ab266b418d90 -r e20532e09d42 MotorController_AbsEC.lib
--- a/MotorController_AbsEC.lib	Fri Sep 10 09:02:52 2021 +0000
+++ b/MotorController_AbsEC.lib	Sun Sep 19 13:33:57 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/oshin1030/code/MotorController_AbsEC_right_arm/#6bff84284ecb
+https://os.mbed.com/users/oshin1030/code/MotorController_AbsEC_right_arm/#2e5f40e761bd

diff -r ab266b418d90 -r e20532e09d42 main.cpp
--- a/main.cpp	Fri Sep 10 09:02:52 2021 +0000
+++ b/main.cpp	Sun Sep 19 13:33:57 2021 +0000
@@ -3,8 +3,8 @@
 #include "CalPID.h"
 #include "MotorController.h"
 #include "KondoServo.h"
-//#include <ros.h>
-//#include <geometry_msgs/Point.h>
+#include <ros.h>
+#include <geometry_msgs/Point.h>
 
 
 #define DELTA_T 0.01    //制御周期
@@ -53,20 +53,6 @@
     return rad*180/M_PI;
 }
 
-void angleControll()
-{
-    //motorA.AcOmega_1(target_rad);    //duty式角度制御
-    //motorB.AcOmega_2(target_rad);   //速度式角度制御
-}
-
-
-double function(double x)
-{
-    double y;
-    y = 0.6 + sqrt(0.01 - (x-0.6)*(x-0.6));//円の軌道
-    return y;
-}
-
 double theta1(double x, double y, double theta_2)//原点角度計算
 {
     //(x:目標座標 y:目標座標)
@@ -84,77 +70,69 @@
     return theta_2;
 }
 
-double robot_x, robot_y; //ロボット座標
+double robot_x, robot_y,robot_theta; //ロボット座標
 double pot_x, pot_y; //ポット座標
-int fan_switch = -1, arm_switch = 0; //扇風機on/off, アーム関節on/off
+int fan_switch = -1, arm_switch =0; //扇風機on/off, アーム関節on/off
+
+void robotCallback(const geometry_msgs::Point &robot_pos)
+{
+    robot_x = robot_pos.x;
+    robot_y = robot_pos.y;
+    robot_theta = robot_pos.z;
 
-//void robotCallback(const geometry_msgs::Point &robot_pos)
-//{
-//    robot_x = robot_pos.x;
-//    robot_y = robot_pos.y;
-//
-//}
-//void potCallback(const geometry_msgs::Point &pot_pos)
-//{
-//    pot_x = pot_pos.x;
-//    pot_y = pot_pos.y;
-//}
-//void switchCallback(const geometry_msgs::Point &button_fa)
-//{
-//    fan_switch  = button_fa.x; //fan
-//    arm_switch = button_fa.y;    //arm
-//}
+}
+void potCallback(const geometry_msgs::Point &pot_pos)
+{
+    pot_x = pot_pos.x;
+    pot_y = pot_pos.y;
+}
+void switchCallback(const geometry_msgs::Point &button_fa)
+{
+    fan_switch  = button_fa.x; //fan
+    arm_switch = button_fa.y;    //arm
+}
+
 
 //////////////////////////////////////////////////////////////////////////////
+ros::NodeHandle nh;
+ros::Subscriber<geometry_msgs::Point> sub_robo_posi("/posi_dr",&robotCallback);
+ros::Subscriber<geometry_msgs::Point> sub_pot_posi("/posi_pot",&potCallback);
+ros::Subscriber<geometry_msgs::Point> sub_switch("/fan_switch",&switchCallback);
 
-float plot[]= {0.6,0.1,0.6,0.1};
-double x[20];
-double target_radA[20];
-double target_radB[20];
-
-//ros::NodeHandle nh;
-//ros::Subscriber<geometry_msgs::Point> sub_robo_posi("/robot",&robotCallback);
-//ros::Subscriber<geometry_msgs::Point> sub_pot_posi("/posi_pot",&potCallback);
-//ros::Subscriber<geometry_msgs::Point> sub_switch("/fan_switch",&switchCallback);
+geometry_msgs::Point relative_posi_pot;
+geometry_msgs::Point target_kakudo;
+ros::Publisher pub_pot("/relative_pot",&relative_posi_pot);
+ros::Publisher pub_theta("/target_theta",&target_kakudo);
 
 int main ()
 {
-    
-//    nh.getHardware()->setBaud(115200);
-//    nh.initNode();
-//    nh.subscribe(sub_robo_posi);
-//    nh.subscribe(sub_pot_posi);
-//    nh.subscribe(sub_switch);
-//    NVIC_SetPriority(TIMER3_IRQn, 5);
+
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    nh.subscribe(sub_robo_posi);
+    nh.subscribe(sub_pot_posi);
+    nh.subscribe(sub_switch);
+    nh.advertise(pub_pot);
+    nh.advertise(pub_theta);
+    NVIC_SetPriority(TIMER3_IRQn, 5);
 
     motorA.setEquation(0.082047,0.020033,-0.08206,0.020087);//原点、速度制御のC値D値
     motorB.setEquation(0.096001,0.067528,-0.08753,0.043631);//肘関節、速度制御のC値D値
     motorR.period_us(50);
     motorL.period_us(50);
 //////////////////////////////////////////////////////////////////////////////
-    int i = 0;
     int mode_1 = 1; //arm_switch = 1 用
-    int mode_0 = 1; //arm_switch = 0 用
-    double Deg,angle,Count,rad;
+    int mode_0 = 4; //arm_switch = 0 用
     double DegA,DegB;
     double angleA,angleB;
-    double CountA,CountB;
     double radA,radB;
     double start_theta_A, start_theta_B; //初期角度
-    double target_theta_A,target_theta_B;//目標角度(robot_xy, pot_xyから計算)
+    double target_theta_A=0,target_theta_B=0;//目標角度(robot_xy, pot_xyから計算)
+    double relative_pot_x,relative_pot_y;//アーム原点から見たポットの相対座標
     int servo_speed = 37;
     int servo_degree;
-    static bool IsFirst = true;
-    int M=0,N=0;
-    /*
-    for(int i = 0; i < 20; i++) {//軌道のプロット
-        x[i] = 0.5 + 0.01*i;
-        target_radA[i] = theta1(x[i],function(x[i]),20.0);
-        target_radB[i] = theta2(x[i],function(x[i]),target_radA[i]);
-        target_radA[i] = -target_radA[i];
-        target_radB[i] = target_radB[i] - 1;
-        printf("%f\t%f\t%f\r\n",x[i],target_radA[i],target_radB[i]);
-    }*/
+    target_kakudo.x=0;
+    target_kakudo.y=0;
 
     double target_x = 1.05; //ポット相対座標x
     double target_y = 1.05; //ポット相対座標y
@@ -162,36 +140,14 @@
     target_theta_A = theta1(target_x,target_y,target_theta_B); //目標角度計算(第一関節)
     servo_degree = 148 - (int)(0.7111*rad_deg_convert(target_theta_B)); //目標位置(サーボモータ)
     target_theta_B = deg_rad_convert(5.0); //第二関節決め打ち(実験用)
-    target_theta_A =deg_rad_convert(45.0); //第一関節決め打ち(実験用)
+    target_theta_A = deg_rad_convert(45.0); //第一関節決め打ち(実験用)
     //printf("%f\t%f\r\n",target_theta_A,target_theta_B);
-    start_theta_A = deg_rad_convert(95);
-    start_theta_B = deg_rad_convert(-165);
+    start_theta_A = deg_rad_convert(85); //初期角度
+    start_theta_B = deg_rad_convert(-140); //初期角度
     wait(3);
 
     while(1) {
-        /*
-        ecA.rewriteCount_1();
-        ecB.rewriteCount_2();
-        DegA = ecA.getDeg();
-        DegB = ecB.getDeg();
-        radA = ecA.getRad()/3 + deg_rad_convert(159.72);
-        if(radA >= 0){
-            radB = ecB.getRad()*36/60 - deg_rad_convert(102 + angleA/3 + angleA*16.63/90);
-        }else if(radA < 0){
-            radB = ecB.getRad()*36/60 - deg_rad_convert(102 + angleA/3 - angleA*16.63/90);
-        }
-        angleA = DegA/3 + 159.72;
-        if(angleA >= 0){
-            angleB = DegB*36/60  - angleA/3-102-angleA*16.63/90;
-        }else if(angleA < 0){
-            angleB = DegB*36/60  - angleA/3-102+angleA*16.63/90;
-        }
-
-        CountA = ecA.getCount();
-        CountB = ecB.getCount();
-        printf("%f\t%f\t%f\t%f\t%f\t%f\r\n",DegA,DegB,CountA,CountB,angleA,angleB);
-        */
-        //nh.spinOnce();
+        nh.spinOnce();
         wait_ms(1);
         ecA.rewriteCount_1();
         ecB.rewriteCount_2();
@@ -199,92 +155,77 @@
         DegB = ecB.getDeg();
         radA = ecA.getRad()/3 + deg_rad_convert(159.72);
         if(radA >= 0) {
-            radB = ecB.getRad()*36/60 - deg_rad_convert(102 + angleA/3 + angleA*16.63/90);
+            radB = ecB.getRad()*36/60 - deg_rad_convert(115 + angleA/3 + angleA*16.63/90);
         } else if(radA < 0) {
-            radB = ecB.getRad()*36/60 - deg_rad_convert(102 + angleA/3 - angleA*16.63/90);
+            radB = ecB.getRad()*36/60 - deg_rad_convert(115 + angleA/3 - angleA*16.63/90);
         }
         angleA = DegA/3 + 159.72;
         if(angleA >= 0) {
-            angleB = DegB*36/60 - 102 - angleA/3 - angleA*16.63/90;
+            angleB = DegB*36/60 - 115 - angleA/3 - angleA*16.63/90;
         } else if(angleA < 0) {
-            angleB = DegB*36/60 - 102 - angleA/3 + angleA*16.63/90;
+            angleB = DegB*36/60 - 115 - angleA/3 + angleA*16.63/90;
         }
         
+        ///////////////目標角度を常に計算する//////////////////
+        relative_pot_x = 6.0 - (robot_x + (0.185*cos(-robot_theta) + 0.29275*sin(-robot_theta)));   //potの相対座標x(目標座標)
+        relative_pot_y = 8.0 - (robot_y + (0.185*sin(-robot_theta) - 0.29275*cos(-robot_theta))); //potの相対座標y(目標座標)
+        target_theta_B = theta2(relative_pot_x,relative_pot_y); //目標角度計算(第二関節)
+        target_theta_A = theta1(relative_pot_x,relative_pot_y,target_theta_B); //目標角度計算(第一関節)
+        
+        relative_posi_pot.x = relative_pot_x;
+        relative_posi_pot.y = relative_pot_y;
+        if(target_theta_A){
+            
+        target_kakudo.x = double(target_theta_A);
+        target_kakudo.y = double(target_theta_B);
+        }
+        target_kakudo.z=0;
+        pub_pot.publish(&relative_posi_pot);
+        pub_theta.publish(&target_kakudo);
         
         if(arm_switch == 1) {
-            printf("%f\t%f\t%f\t%f\t%f\t%f\r\n",angleA,angleB,radA,radB,target_theta_A,target_theta_B);
+            //printf("%f\t%f\t%f\t%f\t%f\t%f\r\n",angleA,angleB,radA,radB,target_theta_A,target_theta_B);
             myled_2 = 1;
             mode_0 = 1;
             if(mode_1 == 1) {
                 motorA.AcOmega_1(target_theta_A,0.2);
-                if(fabs(target_theta_A - radA) < 0.01) { //収束判定
-                    mode_1++;
-                }
-                printf("1_1  ");
-            } else if(mode_1 == 2) {
                 motorB.AcOmega_2(target_theta_B,angleA,0.2);
-                if(fabs(target_theta_B - radB) < 0.01) { //収束判定
+                if(fabs(target_theta_A - radA) < 0.01 && fabs(target_theta_B - radB) < 0.01) { //収束判定
                     mode_1++;
                 }
-                printf("1_2  ");
-            } else if(mode_1 == 3) {
-                mode_1++;
-                printf("1_3  ");
-            } else if(mode_1 == 4) {
-                printf("1_4  ");
-                motorA.AcOmega_1(target_theta_A,0.05); //位置固定
-                motorB.AcOmega_2(target_theta_B,angleA,0.05); //位置固定
-
-                if(fabs(target_theta_A - radA) < 0.001) { //収束判定
-                    target_theta_B = target_theta_B + 0.01744;
-                    M++;
-                    if(M == 2) {
-                        target_theta_B = target_theta_B - 0.01744*2;
-                        M = 0;
-                    }
-                }
-                if(fabs(target_theta_B - radB) < 0.001) { //収束判定
-                    target_theta_B = target_theta_B + 0.01744;
-                    N++;
-                    if(N == 2) {
-                        target_theta_B = target_theta_B - 0.01744*2;
-                        N = 0;
-                    }
-                }
+                //printf("1_1  ");
+            }  else if(mode_1 == 2) {//最初にswitchをオンにした後のため、最大dutyを下げておく
+                //printf("1_2  ");
+                motorA.AcOmega_1(target_theta_A,0.1);//更新し続ける目標値に常に移動
+                motorB.AcOmega_2(target_theta_B,angleA,0.1); //更新し続ける目標値に常に移動
             }
         } else if(arm_switch == 0) {
-            printf("%f\t%f\t%f\t%f\t%f\t%f\r\n",angleA,angleB,radA,radB,start_theta_A,start_theta_B);
-            myled_2 = !myled_2;
+            //printf("%f\t%f\t%f\t%f\t%f\t%f\r\n",angleA,angleB,radA,radB,start_theta_A,start_theta_B);
+            myled_2 = 0;
             mode_1 = 1;
             if(mode_0 == 1) {
-                motorA.AcOmega_1(start_theta_A,0.2);
-                if(fabs(start_theta_A - radA) < 0.01) { //収束判定
+                motorA.AcOmega_1(start_theta_A,0.2);       //最初の位置に移動
+                motorB.AcOmega_2(start_theta_B,angleA,0.2);//最初の位置に移動
+                if(fabs(start_theta_B - radB) < 0.01 && fabs(start_theta_A - radA) < 0.01) { //収束判定
                     mode_0++;
                 }
-                printf("0_1  ");
-            } else if(mode_0 == 2) {
-                motorB.AcOmega_2(start_theta_B,angleA,0.2);
-                if(fabs(start_theta_B - radB) < 0.01) { //収束判定
-                    mode_0++;
-                }
-                printf("0_2  ");
-                
-                
-                
-            } else if(mode_0 == 3) {
-                mode_0++;
-                printf("0_3  ");
-            } else if(mode_0 == 4) {
-                printf("0_4  ");
-            }
+                //printf("0_1  ");
+            }  else if(mode_0 == 2) {
+                motorA.AcOmega_1(start_theta_A,0.01);
+                motorB.AcOmega_2(start_theta_B,angleA,0.04);
+                //printf("0_2  ");
+            } else {}
         }
         if(fan_switch == 0) {
             myled_1 = 0;
             motorR = 0;
         } else if (fan_switch == 1) {
             myled_1 = 1;
-            motorR = 0.1; //扇風機回す
-            printf("fan_on  ");
+            motorR = 0.5; //扇風機回す
+            //printf("fan_on  ");
+        } else if (fan_switch == 2){
+            myled_1 = !myled_1; //点滅
+            motorL = 0.5;
         }
     }
 }
\ No newline at end of file