File content as of revision 1:9d6e9312dab1:

#include "mbed.h"
#include "math.h"
#include <ros.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/String.h>
#include "EC.h" //Encoderライブラリをインクルード
#include "SpeedController.h"    //SpeedControlライブラリをインクルード

ros::NodeHandle nh;
geometry_msgs::Point check_com;

ros::Publisher pub_check("/check_2",&check_com);


int i = 0;

double mt_out1,mt_out2;

#define RESOLUTION 512  //分解能
#define BASIC_SPEED 40   //動確用
//#define TEST_DUTY 0.3   //動確用


Ec4multi EC_1(p13,p14,RESOLUTION);
Ec4multi EC_2(p15,p16,RESOLUTION);
//Ec4multi EC_1(p15,p16,RESOLUTION);
//Ec4multi EC_2(p17,p18,RESOLUTION);

SpeedControl motor_1(p21,p22,50,EC_1);
SpeedControl motor_2(p24,p23,50,EC_2);
//SpeedControl motor_1(p21,p22,50,EC_1);
//SpeedControl motor_2(p24,p23,50,EC_2);

Ticker motor_tick;  //角速度計算用ticker
//Serial pc(USBTX,USBRX); //このコメントを外したままROSとの通信をしようとすると、なぜか通信ができないので、コメントアウトしています。

Timer timer;


int j=0;
double rotate_1=40,rotate_2=40;
double gO_1;
double gO_2;
double MT01=0,MT02=0;
//double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
//int motor1_count[1000]= {},motor2_count[1000]= {};
double gb1, gb2;

void messageCallback(const geometry_msgs::Quaternion &mt2){
    mt_out1 = (double)mt2.x/(double)4;
    mt_out2 = (double)mt2.y/(double)4;
    gb1 = (double)mt2.z;
    gb2 = (double)mt2.w;
}

ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_mt2", &messageCallback);

void CalOmega()
{
    
    check_com.x = mt_out1;
    check_com.y = mt_out2;
    check_com.z = 12;
    
    pub_check.publish(&check_com);
    
    //printf("check\n\r");
    
    
    //motor1_count[j]=EC_1.getCount();
    //motor2_count[j]=EC_2.getCount();
    EC_1.calOmega();
    EC_2.calOmega();
    gO_1=EC_1.getOmega();
    gO_2=EC_2.getOmega();    
    //motor1_omega[j]=EC_1.getOmega();
    //motor2_omega[j]=EC_2.getOmega();
//    time_when[j]=EC_1.timer_.read();
    MT01=mt_out1;
    MT02=mt_out2;
    if(rotate_1==0) motor_1.stop();
    else motor_1.Sc(MT01,3,EC_1.getOmega());
    if(rotate_2==0) motor_2.stop();
    else motor_2.Sc(MT02,4,EC_2.getOmega());


    j++; 
}

int main()
{
    
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.advertise(pub_check);
    nh.subscribe(sub);
    
    motor_1.period_us(50);
    motor_2.period_us(50);
    //motor_1.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
    //motor_2.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
    motor_1.setEquation(0.0013,0.05,0.0013,0.04);    //求めたC,Dの値を設定
    motor_2.setEquation(0.0013,0.055,0.0013,0.047);    //求めたC,Dの値を設定
    motor_1.setPDparam(0,0.0);  //PIDの係数を設定
    motor_2.setPDparam(0,0.0);  //PIDの係数を設定
    //motor_1.setPDparam(0.1,0.0);  //PIDの係数を設定
    //motor_2.setPDparam(0.1,0.0);  //PIDの係数を設定
    motor_tick.attach(&CalOmega,0.01);

    
    while(1) {
        nh.spinOnce();
        wait(0.01);
    }
    printf("finish\n\r");
}














////CAN通信で受け取った(double)rotate‗1～rotate‗3に対し、実際に回転数制御を行う
//#include "mbed.h"
//#include "EC.h" //Encoderライブラリをインクルード
//#include "SpeedController.h"    //SpeedControlライブラリをインクルード
//#include "can.h"
//#define RESOLUTION 512  //分解能
//#define BASIC_SPEED 40   //動確用
////#define TEST_DUTY 0.3   //動確用
//
//
////Ec4multi EC_1(p15,p16,RESOLUTION);
////Ec4multi EC_2(p17,p18,RESOLUTION);
//Ec4multi EC_1(p15,p16,RESOLUTION);
//Ec4multi EC_2(p17,p18,RESOLUTION);
//
////SpeedControl motor_1(p21,p22,50,EC_1);
////SpeedControl motor_2(p23,p24,50,EC_2);
//SpeedControl motor_1(p22,p21,50,EC_1);
//SpeedControl motor_2(p23,p24,50,EC_2);
//
//Ticker motor_tick;  //角速度計算用ticker
//Serial pc(USBTX,USBRX);
//
//Timer timer;
//
//
//int j=0;
//double rotate_1=40,rotate_2=40;
////double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
////int motor1_count[1000]= {},motor2_count[1000]= {};
//
//void CalOmega()
//{
//    //motor1_count[j]=EC_1.getCount();
//    //motor2_count[j]=EC_2.getCount();
//    EC_1.calOmega();
//    EC_2.calOmega();
//    //motor1_omega[j]=EC_1.getOmega();
//    //motor2_omega[j]=EC_2.getOmega();
////    time_when[j]=EC_1.timer_.read();
//    if(rotate_1==0) motor_1.stop();
//    else motor_1.Sc(mt_out1);
//    if(rotate_2==0) motor_2.stop();
//    else motor_2.Sc(mt_out2);
//
//    j++;
//}
//int main()
//{
//    UserLoopSetting_can();
//    motor_1.period_us(50);
//    motor_2.period_us(50);
//    motor_1.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
//    motor_2.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
//    motor_1.setPDparam(0,0.0);  //PIDの係数を設定
//    motor_2.setPDparam(0,0.0);  //PIDの係数を設定
//    motor_tick.attach(&CalOmega,0.05);
//
//    int k = 0;
//    
//    while(1) {
//        // rotate_1 = mt_out1;
////        rotate_2 = mt_out2;
//    
//        if(k>100){
//            printf("\nloop\n\n\r");
//            k=0;
//        }
//        k++;
//    }
//    printf("finish\n\r");
//}