2月25日

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