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

2月25日

Dependencies:   mbed SpeedController___2_25 ros_lib_kinetic

main.cpp@1:9d6e9312dab1, 2020-02-25 (annotated)

Committer:
yuki0701
Date:
Tue Feb 25 01:27:07 2020 +0000
Revision:
1:9d6e9312dab1
Parent:
0:c38fec6e4afd 
a

Who changed what in which revision?

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