2月25日

Dependencies:   mbed SpeedController2_25 ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 //#include "mbed.h"
00002 //#include "math.h"
00003 //#include "EC.h" //Encoderライブラリをインクルード
00004 //#include "SpeedController.h"    //SpeedControlライブラリをインクルード
00005 //
00006 //#include <ros.h>
00007 //#include <geometry_msgs/Point.h>
00008 //#include <geometry_msgs/Pose.h>
00009 //
00010 //ros::NodeHandle nh;
00011 //geometry_msgs::Point posi_xyr;
00012 ////geometry_msgs::Quaternion usw_4info;
00013 //
00014 //ros::Publisher pub_xyr("/check_1",&posi_xyr);
00015 ////ros::Publisher pub_usw("/mbed_main2",&usw_4info);
00016 //
00017 //Ticker cm_pc;
00018 //#define PI 3.141592
00019 //#define RESOLUTION 512  //分解能
00020 //#define BASIC_SPEED 40   //動確
00021 //
00022 //Ec4multi EC_1(p16,p15,RESOLUTION);
00023 //Ec4multi EC_2(p18,p17,RESOLUTION);
00024 //SpeedControl motor_1(p21,p22,50,EC_1);
00025 //SpeedControl motor_2(p24,p23,50,EC_2);
00026 //
00027 //Ticker motor_tick;  //角速度計算用ticker
00028 //Timer timer;
00029 //
00030 //int i = 0;
00031 //int j=0;
00032 ////Serial pc(USBTX,USBRX);
00033 //double rotate_1=40,rotate_2=40;
00034 //
00035 //double gb1, gb2;
00036 //double mt_out1,mt_out2;
00037 //
00038 ////void messageCallback(const geometry_msgs::Quaternion &mt1){
00039 ////    mt_out1 = mt1.x;
00040 ////    mt_out2 = mt1.y;
00041 ////    gb1 = mt1.z;
00042 ////    gb2 = mt1.w;
00043 ////}
00044 //
00045 ////ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_mt1", &messageCallback);
00046 //
00047 //void cm_to_pc(){
00048 //    
00049 //    
00050 //    posi_xyr.x = 1;
00051 //    posi_xyr.y = 2;
00052 //    posi_xyr.z = 3;
00053 //    
00054 //    
00055 //    pub_xyr.publish(&posi_xyr);
00056 //    
00057 //}
00058 //
00059 //
00060 ////-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
00061 ////DigitalOut cansend_led(LED1);  //cansend -> on                //can.cpp
00062 ////DigitalOut canread_led(LED2);  //canread -> on                //can.cpp
00063 ////DigitalOut debug_led(LED3);    //maxon debug programme -> on  //maxonsetting.cpp
00064 //
00065 ////////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
00066 //
00067 //int main()
00068 //{
00069 //    nh.getHardware()->setBaud(115200);
00070 //    nh.initNode();
00071 //    nh.advertise(pub_xyr);
00072 ////    nh.advertise(pub_usw);
00073 //    
00074 //    cm_pc.attach(&cm_to_pc,0.01);
00075 //
00076 ////    UserLoopSetting_sensor();
00077 //    
00078 //    while(1) {
00079 //
00080 //        nh.spinOnce();
00081 //        wait(0.01);
00082 //
00083 //    }
00084 //
00085 //}
00086 //
00087 //
00088 
00089 
00090 
00091 
00092 
00093 
00094 ////
00095 
00096 //CAN通信で受け取った(double)rotate‗1~rotate‗3に対し、実際に回転数制御を行う
00097 
00098 #include "mbed.h"
00099 #include "math.h"
00100 #include <ros.h>
00101 #include <geometry_msgs/Point.h>
00102 #include <geometry_msgs/Pose.h>
00103 #include <std_msgs/String.h>
00104 #include "EC.h" //Encoderライブラリをインクルード
00105 #include "SpeedController.h"    //SpeedControlライブラリをインクルード
00106 
00107 ros::NodeHandle nh;
00108 geometry_msgs::Point check_com;
00109 
00110 ros::Publisher pub_check("/check_1",&check_com);
00111 
00112 
00113 int i = 0;
00114 
00115 double mt_out1,mt_out2;
00116 
00117 #define RESOLUTION 512  //分解能
00118 #define BASIC_SPEED 40   //動確用
00119 //#define TEST_DUTY 0.3   //動確用
00120 
00121 
00122 Ec4multi EC_1(p13,p14,RESOLUTION);
00123 Ec4multi EC_2(p15,p16,RESOLUTION);
00124 //Ec4multi EC_1(p15,p16,RESOLUTION);
00125 //Ec4multi EC_2(p17,p18,RESOLUTION);
00126 
00127 SpeedControl motor_1(p21,p22,50,EC_1);
00128 SpeedControl motor_2(p24,p23,50,EC_2);
00129 //SpeedControl motor_1(p21,p22,50,EC_1);
00130 //SpeedControl motor_2(p24,p23,50,EC_2);
00131 
00132 Ticker motor_tick;  //角速度計算用ticker
00133 //Serial pc(USBTX,USBRX); //このコメントを外したままROSとの通信をしようとすると、なぜか通信ができないので、コメントアウトしています。
00134 
00135 Timer timer;
00136 
00137 
00138 int j=0;
00139 double rotate_1=40,rotate_2=40;
00140 double gO_1;
00141 double gO_2;
00142 double MT01=0,MT02=0;
00143 //double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
00144 //int motor1_count[1000]= {},motor2_count[1000]= {};
00145 double gb1, gb2;
00146 
00147 void messageCallback(const geometry_msgs::Quaternion &mt1){
00148     mt_out1 = (double)mt1.x/(double)4;
00149     mt_out2 = (double)mt1.y/(double)4;
00150     gb1 = (double)mt1.z;
00151     gb2 = (double)mt1.w;
00152 }
00153 
00154 ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_mt1", &messageCallback);
00155 
00156 void CalOmega()
00157 {
00158     
00159     check_com.x = mt_out1;
00160     check_com.y = mt_out2;
00161     check_com.z = 12;
00162     
00163     pub_check.publish(&check_com);
00164     
00165     //printf("check\n\r");
00166     
00167     
00168     //motor1_count[j]=EC_1.getCount();
00169     //motor2_count[j]=EC_2.getCount();
00170     EC_1.calOmega();
00171     EC_2.calOmega();
00172     gO_1=EC_1.getOmega();
00173     gO_2=EC_2.getOmega();    
00174     //motor1_omega[j]=EC_1.getOmega();
00175     //motor2_omega[j]=EC_2.getOmega();
00176 //    time_when[j]=EC_1.timer_.read();
00177     MT01=mt_out1;
00178     MT02=mt_out2;
00179     if(rotate_1==0) motor_1.stop();
00180     else motor_1.Sc(MT01,1,EC_1.getOmega());
00181     if(rotate_2==0) motor_2.stop();
00182     else motor_2.Sc(MT02,2,EC_2.getOmega());
00183 
00184     j++; 
00185 }
00186 
00187 int main()
00188 {
00189     
00190     nh.getHardware()->setBaud(115200);
00191     nh.initNode();
00192     nh.advertise(pub_check);
00193     nh.subscribe(sub);
00194     
00195     motor_1.period_us(50);
00196     motor_2.period_us(50);
00197     //motor_1.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
00198     //motor_2.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
00199     motor_1.setEquation(0.0013,0.0525,0.0013,0.045);    //求めたC,Dの値を設定
00200     //motor_2.setEquation(0.0013,0.03,0.0013,0.035);    //求めたC,Dの値を設定
00201     motor_2.setEquation(0.0013,0.05,0.0013,0.055);    //求めたC,Dの値を設定//no arm ver
00202     motor_1.setPDparam(0,0.0);  //PIDの係数を設定
00203     motor_2.setPDparam(0,0.0);  //PIDの係数を設定
00204     //motor_1.setPDparam(0.1,0.0);  //PIDの係数を設定
00205     //motor_2.setPDparam(0.1,0.0);  //PIDの係数を設定
00206     motor_tick.attach(&CalOmega,0.01);
00207 
00208     
00209     while(1) {
00210         nh.spinOnce();
00211         wait(0.01);
00212     }
00213     printf("finish\n\r");
00214 }