File content as of revision 3:ab266b418d90:

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

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:目標座標)
    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; //ロボット座標
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
//}

//////////////////////////////////////////////////////////////////////////////

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 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) {
        /*
        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();
        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(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); //位置固定

                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(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  ");
            }
        }
        if(fan_switch == 0) {
            myled_1 = 0;
            motorR = 0;
        } else if (fan_switch == 1) {
            myled_1 = 1;
            motorR = 0.1; //扇風機回す
            printf("fan_on  ");
        }
    }
}