File content as of revision 11:df2acf22123e:

////c = center arm
#include "mbed.h"
#include "AMT21.h"
#include "CalPID.h"
#include "KondoServo.h"
#include "MotorController.h"
#include <stdlib.h>
#include <ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Point.h>

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

int state = 1;

////角度制御(アブソリュートエンコーダ)/////
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.0,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 degToRad(double x);
double target_rad=0;
double target_deg = 0;
double turn_deg = 0;
double turn_rad = 0;
DigitalIn toggle1(p16, PullUp);     //トグルright(マイコン側)
DigitalIn toggle2(p20, PullUp);     //トグルleft(外側)

////先端アーム(サーボ)/////
DigitalIn ts_c1(p6, PullUp);
DigitalIn ts_c2(p7, PullUp);
KondoServo servo_c(p13, p14);
double servoDeg(double servo_value);
void touchGrab();   //タッチセンサ反応したらサーボ掴む
float deg_c_grab = 11000;
float deg_c_open = 8300;

////データ記録関連/////
double now_deg=0;
double now_count=0;

/////////////////               ROS               /////////////////
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);
float arm_turn=0,arm_deg=0,arm_rad=0;
void cArmCallback(const geometry_msgs::Point &arm_state)
{
    arm_turn = arm_state.x;     //左回転＝1、右回転＝2
    arm_deg = arm_state.y;      //目標角度
}
ros::NodeHandle  nh;
ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);

int main()
{
    int mode = 0;
    myled1 = 0;     //目標角度以上離れている:点灯
    myled2 = 0;     //通信で欲しい値が送られているとき:点灯
    myled3 = 0;     //state2に入ったとき←基本不要
    myled4 = 0;     //サーボを掴む→点灯．離す→消灯

    /////////       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"の図より
    ////////////////////////////////////////
    /*arm_turn = 2;     //左回転＝1、右回転＝2
    arm_deg = -90;      //目標角度*/
///////////////////////////////////////

    while (1) {
        nh.spinOnce();      //ROS

        if(timer.read()>DELTA_T) {
            if(arm_turn==2&&arm_deg==-10) {       //動作確認(通信)用
                myled2 = 1;
            }
            arm_rad = degToRad(arm_deg);
            target_deg = arm_deg;
            target_rad = arm_rad;

            if(state == 0) {        //トグルスイッチONでstate=1へ．
                ///トグルスイッチ1////マイコン方向
                if(toggle1 == 0) {  //トグルスイッチ1(mbed側)ONで初start
                    wait_us(50);
                    if(toggle1 == 0) {
                        state++;
                    }
                }
            }
            if(state == 1) {        //回転方向の変数"arm_turn"に1か2が代入されたらstate=2へ．
                if(arm_turn == 1) {
                    turn_deg = 90;
                    state++;
                } else if(arm_turn == 2) {
                    turn_deg = -90;
                    state++;
                }
                turn_rad = degToRad(turn_deg);
            } else if(state == 2) { //サーボを離してそのまますぐstate=3へ．
                servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す
                state++;
            } else if (state == 3) {//タッチセンサがハンドルに反応したらサーボで掴んでstate=4へ．
                myled3 = 1;
                if(fabs(now_deg-turn_deg) > 0.5) {          //現在角度と目標角度が0.5°以上離れている間回す
                    myled1 = 1; //LED1 点灯
                    motor.AcOmega(turn_rad);                //速度式角度制御
                } else { //目標角度に達したら停止
                    myled1=0;   //LED1 消灯
                    motor.stop();
                }
                touchGrab();
            } else if (state == 4) {//目標角度を設定してそのまますぐstate=5へ．
                myled3 = 0;
                if(mode == 0){
                    target_rad = arm_rad;
                    target_deg = arm_deg;
                }
                else if(mode == 1){
                    target_deg = 45;
                    target_rad = degToRad(target_deg);   
                }
                else if(mode == 2){
                    target_deg = -45;
                    target_rad = degToRad(target_deg);   
                }
                state++;
            } else if (state == 5) {//目標角度に近づいたら停止してサーボを離す．
                if(fabs(now_deg-target_deg) > 0.5) {        //現在角度と目標角度が0.5°以上離れている間回す
                    myled1 = 1; //LED1 点灯
                    motor.AcOmega(target_rad);              //速度式角度制御
                } else { //目標角度に達したら停止
                    myled1 = 0; //LED1 消灯
                    motor.stop();
                    //servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
                    myled4 = 0;
                    if(mode == 0){
                        mode = 1;
                    }
                    else{
                        mode = 3-mode;
                    }
                    state = 4;    /////////////////////連続して動かす時はここのコメントアウト外す．
                }
            }

            amt.rewriteCount();
            amt.calOmega();
            now_deg = amt.getDeg();
            now_count=amt.getCount();

            timer.reset();
        }
        wait_us(10);
    }
}

/////////////////               関数宣言部分                  //////////////////////
double servoDeg(double servo_value)
{
    return (servo_value - 3500)*270/(11500 - 3500);
}
double degToRad(double x)
{
    return x*M_PI/180;
}
void touchGrab()
{
    if(ts_c1 == 0) {      //タッチセンサ反応したらハンドルを掴む
        wait_us(50);
        if(ts_c1 == 0) {
            servo_c.set_degree(0,servoDeg(deg_c_grab));   //servo_c掴む
            state++;
            myled4=1;   //LED4 点灯
        }
    }
}