ロボステ6期 / Mbed 2 deprecated NHK2020-m2-1-ros_2_25

2月25日

Dependencies:   mbed SpeedController2_25 ros_lib_kinetic

main.cpp@5:fe91d31db27e, 2020-02-25 (annotated)

Committer:
yuki0701
Date:
Tue Feb 25 01:23:14 2020 +0000
Revision:
5:fe91d31db27e
Parent:
4:46c10d34af27 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0701 5:fe91d31db27e 1 //#include "mbed.h"
yuki0701 5:fe91d31db27e 2 //#include "math.h"
yuki0701 5:fe91d31db27e 3 //#include "EC.h" //Encoderライブラリをインクルード
yuki0701 5:fe91d31db27e 4 //#include "SpeedController.h" //SpeedControlライブラリをインクルード
yuki0701 5:fe91d31db27e 5 //
yuki0701 5:fe91d31db27e 6 //#include <ros.h>
yuki0701 5:fe91d31db27e 7 //#include <geometry_msgs/Point.h>
yuki0701 5:fe91d31db27e 8 //#include <geometry_msgs/Pose.h>
yuki0701 5:fe91d31db27e 9 //
yuki0701 5:fe91d31db27e 10 //ros::NodeHandle nh;
yuki0701 5:fe91d31db27e 11 //geometry_msgs::Point posi_xyr;
yuki0701 5:fe91d31db27e 12 ////geometry_msgs::Quaternion usw_4info;
yuki0701 5:fe91d31db27e 13 //
yuki0701 5:fe91d31db27e 14 //ros::Publisher pub_xyr("/check_1",&posi_xyr);
yuki0701 5:fe91d31db27e 15 ////ros::Publisher pub_usw("/mbed_main2",&usw_4info);
yuki0701 5:fe91d31db27e 16 //
yuki0701 5:fe91d31db27e 17 //Ticker cm_pc;
yuki0701 5:fe91d31db27e 18 //#define PI 3.141592
yuki0701 5:fe91d31db27e 19 //#define RESOLUTION 512 //分解能
yuki0701 5:fe91d31db27e 20 //#define BASIC_SPEED 40 //動確
yuki0701 5:fe91d31db27e 21 //
yuki0701 5:fe91d31db27e 22 //Ec4multi EC_1(p16,p15,RESOLUTION);
yuki0701 5:fe91d31db27e 23 //Ec4multi EC_2(p18,p17,RESOLUTION);
yuki0701 5:fe91d31db27e 24 //SpeedControl motor_1(p21,p22,50,EC_1);
yuki0701 5:fe91d31db27e 25 //SpeedControl motor_2(p24,p23,50,EC_2);
yuki0701 5:fe91d31db27e 26 //
yuki0701 5:fe91d31db27e 27 //Ticker motor_tick; //角速度計算用ticker
yuki0701 5:fe91d31db27e 28 //Timer timer;
yuki0701 5:fe91d31db27e 29 //
yuki0701 5:fe91d31db27e 30 //int i = 0;
yuki0701 5:fe91d31db27e 31 //int j=0;
yuki0701 5:fe91d31db27e 32 ////Serial pc(USBTX,USBRX);
yuki0701 5:fe91d31db27e 33 //double rotate_1=40,rotate_2=40;
yuki0701 5:fe91d31db27e 34 //
yuki0701 5:fe91d31db27e 35 //double gb1, gb2;
yuki0701 5:fe91d31db27e 36 //double mt_out1,mt_out2;
yuki0701 5:fe91d31db27e 37 //
yuki0701 5:fe91d31db27e 38 ////void messageCallback(const geometry_msgs::Quaternion &mt1){
yuki0701 5:fe91d31db27e 39 //// mt_out1 = mt1.x;
yuki0701 5:fe91d31db27e 40 //// mt_out2 = mt1.y;
yuki0701 5:fe91d31db27e 41 //// gb1 = mt1.z;
yuki0701 5:fe91d31db27e 42 //// gb2 = mt1.w;
yuki0701 5:fe91d31db27e 43 ////}
yuki0701 5:fe91d31db27e 44 //
yuki0701 5:fe91d31db27e 45 ////ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_mt1", &messageCallback);
yuki0701 5:fe91d31db27e 46 //
yuki0701 5:fe91d31db27e 47 //void cm_to_pc(){
yuki0701 5:fe91d31db27e 48 //
yuki0701 5:fe91d31db27e 49 //
yuki0701 5:fe91d31db27e 50 // posi_xyr.x = 1;
yuki0701 5:fe91d31db27e 51 // posi_xyr.y = 2;
yuki0701 5:fe91d31db27e 52 // posi_xyr.z = 3;
yuki0701 5:fe91d31db27e 53 //
yuki0701 5:fe91d31db27e 54 //
yuki0701 5:fe91d31db27e 55 // pub_xyr.publish(&posi_xyr);
yuki0701 5:fe91d31db27e 56 //
yuki0701 5:fe91d31db27e 57 //}
yuki0701 5:fe91d31db27e 58 //
yuki0701 5:fe91d31db27e 59 //
yuki0701 5:fe91d31db27e 60 ////-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
yuki0701 5:fe91d31db27e 61 ////DigitalOut cansend_led(LED1); //cansend -> on //can.cpp
yuki0701 5:fe91d31db27e 62 ////DigitalOut canread_led(LED2); //canread -> on //can.cpp
yuki0701 5:fe91d31db27e 63 ////DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
yuki0701 5:fe91d31db27e 64 //
yuki0701 5:fe91d31db27e 65 ////////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
yuki0701 5:fe91d31db27e 66 //
yuki0701 5:fe91d31db27e 67 //int main()
yuki0701 5:fe91d31db27e 68 //{
yuki0701 5:fe91d31db27e 69 // nh.getHardware()->setBaud(115200);
yuki0701 5:fe91d31db27e 70 // nh.initNode();
yuki0701 5:fe91d31db27e 71 // nh.advertise(pub_xyr);
yuki0701 5:fe91d31db27e 72 //// nh.advertise(pub_usw);
yuki0701 5:fe91d31db27e 73 //
yuki0701 5:fe91d31db27e 74 // cm_pc.attach(&cm_to_pc,0.01);
yuki0701 5:fe91d31db27e 75 //
yuki0701 5:fe91d31db27e 76 //// UserLoopSetting_sensor();
yuki0701 5:fe91d31db27e 77 //
yuki0701 5:fe91d31db27e 78 // while(1) {
yuki0701 5:fe91d31db27e 79 //
yuki0701 5:fe91d31db27e 80 // nh.spinOnce();
yuki0701 5:fe91d31db27e 81 // wait(0.01);
yuki0701 5:fe91d31db27e 82 //
yuki0701 5:fe91d31db27e 83 // }
yuki0701 5:fe91d31db27e 84 //
yuki0701 5:fe91d31db27e 85 //}
yuki0701 5:fe91d31db27e 86 //
yuki0701 5:fe91d31db27e 87 //
yuki0701 5:fe91d31db27e 88
yuki0701 5:fe91d31db27e 89
yuki0701 5:fe91d31db27e 90
yuki0701 5:fe91d31db27e 91
yuki0701 5:fe91d31db27e 92
yuki0701 5:fe91d31db27e 93
yuki0701 5:fe91d31db27e 94 ////
yuki0701 5:fe91d31db27e 95
aoikoizumi 0:6db935b161f8 96 //CAN通信で受け取った(double)rotate‗1～rotate‗3に対し、実際に回転数制御を行う
yuki0701 5:fe91d31db27e 97
aoikoizumi 0:6db935b161f8 98 #include "mbed.h"
yuki0701 5:fe91d31db27e 99 #include "math.h"
yuki0701 5:fe91d31db27e 100 #include <ros.h>
yuki0701 5:fe91d31db27e 101 #include <geometry_msgs/Point.h>
yuki0701 5:fe91d31db27e 102 #include <geometry_msgs/Pose.h>
yuki0701 5:fe91d31db27e 103 #include <std_msgs/String.h>
aoikoizumi 0:6db935b161f8 104 #include "EC.h" //Encoderライブラリをインクルード
aoikoizumi 0:6db935b161f8 105 #include "SpeedController.h" //SpeedControlライブラリをインクルード
yuki0701 5:fe91d31db27e 106
yuki0701 5:fe91d31db27e 107 ros::NodeHandle nh;
yuki0701 5:fe91d31db27e 108 geometry_msgs::Point check_com;
yuki0701 5:fe91d31db27e 109
yuki0701 5:fe91d31db27e 110 ros::Publisher pub_check("/check_1",&check_com);
yuki0701 5:fe91d31db27e 111
yuki0701 5:fe91d31db27e 112
yuki0701 5:fe91d31db27e 113 int i = 0;
yuki0701 5:fe91d31db27e 114
yuki0701 5:fe91d31db27e 115 double mt_out1,mt_out2;
yuki0701 5:fe91d31db27e 116
aoikoizumi 0:6db935b161f8 117 #define RESOLUTION 512 //分解能
aoikoizumi 3:e7807f60c9bf 118 #define BASIC_SPEED 40 //動確用
aoikoizumi 0:6db935b161f8 119 //#define TEST_DUTY 0.3 //動確用
aoikoizumi 0:6db935b161f8 120
aoikoizumi 0:6db935b161f8 121
yuki0701 5:fe91d31db27e 122 Ec4multi EC_1(p13,p14,RESOLUTION);
yuki0701 5:fe91d31db27e 123 Ec4multi EC_2(p15,p16,RESOLUTION);
yuki0701 4:46c10d34af27 124 //Ec4multi EC_1(p15,p16,RESOLUTION);
yuki0701 4:46c10d34af27 125 //Ec4multi EC_2(p17,p18,RESOLUTION);
aoikoizumi 0:6db935b161f8 126
aoikoizumi 0:6db935b161f8 127 SpeedControl motor_1(p21,p22,50,EC_1);
yuki0701 4:46c10d34af27 128 SpeedControl motor_2(p24,p23,50,EC_2);
yuki0701 4:46c10d34af27 129 //SpeedControl motor_1(p21,p22,50,EC_1);
yuki0701 4:46c10d34af27 130 //SpeedControl motor_2(p24,p23,50,EC_2);
aoikoizumi 0:6db935b161f8 131
aoikoizumi 0:6db935b161f8 132 Ticker motor_tick; //角速度計算用ticker
yuki0701 5:fe91d31db27e 133 //Serial pc(USBTX,USBRX); //このコメントを外したままROSとの通信をしようとすると、なぜか通信ができないので、コメントアウトしています。
aoikoizumi 0:6db935b161f8 134
aoikoizumi 1:0e8ec231cb2f 135 Timer timer;
aoikoizumi 1:0e8ec231cb2f 136
aoikoizumi 1:0e8ec231cb2f 137
aoikoizumi 1:0e8ec231cb2f 138 int j=0;
yuki0701 4:46c10d34af27 139 double rotate_1=40,rotate_2=40;
yuki0701 5:fe91d31db27e 140 double gO_1;
yuki0701 5:fe91d31db27e 141 double gO_2;
yuki0701 5:fe91d31db27e 142 double MT01=0,MT02=0;
yuki0701 4:46c10d34af27 143 //double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
yuki0701 4:46c10d34af27 144 //int motor1_count[1000]= {},motor2_count[1000]= {};
yuki0701 5:fe91d31db27e 145 double gb1, gb2;
yuki0701 5:fe91d31db27e 146
yuki0701 5:fe91d31db27e 147 void messageCallback(const geometry_msgs::Quaternion &mt1){
yuki0701 5:fe91d31db27e 148 mt_out1 = (double)mt1.x/(double)4;
yuki0701 5:fe91d31db27e 149 mt_out2 = (double)mt1.y/(double)4;
yuki0701 5:fe91d31db27e 150 gb1 = (double)mt1.z;
yuki0701 5:fe91d31db27e 151 gb2 = (double)mt1.w;
yuki0701 5:fe91d31db27e 152 }
yuki0701 5:fe91d31db27e 153
yuki0701 5:fe91d31db27e 154 ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_mt1", &messageCallback);
aoikoizumi 1:0e8ec231cb2f 155
aoikoizumi 1:0e8ec231cb2f 156 void CalOmega()
aoikoizumi 1:0e8ec231cb2f 157 {
yuki0701 5:fe91d31db27e 158
yuki0701 5:fe91d31db27e 159 check_com.x = mt_out1;
yuki0701 5:fe91d31db27e 160 check_com.y = mt_out2;
yuki0701 5:fe91d31db27e 161 check_com.z = 12;
yuki0701 5:fe91d31db27e 162
yuki0701 5:fe91d31db27e 163 pub_check.publish(&check_com);
yuki0701 5:fe91d31db27e 164
yuki0701 5:fe91d31db27e 165 //printf("check\n\r");
yuki0701 5:fe91d31db27e 166
yuki0701 5:fe91d31db27e 167
yuki0701 4:46c10d34af27 168 //motor1_count[j]=EC_1.getCount();
yuki0701 4:46c10d34af27 169 //motor2_count[j]=EC_2.getCount();
aoikoizumi 3:e7807f60c9bf 170 EC_1.calOmega();
aoikoizumi 3:e7807f60c9bf 171 EC_2.calOmega();
yuki0701 5:fe91d31db27e 172 gO_1=EC_1.getOmega();
yuki0701 5:fe91d31db27e 173 gO_2=EC_2.getOmega();
yuki0701 4:46c10d34af27 174 //motor1_omega[j]=EC_1.getOmega();
yuki0701 4:46c10d34af27 175 //motor2_omega[j]=EC_2.getOmega();
aoikoizumi 1:0e8ec231cb2f 176 // time_when[j]=EC_1.timer_.read();
yuki0701 5:fe91d31db27e 177 MT01=mt_out1;
yuki0701 5:fe91d31db27e 178 MT02=mt_out2;
aoikoizumi 1:0e8ec231cb2f 179 if(rotate_1==0) motor_1.stop();
yuki0701 5:fe91d31db27e 180 else motor_1.Sc(MT01,1,EC_1.getOmega());
aoikoizumi 1:0e8ec231cb2f 181 if(rotate_2==0) motor_2.stop();
yuki0701 5:fe91d31db27e 182 else motor_2.Sc(MT02,2,EC_2.getOmega());
aoikoizumi 3:e7807f60c9bf 183
yuki0701 5:fe91d31db27e 184 j++;
aoikoizumi 1:0e8ec231cb2f 185 }
yuki0701 5:fe91d31db27e 186
aoikoizumi 0:6db935b161f8 187 int main()
aoikoizumi 0:6db935b161f8 188 {
yuki0701 5:fe91d31db27e 189
yuki0701 5:fe91d31db27e 190 nh.getHardware()->setBaud(115200);
yuki0701 5:fe91d31db27e 191 nh.initNode();
yuki0701 5:fe91d31db27e 192 nh.advertise(pub_check);
yuki0701 5:fe91d31db27e 193 nh.subscribe(sub);
yuki0701 5:fe91d31db27e 194
aoikoizumi 0:6db935b161f8 195 motor_1.period_us(50);
aoikoizumi 0:6db935b161f8 196 motor_2.period_us(50);
yuki0701 5:fe91d31db27e 197 //motor_1.setEquation(0.0429,0.05,0.0429,0.05); //求めたC,Dの値を設定
yuki0701 5:fe91d31db27e 198 //motor_2.setEquation(0.0429,0.05,0.0429,0.05); //求めたC,Dの値を設定
yuki0701 5:fe91d31db27e 199 motor_1.setEquation(0.0013,0.0525,0.0013,0.045); //求めたC,Dの値を設定
yuki0701 5:fe91d31db27e 200 //motor_2.setEquation(0.0013,0.03,0.0013,0.035); //求めたC,Dの値を設定
yuki0701 5:fe91d31db27e 201 motor_2.setEquation(0.0013,0.05,0.0013,0.055); //求めたC,Dの値を設定//no arm ver
aoikoizumi 0:6db935b161f8 202 motor_1.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:6db935b161f8 203 motor_2.setPDparam(0,0.0); //PIDの係数を設定
yuki0701 5:fe91d31db27e 204 //motor_1.setPDparam(0.1,0.0); //PIDの係数を設定
yuki0701 5:fe91d31db27e 205 //motor_2.setPDparam(0.1,0.0); //PIDの係数を設定
yuki0701 5:fe91d31db27e 206 motor_tick.attach(&CalOmega,0.01);
aoikoizumi 0:6db935b161f8 207
yuki0701 4:46c10d34af27 208
aoikoizumi 0:6db935b161f8 209 while(1) {
yuki0701 5:fe91d31db27e 210 nh.spinOnce();
yuki0701 5:fe91d31db27e 211 wait(0.01);
aoikoizumi 0:6db935b161f8 212 }
yuki0701 4:46c10d34af27 213 printf("finish\n\r");
aoikoizumi 0:6db935b161f8 214 }