File content as of revision 0:83b2c6a67cce:

////c = center arm
#include "mbed.h"
#include "AMT21.h"
#include "CalPID.h"
#include "KondoServo.h"
#include "MotorController.h"
#include <stdlib.h>

/////////////////               ROS               /////////////////
#include <ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Point.h>
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);
float arm_num=0,arm_deg=0;
void cArmCallback(const geometry_msgs::Point &arm_state)
{
    arm_num = arm_state.x;
    arm_deg = arm_state.y;
}
ros::NodeHandle  nh;
ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);

/////////////////               宣言部分                  //////////////////////
#ifndef M_PI
#define M_PI 3.14159265359f
#endif
#define DELTA_T 0.01    //制御周期
#define DUTY_MAX 0.07     //duty比の最大値
#define OMEGA_MAX 0.6    //速度制御を利用した角度制御での角速度の最大値
#define NUM_DATA 270

Serial pc(USBTX, USBRX);
////角度制御(アブソリュートエンコーダ)/////
DigitalOut myled1(LED1);
Timer timer;
CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX);   //速度制御のPD計算
CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);   //角度制御（duty式）のPD計算     不要？
CalPID angle_omega_pid(5.5,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
Amt21 amt(p9,p10,p8);    //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode)
MotorController motor(p26,p25,DELTA_T,amt,speed_pid,angle_duty_pid,angle_omega_pid);    //MotorController(PinName motor_p_, PinName motor_n_,double dt, Amt21 &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega)
double target_speed=0;
double target_rad=0;
double target_deg = 0;
double radToDeg(double x);

////先端アーム(サーボ)/////
DigitalIn ts_c1(p6, PullUp);
DigitalIn ts_c2(p7, PullUp);
KondoServo servo_c(p13, p14);
double servoDeg(double servo_value);
double calServoDeg(double servo_value);     //360°の度数法にする
int id_c1 = 0;
float deg_c_grab = 9000;
float deg_c_open = 3500;


////データ記録まわり/////
int angle_count=0;
int omega_count=0;
double now_deg=0;    //度数法
double now_count=0;

int main()
{
    myled1 = 0;     //アブソリュートエンコーダ用if文中で点灯
    /////////       ROS       /////////
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.subscribe(sub);

    timer.reset();
    timer.start();

    motor.setEquation(0.0523,0.0148,0.0459,-0.0341);    //Excel"omega_controll"の図より

//    target_rad = -(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）           //////////////////////////
//    target_deg = radToDeg(target_rad);

    while (1) {
        ////    ROS     ////
        nh.spinOnce();
        if(arm_num==1&&arm_deg==60) {       //LEDは動作確認用
            myled2 = !myled2;
        }
        if(arm_num == 1) {
            myled3 = 1;
        }
        if(arm_deg == 60) {
            myled4 = 1;
        }
        
        ////タッチセンサ////
        if(ts_c1 == 0) {      //タッチセンサ反応したらハンドルを掴む
            wait_us(50);
            if(ts_c1 == 0) {
                servo_c.set_degree(0,servoDeg(4500));   //servo_c掴む
            }
        }

        ////アブソリュートエンコーダ////
        if(timer.read()>DELTA_T) {
            if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
                myled1=1;   //LED2 点灯
                motor.AcOmega(target_rad);   //速度式角度制御
            } else { //目標角度に達したら停止
                myled1=0;   //LED2 消灯
                motor.stop();
                servo_c.set_degree(0,servoDeg(1000));   //servo_c離す
            }
            amt.rewriteCount();
            amt.calOmega();
            amt.getOmega();
            now_deg = amt.getDeg();    //度数法
            now_count=amt.getCount();
            motor.AcOmega(target_rad);

//            pc.printf("now_deg = %f,%f\r\n", now_deg,now_count);       //現在角度取得 (確認以外ではコメントアウトしないとモーター回らない)////////////////////
            timer.reset();
        }
        wait_us(10);
    }
}

/////////////////               関数宣言部分                  //////////////////////
double servoDeg(double servo_value)
{
    return (servo_value - 3500)*270/(11500 - 3500);
}
double radToDeg(double x)
{
    return x*180/M_PI;
}