File content as of revision 1:c79ecadc3b18:

#include "mbed.h"
#include "EC.h"
#include <ros.h>
//#include <geometry_msgs/Inertia.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <std_msgs/Int8.h>

SpeedControl motorC(p16,p15,NC,360,0.05,p25,p26);
SpeedControl motorD(p11,p12,NC,360,0.05,p21,p23);


DigitalOut led(LED1);

Ticker ticker;
Ticker ticker2;
Timer timer;

double vC=0,vD=0;
bool waiting=0;
double time1;

int k=0;
int check_k=0;

void calOmega(){
    motorC.CalOmega();
    motorD.CalOmega();
}

ros::NodeHandle nh;

//bool ticker_stop=1;

//void messageCallback(const geometry_msgs::Inertia& msg){
void messageCallback(const geometry_msgs::Point& msg){

    vC=msg.x;
    vD=msg.y;
    bool reset=msg.z;
    
        if(reset){
            vC=0;
            vD=0;
            motorC.reset();
            motorD.reset();
        }
    if(waiting==1){
        timer.start();
        waiting=0;
    }else{
        time1=timer.read();
    }
}

//ros::Subscriber<geometry_msgs::Inertia> motion_sub("motion",&messageCallback);
ros::Subscriber<geometry_msgs::Point> motion_sub("motion_motorCD",&messageCallback);

geometry_msgs::Quaternion data;
ros::Publisher debug_pub("debug_duty_cd", &data);

std_msgs::Int8 msg_motor;
ros::Publisher pub("motor_checkCD",&msg_motor);


void debug(){
    data.x=motorC.getOmega();
    data.y=motorD.getOmega();
    data.z=motorC.duty;
    data.w=motorD.duty;
    debug_pub.publish( &data );
}

int k_1=0;
void spin(){
    /*k_1++;
    if(k_1>50){
        k_1=0;
        debug();
    }*/
    debug();
    check_k++;
    if(check_k>100){
        check_k=0;    
    }
    msg_motor.data=check_k;
    pub.publish(&msg_motor);
    nh.spinOnce();
}

int main() {
    //nh.getHardware()->setBaud(921600);
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.subscribe(motion_sub);
    nh.advertise(pub);
    nh.advertise(debug_pub);
    
    timer.start();
    
    motorC.pwm_F_.period_us(100);
    motorC.pwm_B_.period_us(100);
    motorD.pwm_F_.period_us(100);
    motorD.pwm_B_.period_us(100);
    
    
    
    
    
    //速度に対するpid係数
    //motorC.setPDparam(0.1,0.001);
    //motorD.setPDparam(0.1,0.001);  //MR1の値
    motorC.setPDparam(0.1,0);
    motorD.setPDparam(0.02,0);
    
    
    //Cは傾き，Dは切片(縦デューティー，横入力値 のグラフにおいて)  C=1/c, D=d 
    motorC.setDOconstant(277,0,262,0);
    motorD.setDOconstant(270,0,270,0);
    
    
    //合わせたver
    //motorD.setDOconstant(270,0,255,0);
    //motorC.setDOconstant(267,0,250,0);
   

   
   
   
    ticker.attach(&calOmega,0.05);
    wait_ms(5);
    ticker2.attach(&spin,0.01);
    
    while(1) {
        if(vC)motorC.Sc(vC);
        else motorC.stop();
        if(vD)motorD.Sc(vD);
        else motorD.stop();
        k++;
        if((k>5000)&&(waiting==0)){
            double time_a=timer.read()-time1;
            if(time_a>0.5){
                led=0;
                vC=0;
                vD=0;
                motorC.reset();
                motorD.reset();

                waiting=1;
                timer.stop();
                timer.reset();
                time1=0;
            }else{
                led=1;
            }
        }
    }
}