File content as of revision 1:163629043391:

#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 motorA(p7,p6,NC,360,0.05,p21,p22);
SpeedControl motorB(p10,p11,NC,360,0.05,p23,p24);


DigitalOut led(LED1);

Ticker ticker;
Ticker ticker2;
Timer timer;

double vA=0,vB=0;
bool waiting=0;
double time1;

int k=0;
int check_k=0;

void calOmega(){
    motorA.CalOmega();
    motorB.CalOmega();
}

ros::NodeHandle nh;

//bool tickerstop=1;

//void messageCallback(const geometry_msgs::Inertia& msg){
void messageCallback(const geometry_msgs::Point& msg){
    
    vA=msg.x;
    vB=msg.y;
    bool reset=msg.z;
   
        if(reset){
            vA=0;
            vB=0;
            motorA.reset();
            motorB.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_motorAB",&messageCallback);

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

std_msgs::Int8 msg_main;
ros::Publisher pub("main_checkAB",&msg_main);


void debug(){
    data.x=motorA.getOmega();
    data.y=motorB.getOmega();
    data.z=motorA.duty;
    data.w=motorB.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_main.data=check_k;
    pub.publish(&msg_main);
    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();
    
    motorA.pwm_F_.period_us(100);
    motorA.pwm_B_.period_us(100);
    motorB.pwm_F_.period_us(100);
    motorB.pwm_B_.period_us(100);


    
    
       
    //速度に対するpid係数
    /*motorA.setPDparam(0.1,0.001);
    motorB.setPDparam(0.1,0.001);*/
    motorA.setPDparam(0.09,0);
    motorB.setPDparam(0.05,0);

    
    //Cは傾き，Dは切片(縦デューティー，横入力値 のグラフにおいて)  C=1/c, D=d 
    motorA.setDOconstant(295,0,295,0);
    motorB.setDOconstant(305,0,305,0);

    
    //合わせたver
    /*motorA.setDOconstant(250,0,256,0);
    motorB.setDOconstant(252,0,267,0);*/
    
    
    
     
    
    ticker.attach(&calOmega,0.05);
    wait_ms(5);
    ticker2.attach(&spin,0.01);
    
    while(1) {
        if(vA)motorA.Sc(vA);
        else motorA.stop();
        if(vB)motorB.Sc(vB);
        else motorB.stop();
        k++;        
        if((k>5000)&&(waiting==0)){
            double time_a=timer.read()-time1;
            if(time_a>0.5){
                led=0;
                vA=0;
                vB=0;
                motorA.reset();
                motorB.reset();
             
                waiting=1;
                timer.stop();
                timer.reset();
                time1=0;
            }else{
                led=1;
            }
        }
    }
}