2月25日
Dependencies: mbed SpeedController___2_25 ros_lib_kinetic
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 //}
Generated on Sun Jul 17 2022 14:41:21 by
1.7.2