ロボステ6期 / Mbed 2 deprecated PR_test_3w

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SpeedController

main.cpp@0:6db935b161f8, 2019-10-09 (annotated)

Committer:
aoikoizumi
Date:
Wed Oct 09 11:19:20 2019 +0000
Revision:
0:6db935b161f8
Child:
1:0e8ec231cb2f
NHK motorcontrol(10/09)

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ライブラリをインクルード
aoikoizumi 0:6db935b161f8 5 #define RESOLUTION 512 //分解能
aoikoizumi 0:6db935b161f8 6 //#define BASIC_SPEED 1 //動確用
aoikoizumi 0:6db935b161f8 7 //#define TEST_DUTY 0.3 //動確用
aoikoizumi 0:6db935b161f8 8
aoikoizumi 0:6db935b161f8 9
aoikoizumi 0:6db935b161f8 10 Ec4multi EC_1(p15,p16,RESOLUTION);
aoikoizumi 0:6db935b161f8 11 Ec4multi EC_2(p17,p18,RESOLUTION);
aoikoizumi 0:6db935b161f8 12 Ec4multi EC_3(p19,p20,RESOLUTION);
aoikoizumi 0:6db935b161f8 13
aoikoizumi 0:6db935b161f8 14 SpeedControl motor_1(p21,p22,50,EC_1);
aoikoizumi 0:6db935b161f8 15 SpeedControl motor_2(p23,p24,50,EC_2);
aoikoizumi 0:6db935b161f8 16 SpeedControl motor_3(p25,p26,50,EC_3);
aoikoizumi 0:6db935b161f8 17
aoikoizumi 0:6db935b161f8 18 Ticker motor_tick; //角速度計算用ticker
aoikoizumi 0:6db935b161f8 19 Serial pc(USBTX,USBRX);
aoikoizumi 0:6db935b161f8 20
aoikoizumi 0:6db935b161f8 21
aoikoizumi 0:6db935b161f8 22 int main()
aoikoizumi 0:6db935b161f8 23 {
aoikoizumi 0:6db935b161f8 24 motor_1.period_us(50);
aoikoizumi 0:6db935b161f8 25 motor_2.period_us(50);
aoikoizumi 0:6db935b161f8 26 motor_3.period_us(50);
aoikoizumi 0:6db935b161f8 27
aoikoizumi 0:6db935b161f8 28 motor_1.setEquation(0.322,0.08,0,0.0100); //求めたC,Dの値を設定
aoikoizumi 0:6db935b161f8 29 motor_2.setEquation(0.0302,0.0755,-0.0241,0.1301); //求めたC,Dの値を設定
aoikoizumi 0:6db935b161f8 30 motor_3.setEquation(0.0302,0.1500,-0.0301,0.2000); //求めたC,Dの値を設定
aoikoizumi 0:6db935b161f8 31
aoikoizumi 0:6db935b161f8 32 motor_1.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:6db935b161f8 33 motor_2.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:6db935b161f8 34 motor_3.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:6db935b161f8 35
aoikoizumi 0:6db935b161f8 36 int kai=0;
aoikoizumi 0:6db935b161f8 37 double rotate_1=0,rotate_2=0,rotate_3=0;
aoikoizumi 0:6db935b161f8 38
aoikoizumi 0:6db935b161f8 39 while(1) {
aoikoizumi 0:6db935b161f8 40 if(rotate_1==0) motor_1.stop();
aoikoizumi 0:6db935b161f8 41 else motor_1.Sc(rotate_1);
aoikoizumi 0:6db935b161f8 42 if(rotate_2==0) motor_2.stop();
aoikoizumi 0:6db935b161f8 43 else motor_2.Sc(rotate_2);
aoikoizumi 0:6db935b161f8 44 if(rotate_3==0) motor_3.stop();
aoikoizumi 0:6db935b161f8 45 else motor_3.Sc(rotate_3);
aoikoizumi 0:6db935b161f8 46
aoikoizumi 0:6db935b161f8 47 if(kai>=4) {
aoikoizumi 0:6db935b161f8 48 //printf("\r\n");
aoikoizumi 0:6db935b161f8 49 printf("target:%.2f,%.2f.%.2f ",rotate_1,rotate_2,rotate_3);
aoikoizumi 0:6db935b161f8 50 printf("omega_=%.2f,%.2f,%.2f \r\n",EC_1.getOmega(),EC_2.getOmega(),EC_3.getOmega());
aoikoizumi 0:6db935b161f8 51 kai=0;
aoikoizumi 0:6db935b161f8 52 }
aoikoizumi 0:6db935b161f8 53 }
aoikoizumi 0:6db935b161f8 54 }