7th_DENSOU
9/19
Dependencies: mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm
Diff: main.cpp
- Revision:
- 3:ab266b418d90
- Parent:
- 2:aeae129daa37
- Child:
- 4:e20532e09d42
--- a/main.cpp Tue Jun 22 08:35:12 2021 +0000
+++ b/main.cpp Fri Sep 10 09:02:52 2021 +0000
@@ -2,42 +2,55 @@
#include "AMT21.h"
#include "CalPID.h"
#include "MotorController.h"
-#include <stdlib.h>
-#include <math.h>
+#include "KondoServo.h"
+//#include <ros.h>
+//#include <geometry_msgs/Point.h>
+
#define DELTA_T 0.01 //制御周期
#define DUTY_MAX 0.8 //duty比の最大値
#define OMEGA_MAX 15 //速度制御を利用した角度制御での角速度の最大値
-#define NUM_DATA 150 //記録データ数
-#define L_1 0.416
+#define L_1 0.891
#define L_2 0.820
+#define L_3 0.71362
#ifndef M_PI
#define M_PI 3.14159265359f
#endif
Timer timer_loop; //制御周期用
-
+DigitalOut myled_1(LED1);
+DigitalOut myled_2(LED2);
+DigitalOut myled_3(LED3);
+DigitalOut myled_4(LED4);
///////////////// 宣言部分 //////////////////////
CalPID speed_pid(0.2,0,0.00455,DELTA_T,DUTY_MAX); //速度制御のPD計算
-CalPID angle_duty_pid(0.5,0,0.00056,DELTA_T,DUTY_MAX); //角度制御（duty式）のPD計算
-CalPID angle_omega_pid_1(1.0,0,0,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算 原点
-CalPID angle_omega_pid_2(1.0,0,0,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算 肘関節
-Amt21 ecA(p27,p28,p8); //原点
-Amt21 ecB(p13,p14,p15);//肘関節
+CalPID angle_duty_pid_1(0.5,0,0.00056,DELTA_T,DUTY_MAX); //角度制御（duty式）のPD計算
+CalPID angle_duty_pid_2(0.5,0.01,0.00056,DELTA_T,DUTY_MAX); //角度制御（duty式）のPD計算
+
+CalPID angle_omega_pid_1(4.3,0,0.01,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算 原点
+CalPID angle_omega_pid_2(2.5,0,0.1,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算 肘関節
+
+KondoServo servo(p13,p14,1,115200);
+//Amt21 ecB(p28,p27,p8);//肘関節
+Amt21 ecA(p28,p27,p8); //原点
+Amt21 ecB(p9,p10,p15);//肘関節
// 上で宣言したインスタンスをMotorControllerに与える //
-MotorController motorA(p26,p25,DELTA_T,ecA,speed_pid,angle_duty_pid,angle_omega_pid_1);//原点
-MotorController motorB(p24,p23,DELTA_T,ecB,speed_pid,angle_duty_pid,angle_omega_pid_2);//肘関節
+MotorController motorA(p25,p26,DELTA_T,ecA,speed_pid,angle_duty_pid_1,angle_omega_pid_1);//原点
+MotorController motorB(p24,p23,DELTA_T,ecB,speed_pid,angle_duty_pid_2,angle_omega_pid_2);//肘関節
+PwmOut motorR(p22); //fan
+PwmOut motorL(p21);
//////////////////////////////////////////////////////////////////////////
-float target_rad=0; //目標角速度（速度制御）
-float target_speed=0;//目標角度（角度制御）
+double deg_rad_convert(double deg)
+{
+ return deg*M_PI/180;
+}
-void speedControll()
+double rad_deg_convert(double rad)
{
- motorA.Sc(target_speed); //速度制御関数]
- motorB.Sc(target_speed);
+ return rad*180/M_PI;
}
void angleControll()
@@ -46,88 +59,232 @@
//motorB.AcOmega_2(target_rad); //速度式角度制御
}
+
double function(double x)
{
double y;
- y = 0.5;
+ y = 0.6 + sqrt(0.01 - (x-0.6)*(x-0.6));//円の軌道
return y;
}
-double theta1(double x, double y, double angle_1)//原点角度計算
-{ //(x:目標座標 y:目標座標 angle_1:現在角度)
- double theta_1_a, theta_1_b;
- double angle_1_PI = angle_1 * M_PI / 180;
- theta_1_a = -acos((pow(x, 2) + pow(y, 2) + pow(L_1, 2) - pow(L_2, 2)) / (2 * L_1 * hypot(x, y))) + atan(y / x);
- theta_1_b = acos((pow(x, 2) + pow(y, 2) + pow(L_1, 2) - pow(L_2, 2)) / (2 * L_1 * hypot(x, y))) + atan(y / x);
- if (fabs(angle_1_PI - theta_1_a) > fabs(angle_1_PI - theta_1_b))
- return theta_1_b;
- else
- {
- return theta_1_a;
- }
+double theta1(double x, double y, double theta_2)//原点角度計算
+{
+ //(x:目標座標 y:目標座標)
+ double theta_1;
+ theta_1 = atan(y/x)-atan(L_2*sin(theta_1)/(L_1+L_2*cos(theta_2)));
+ //printf("%f\r\n",theta_1);
+ return theta_1;
+}
+
+double theta2(double x, double y)//肘関節角度計算
+{
+ double theta_2;
+ theta_2 = acos((x*x+y*y-L_1*L_1-L_2*L_2)/(2*L_1*L_2));
+ //printf("%f ",theta_2);
+ return theta_2;
}
-double theta2(double x, double y, double theta_1)//肘関節角度計算
-{
- double theta_2;
- theta_2 = atan((y - L_1 * sin(theta_1)) / (x - L_1 * cos(theta_1))) - theta_1;
- return theta_2;
-}
+double robot_x, robot_y; //ロボット座標
+double pot_x, pot_y; //ポット座標
+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;
+//
+//}
+//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
+//}
//////////////////////////////////////////////////////////////////////////////
-int main () //調整時を想定しているのでデータを取っている。
+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);
+
+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);
+
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 state=0;
- int x;
- double DegA;
- double DegB;
- double angleA;
- double angleB;
+ int i = 0;
+ int mode_1 = 1; //arm_switch = 1 用
+ int mode_0 = 1; //arm_switch = 0 用
+ double Deg,angle,Count,rad;
+ 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から計算)
+ 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]);
+ }*/
+
+ double target_x = 1.05; //ポット相対座標x
+ double target_y = 1.05; //ポット相対座標y
+ target_theta_B = theta2(target_x,target_y); //目標角度計算(第二関節)
+ 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); //第一関節決め打ち(実験用)
+ //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);
+ wait(3);
+
while(1) {
- x = 0.5;
- ecA.rewriteCount();
- ecB.rewriteCount();
+ /*
+ ecA.rewriteCount_1();
+ ecB.rewriteCount_2();
DegA = ecA.getDeg();
DegB = ecB.getDeg();
- angleA = DegA/3;
- angleB = DegB*36/60;
- //double target_thetaA = theta1(x,function(x),angleA);
- //double target_thetaB = theta2(x,function(x),target_thetaA);
- //printf("%f\t%f\t%f\t%f\t%f\r\n",ecA.read(),ecA.getCount(),ecA.getDeg(),ecA.getOmega(),angleA,target_thetaA,target_thetaB);
-
-
-
+ 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();
+ wait_ms(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 - 102 - angleA/3 - angleA*16.63/90;
+ } else if(angleA < 0) {
+ angleB = DegB*36/60 - 102 - angleA/3 + angleA*16.63/90;
+ }
- /*
- if(state==0) {
- if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に
- state++;
- target_speed=5; //目標角速度を5rad/sに
- target_rad=(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）
+ 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);
+ 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) { //収束判定
+ 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); //位置固定
- timer_loop.reset();
- timer_loop.start();
+ 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;
+ }
+ }
}
- } else if(state==1) {
- if(timer_loop.read()>DELTA_T) {//Tickerだと通信と割り込みが干渉してしまうのでタイマーで
-// speedControll();//速度制御
- angleControll();//角度制御
-
- timer_loop.reset();
- }
- if(omega_count==NUM_DATA) {//データが集まったら表示
- motor.stop();
- displayData();
- timer_loop.stop();
- state++;
+ } 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;
+ mode_1 = 1;
+ if(mode_0 == 1) {
+ motorA.AcOmega_1(start_theta_A,0.2);
+ if(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 ");
}
}
- wait_us(10);*/
+ if(fan_switch == 0) {
+ myled_1 = 0;
+ motorR = 0;
+ } else if (fan_switch == 1) {
+ myled_1 = 1;
+ motorR = 0.1; //扇風機回す
+ printf("fan_on ");
+ }
}
-}
-
+}
\ No newline at end of file