7th_DENSOU
9/19
Dependencies: mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm
Diff: main.cpp
- Revision:
- 4:e20532e09d42
- Parent:
- 3:ab266b418d90
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