ロボステ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@3:e7807f60c9bf, 2019-11-09 (annotated)

Committer:
aoikoizumi
Date:
Sat Nov 09 03:35:33 2019 +0000
Revision:
3:e7807f60c9bf
Parent:
2:c4e456559941 
2019 11 9 ;

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 3:e7807f60c9bf 6 #define BASIC_SPEED 40 //動確用
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
aoikoizumi 0:6db935b161f8 13 SpeedControl motor_1(p21,p22,50,EC_1);
aoikoizumi 0:6db935b161f8 14 SpeedControl motor_2(p23,p24,50,EC_2);
aoikoizumi 0:6db935b161f8 15
aoikoizumi 0:6db935b161f8 16 Ticker motor_tick; //角速度計算用ticker
aoikoizumi 0:6db935b161f8 17 Serial pc(USBTX,USBRX);
aoikoizumi 0:6db935b161f8 18
aoikoizumi 1:0e8ec231cb2f 19 Timer timer;
aoikoizumi 1:0e8ec231cb2f 20
aoikoizumi 1:0e8ec231cb2f 21
aoikoizumi 1:0e8ec231cb2f 22 int j=0;
aoikoizumi 1:0e8ec231cb2f 23 double rotate_1=0,rotate_2=0;
aoikoizumi 3:e7807f60c9bf 24 double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
aoikoizumi 3:e7807f60c9bf 25 int motor1_count[1000]= {},motor2_count[1000]= {};
aoikoizumi 1:0e8ec231cb2f 26
aoikoizumi 1:0e8ec231cb2f 27 void CalOmega()
aoikoizumi 1:0e8ec231cb2f 28 {
aoikoizumi 1:0e8ec231cb2f 29 motor1_count[j]=EC_1.getCount();
aoikoizumi 3:e7807f60c9bf 30 motor2_count[j]=EC_2.getCount();
aoikoizumi 3:e7807f60c9bf 31 EC_1.calOmega();
aoikoizumi 3:e7807f60c9bf 32 EC_2.calOmega();
aoikoizumi 1:0e8ec231cb2f 33 motor1_omega[j]=EC_1.getOmega();
aoikoizumi 1:0e8ec231cb2f 34 motor2_omega[j]=EC_2.getOmega();
aoikoizumi 1:0e8ec231cb2f 35 // time_when[j]=EC_1.timer_.read();
aoikoizumi 1:0e8ec231cb2f 36 if(rotate_1==0) motor_1.stop();
aoikoizumi 1:0e8ec231cb2f 37 else motor_1.Sc(rotate_1);
aoikoizumi 1:0e8ec231cb2f 38 if(rotate_2==0) motor_2.stop();
aoikoizumi 1:0e8ec231cb2f 39 else motor_2.Sc(rotate_2);
aoikoizumi 3:e7807f60c9bf 40
aoikoizumi 1:0e8ec231cb2f 41 j++;
aoikoizumi 1:0e8ec231cb2f 42 }
aoikoizumi 0:6db935b161f8 43 int main()
aoikoizumi 0:6db935b161f8 44 {
aoikoizumi 0:6db935b161f8 45 motor_1.period_us(50);
aoikoizumi 0:6db935b161f8 46 motor_2.period_us(50);
aoikoizumi 3:e7807f60c9bf 47 motor_1.setEquation(0.0322,0.08,0.0322,0.0800); //求めたC,Dの値を設定
aoikoizumi 3:e7807f60c9bf 48 motor_2.setEquation(0.0302,0.0755,0.0241,0.1301); //求めたC,Dの値を設定
aoikoizumi 0:6db935b161f8 49 motor_1.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:6db935b161f8 50 motor_2.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 1:0e8ec231cb2f 51 motor_tick.attach(&CalOmega,0.05);
aoikoizumi 0:6db935b161f8 52
aoikoizumi 0:6db935b161f8 53 while(1) {
aoikoizumi 1:0e8ec231cb2f 54 if(pc.readable()) {
aoikoizumi 1:0e8ec231cb2f 55 char sel=pc.getc();
aoikoizumi 1:0e8ec231cb2f 56 if(sel=='q') {
aoikoizumi 1:0e8ec231cb2f 57 for(int i=0; i<1000; i++) {
aoikoizumi 1:0e8ec231cb2f 58 // pc.printf("%f ",time_when[i]);
aoikoizumi 1:0e8ec231cb2f 59 pc.printf("%f,%f ",motor1_omega[i],motor2_omega[i]);
aoikoizumi 1:0e8ec231cb2f 60 pc.printf("%d,%d\r\n",motor1_count[i],motor2_count[i]);
aoikoizumi 1:0e8ec231cb2f 61 }
aoikoizumi 3:e7807f60c9bf 62 } else if(sel=='i') {
aoikoizumi 3:e7807f60c9bf 63 pc.printf("fffff\r\n");
aoikoizumi 3:e7807f60c9bf 64 rotate_1=1*BASIC_SPEED;
aoikoizumi 3:e7807f60c9bf 65 rotate_2=1*BASIC_SPEED;
aoikoizumi 3:e7807f60c9bf 66 } else if(sel=='m') {
aoikoizumi 3:e7807f60c9bf 67 pc.printf("bbbbb\r\n");
aoikoizumi 3:e7807f60c9bf 68 rotate_1=-1*BASIC_SPEED;
aoikoizumi 3:e7807f60c9bf 69 rotate_2=-1*BASIC_SPEED;
aoikoizumi 3:e7807f60c9bf 70 }else if(sel=='s') {
aoikoizumi 3:e7807f60c9bf 71 pc.printf("sssss\r\n");
aoikoizumi 3:e7807f60c9bf 72 rotate_1=0;
aoikoizumi 3:e7807f60c9bf 73 rotate_2=0;
aoikoizumi 3:e7807f60c9bf 74 }
aoikoizumi 0:6db935b161f8 75 }
aoikoizumi 0:6db935b161f8 76 }
aoikoizumi 0:6db935b161f8 77 }