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

2月25日

Dependencies:   mbed SpeedController2_25 ros_lib_kinetic

main.cpp@4:46c10d34af27, 2019-11-16 (annotated)

Committer:
yuki0701
Date:
Sat Nov 16 06:28:18 2019 +0000
Revision:
4:46c10d34af27
Parent:
3:e7807f60c9bf
Child:
5:fe91d31db27e
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aoikoizumi 0:6db935b161f8 1 //CAN通信で受け取った(double)rotate‗1～rotate‗3に対し、実際に回転数制御を行う
aoikoizumi 0:6db935b161f8 2 #include "mbed.h"
aoikoizumi 0:6db935b161f8 3 #include "EC.h" //Encoderライブラリをインクルード
aoikoizumi 0:6db935b161f8 4 #include "SpeedController.h" //SpeedControlライブラリをインクルード
yuki0701 4:46c10d34af27 5 #include "can.h"
aoikoizumi 0:6db935b161f8 6 #define RESOLUTION 512 //分解能
aoikoizumi 3:e7807f60c9bf 7 #define BASIC_SPEED 40 //動確用
aoikoizumi 0:6db935b161f8 8 //#define TEST_DUTY 0.3 //動確用
aoikoizumi 0:6db935b161f8 9
aoikoizumi 0:6db935b161f8 10
yuki0701 4:46c10d34af27 11 Ec4multi EC_1(p16,p15,RESOLUTION);
yuki0701 4:46c10d34af27 12 Ec4multi EC_2(p18,p17,RESOLUTION);
yuki0701 4:46c10d34af27 13 //Ec4multi EC_1(p15,p16,RESOLUTION);
yuki0701 4:46c10d34af27 14 //Ec4multi EC_2(p17,p18,RESOLUTION);
aoikoizumi 0:6db935b161f8 15
aoikoizumi 0:6db935b161f8 16 SpeedControl motor_1(p21,p22,50,EC_1);
yuki0701 4:46c10d34af27 17 SpeedControl motor_2(p24,p23,50,EC_2);
yuki0701 4:46c10d34af27 18 //SpeedControl motor_1(p21,p22,50,EC_1);
yuki0701 4:46c10d34af27 19 //SpeedControl motor_2(p24,p23,50,EC_2);
aoikoizumi 0:6db935b161f8 20
aoikoizumi 0:6db935b161f8 21 Ticker motor_tick; //角速度計算用ticker
aoikoizumi 0:6db935b161f8 22 Serial pc(USBTX,USBRX);
aoikoizumi 0:6db935b161f8 23
aoikoizumi 1:0e8ec231cb2f 24 Timer timer;
aoikoizumi 1:0e8ec231cb2f 25
aoikoizumi 1:0e8ec231cb2f 26
aoikoizumi 1:0e8ec231cb2f 27 int j=0;
yuki0701 4:46c10d34af27 28 double rotate_1=40,rotate_2=40;
yuki0701 4:46c10d34af27 29 //double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
yuki0701 4:46c10d34af27 30 //int motor1_count[1000]= {},motor2_count[1000]= {};
aoikoizumi 1:0e8ec231cb2f 31
aoikoizumi 1:0e8ec231cb2f 32 void CalOmega()
aoikoizumi 1:0e8ec231cb2f 33 {
yuki0701 4:46c10d34af27 34 //motor1_count[j]=EC_1.getCount();
yuki0701 4:46c10d34af27 35 //motor2_count[j]=EC_2.getCount();
aoikoizumi 3:e7807f60c9bf 36 EC_1.calOmega();
aoikoizumi 3:e7807f60c9bf 37 EC_2.calOmega();
yuki0701 4:46c10d34af27 38 //motor1_omega[j]=EC_1.getOmega();
yuki0701 4:46c10d34af27 39 //motor2_omega[j]=EC_2.getOmega();
aoikoizumi 1:0e8ec231cb2f 40 // time_when[j]=EC_1.timer_.read();
aoikoizumi 1:0e8ec231cb2f 41 if(rotate_1==0) motor_1.stop();
yuki0701 4:46c10d34af27 42 else motor_1.Sc(mt_out1);
aoikoizumi 1:0e8ec231cb2f 43 if(rotate_2==0) motor_2.stop();
yuki0701 4:46c10d34af27 44 else motor_2.Sc(mt_out2);
aoikoizumi 3:e7807f60c9bf 45
aoikoizumi 1:0e8ec231cb2f 46 j++;
aoikoizumi 1:0e8ec231cb2f 47 }
aoikoizumi 0:6db935b161f8 48 int main()
aoikoizumi 0:6db935b161f8 49 {
yuki0701 4:46c10d34af27 50 UserLoopSetting_can();
aoikoizumi 0:6db935b161f8 51 motor_1.period_us(50);
aoikoizumi 0:6db935b161f8 52 motor_2.period_us(50);
yuki0701 4:46c10d34af27 53 motor_1.setEquation(0.0429,0.05,0.0429,0.05); //求めたC,Dの値を設定
yuki0701 4:46c10d34af27 54 motor_2.setEquation(0.0429,0.05,0.0429,0.05); //求めたC,Dの値を設定
aoikoizumi 0:6db935b161f8 55 motor_1.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:6db935b161f8 56 motor_2.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 1:0e8ec231cb2f 57 motor_tick.attach(&CalOmega,0.05);
aoikoizumi 0:6db935b161f8 58
yuki0701 4:46c10d34af27 59 int k = 0;
yuki0701 4:46c10d34af27 60
aoikoizumi 0:6db935b161f8 61 while(1) {
yuki0701 4:46c10d34af27 62 // rotate_1 = mt_out1;
yuki0701 4:46c10d34af27 63 // rotate_2 = mt_out2;
yuki0701 4:46c10d34af27 64
yuki0701 4:46c10d34af27 65 if(k>100){
yuki0701 4:46c10d34af27 66 printf("\nloop\n\n\r");
yuki0701 4:46c10d34af27 67 k=0;
aoikoizumi 3:e7807f60c9bf 68 }
yuki0701 4:46c10d34af27 69 k++;
aoikoizumi 0:6db935b161f8 70 }
yuki0701 4:46c10d34af27 71 printf("finish\n\r");
aoikoizumi 0:6db935b161f8 72 }