2月25日
Dependencies: mbed SpeedController2_25 ros_lib_kinetic
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 }
Generated on Tue Aug 2 2022 16:15:03 by
1.7.2