File content as of revision 2:aeae129daa37:

#include "mbed.h"
#include "AMT21.h"
#include "CalPID.h"
#include "MotorController.h"
#include <stdlib.h>
#include <math.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_2 0.820

#ifndef M_PI
#define M_PI 3.14159265359f
#endif

Timer timer_loop;  //制御周期用


/////////////////               宣言部分                  //////////////////////
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);//肘関節
//　上で宣言したインスタンスを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);//肘関節
//////////////////////////////////////////////////////////////////////////

float target_rad=0;  //目標角速度（速度制御）
float target_speed=0;//目標角度（角度制御）

void speedControll()
{
    motorA.Sc(target_speed);       //速度制御関数]
    motorB.Sc(target_speed);
}

void angleControll()
{
    //motorA.AcOmega_1(target_rad);    //duty式角度制御
    //motorB.AcOmega_2(target_rad);   //速度式角度制御
}

double function(double x)
{
    double y;
    y = 0.5;
    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 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;
}

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

int main ()     //調整時を想定しているのでデータを取っている。
{
    motorA.setEquation(0.082047,0.020033,-0.08206,0.020087);//原点、速度制御のC値D値
    motorB.setEquation(0.096001,0.067528,-0.08753,0.043631);//肘関節、速度制御のC値D値
//////////////////////////////////////////////////////////////////////////////
    //int state=0;
    int x;
    double DegA;
    double DegB;
    double angleA;
    double angleB;
    while(1) {
        x = 0.5;
        ecA.rewriteCount();
        ecB.rewriteCount();
        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);
        
        
        
        
        
        /*
        if(state==0) {
            if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に
                state++;
                target_speed=5; //目標角速度を5rad/sに
                target_rad=(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）

                timer_loop.reset();
                timer_loop.start();
            }
        } 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++;
            }
        }
        wait_us(10);*/
    }
}