File content as of revision 4:e20532e09d42:

#include "mbed.h"
#include "AMT21.h"
#include "CalPID.h"
#include "MotorController.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 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_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(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);
//////////////////////////////////////////////////////////////////////////

double deg_rad_convert(double deg)
{
    return deg*M_PI/180;
}

double rad_deg_convert(double rad)
{
    return rad*180/M_PI;
}

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 robot_x, robot_y,robot_theta; //ロボット座標
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;
    robot_theta = robot_pos.z;

}
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);

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);
    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 mode_1 = 1; //arm_switch = 1 用
    int mode_0 = 4; //arm_switch = 0 用
    double DegA,DegB;
    double angleA,angleB;
    double radA,radB;
    double start_theta_A, start_theta_B; //初期角度
    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;
    target_kakudo.x=0;
    target_kakudo.y=0;

    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(85); //初期角度
    start_theta_B = deg_rad_convert(-140); //初期角度
    wait(3);

    while(1) {
        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(115 + angleA/3 + angleA*16.63/90);
        } else if(radA < 0) {
            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 - 115 - angleA/3 - angleA*16.63/90;
        } else if(angleA < 0) {
            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);
            myled_2 = 1;
            mode_0 = 1;
            if(mode_1 == 1) {
                motorA.AcOmega_1(target_theta_A,0.2);
                motorB.AcOmega_2(target_theta_B,angleA,0.2);
                if(fabs(target_theta_A - radA) < 0.01 && fabs(target_theta_B - radB) < 0.01) { //収束判定
                    mode_1++;
                }
                //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 = 0;
            mode_1 = 1;
            if(mode_0 == 1) {
                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) {
                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.5; //扇風機回す
            //printf("fan_on  ");
        } else if (fan_switch == 2){
            myled_1 = !myled_1; //点滅
            motorL = 0.5;
        }
    }
}