ロボステ6期 / Mbed 2 deprecated 4rinCchecker

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

dcheck.cpp@0:112e277bd7f2, 2019-09-02 (annotated)

Committer:
aoikoizumi
Date:
Mon Sep 02 01:03:05 2019 +0000
Revision:
0:112e277bd7f2
ccheck

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aoikoizumi 0:112e277bd7f2 1 //2,3で使えるサンプルコード
aoikoizumi 0:112e277bd7f2 2 //入力はTera Termで行う
aoikoizumi 0:112e277bd7f2 3 #include "mbed.h"
aoikoizumi 0:112e277bd7f2 4 #include "EC.h" //Encoderライブラリをインクルード
aoikoizumi 0:112e277bd7f2 5 #include "SpeedController.h"
aoikoizumi 0:112e277bd7f2 6 #define RESOLUTION 2048
aoikoizumi 0:112e277bd7f2 7 #define BASIC_SPEED 20
aoikoizumi 0:112e277bd7f2 8 #define TEST_DUTY 0.3
aoikoizumi 0:112e277bd7f2 9
aoikoizumi 0:112e277bd7f2 10 Ec4multi EC_kagawa(PA_5,PC_9,RESOLUTION);
aoikoizumi 0:112e277bd7f2 11 SpeedControl kagawa(PB_9,PB_8,50,EC_kagawa);
aoikoizumi 0:112e277bd7f2 12 Ec4multi EC_tokusima(PA_15,PA_14,RESOLUTION);
aoikoizumi 0:112e277bd7f2 13 SpeedControl tokusima(PC_7,PA_6,50,EC_tokusima);
aoikoizumi 0:112e277bd7f2 14 Ec4multi EC_kouchi(PC_13,PB_7,RESOLUTION);
aoikoizumi 0:112e277bd7f2 15 SpeedControl kouchi(PA_9,PB_6,50,EC_kouchi);
aoikoizumi 0:112e277bd7f2 16 Ec4multi EC_ehime(PC_2,PC_3,RESOLUTION);
aoikoizumi 0:112e277bd7f2 17 SpeedControl ehime(PB_3,PA_10,50,EC_ehime);
aoikoizumi 0:112e277bd7f2 18 Serial pc(USBTX,USBRX);
aoikoizumi 0:112e277bd7f2 19 Ticker motor_tick;
aoikoizumi 0:112e277bd7f2 20
aoikoizumi 0:112e277bd7f2 21 /*void calOmega(){
aoikoizumi 0:112e277bd7f2 22 motor.CalOmega(); //角速度計算用
aoikoizumi 0:112e277bd7f2 23 }
aoikoizumi 0:112e277bd7f2 24 */
aoikoizumi 0:112e277bd7f2 25 int main(void){
aoikoizumi 0:112e277bd7f2 26 // motor_tick.attach(calOmega,0.05);
aoikoizumi 0:112e277bd7f2 27 kagawa.period_us(50);
aoikoizumi 0:112e277bd7f2 28 tokusima.period_us(50);
aoikoizumi 0:112e277bd7f2 29 kouchi.period_us(50);
aoikoizumi 0:112e277bd7f2 30 ehime.period_us(50);
aoikoizumi 0:112e277bd7f2 31 double kagawa_Cf=0,kagawa_Cb=0;
aoikoizumi 0:112e277bd7f2 32 double tokusima_Cf=0,tokusima_Cb=0;
aoikoizumi 0:112e277bd7f2 33 double kouchi_Cf=0,kouchi_Cb=0;
aoikoizumi 0:112e277bd7f2 34 double ehime_Cf=0,ehime_Cb=0;
aoikoizumi 0:112e277bd7f2 35
aoikoizumi 0:112e277bd7f2 36 kagawa.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:112e277bd7f2 37 tokusima.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:112e277bd7f2 38 kouchi.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:112e277bd7f2 39 ehime.setPDparam(0,0.0); //PIDの係数を設定
aoikoizumi 0:112e277bd7f2 40 int loop_time=0;
aoikoizumi 0:112e277bd7f2 41 double target_kagawa=0,target_tokusima=0,target_kouchi=0,target_ehime=0;
aoikoizumi 0:112e277bd7f2 42 bool print=false;
aoikoizumi 0:112e277bd7f2 43
aoikoizumi 0:112e277bd7f2 44 while(1){
aoikoizumi 0:112e277bd7f2 45 loop_time++;
aoikoizumi 0:112e277bd7f2 46
aoikoizumi 0:112e277bd7f2 47 if(target_kagawa==0) kagawa.stop();
aoikoizumi 0:112e277bd7f2 48 else kagawa.Sc(target_kagawa);
aoikoizumi 0:112e277bd7f2 49 if(target_tokusima==0) tokusima.stop();
aoikoizumi 0:112e277bd7f2 50 else tokusima.Sc(target_tokusima);
aoikoizumi 0:112e277bd7f2 51 if(target_kouchi==0) kouchi.stop();
aoikoizumi 0:112e277bd7f2 52 else kouchi.Sc(target_kouchi);
aoikoizumi 0:112e277bd7f2 53 if(target_ehime==0) ehime.stop();
aoikoizumi 0:112e277bd7f2 54 else ehime.Sc(target_ehime);
aoikoizumi 0:112e277bd7f2 55
aoikoizumi 0:112e277bd7f2 56 kagawa.setEquation(kagawa_Cf,0,kagawa_Cb,0); //求めたDの値を設定
aoikoizumi 0:112e277bd7f2 57 tokusima.setEquation(tokusima_Cf,0,tokusima_Cb,0); //求めたDの値を設定
aoikoizumi 0:112e277bd7f2 58 kouchi.setEquation(kouchi_Cf,0,kouchi_Cb,0); //求めたDの値を設定
aoikoizumi 0:112e277bd7f2 59 ehime.setEquation(ehime_Cf,0,ehime_Cb,0); //求めたDの値を設定
aoikoizumi 0:112e277bd7f2 60
aoikoizumi 0:112e277bd7f2 61 if(pc.readable()){
aoikoizumi 0:112e277bd7f2 62 char sel=pc.getc();
aoikoizumi 0:112e277bd7f2 63 if(sel=='s'){
aoikoizumi 0:112e277bd7f2 64 kagawa.stop();
aoikoizumi 0:112e277bd7f2 65 tokusima.stop();
aoikoizumi 0:112e277bd7f2 66 kouchi.stop();
aoikoizumi 0:112e277bd7f2 67 ehime.stop();
aoikoizumi 0:112e277bd7f2 68 } else if(sel=='e'){
aoikoizumi 0:112e277bd7f2 69 pc.printf("kagawa_Cf");
aoikoizumi 0:112e277bd7f2 70 if(sel=='i')kagawa_Cf+=0.002; //e->iを押すとCfがo.o02ずつあがる
aoikoizumi 0:112e277bd7f2 71 else if(sel=='m'&&kagawa_Cf>0)kagawa_Cf-=0.002; //e->mを押すとCfがo.o02ずつさがる
aoikoizumi 0:112e277bd7f2 72 pc.printf("duty=%f\r\n",kagawa_Cf);
aoikoizumi 0:112e277bd7f2 73 } else if(sel=='d'){
aoikoizumi 0:112e277bd7f2 74 pc.printf("kagawa_Cb");
aoikoizumi 0:112e277bd7f2 75 if(sel=='i')kagawa_Cb+=0.002; //e->iを押すとCfがo.o02ずつあがる
aoikoizumi 0:112e277bd7f2 76 else if(sel=='m'&&kagawa_Cf>0)kagawa_Cb-=0.002; //e->mを押すとCfがo.o02ずつさがる
aoikoizumi 0:112e277bd7f2 77 pc.printf("duty=%f\r\n",kagawa_Cf);
aoikoizumi 0:112e277bd7f2 78 }else if(sel=='r'){
aoikoizumi 0:112e277bd7f2 79 pc.printf("tokusima_Cf");
aoikoizumi 0:112e277bd7f2 80 if(sel=='i')tokusima_Cf+=0.002; //r->iを押すとCfがo.o02ずつあがる
aoikoizumi 0:112e277bd7f2 81 else if(sel=='m'&&tokusima_Cf>0)tokusima_Cf-=0.002; //r->mを押すとCfがo.o02ずつさがる
aoikoizumi 0:112e277bd7f2 82 pc.printf("duty=%f\r\n",tokusima_Cf);
aoikoizumi 0:112e277bd7f2 83 } else if(sel=='f'){
aoikoizumi 0:112e277bd7f2 84 pc.printf("tokusima_Cb");
aoikoizumi 0:112e277bd7f2 85 if(sel=='i')tokusima_Cb+=0.002; //f->iを押すとCfがo.o02ずつあがる
aoikoizumi 0:112e277bd7f2 86 else if(sel=='m'&&tokusima_Cf>0)tokusima_Cb-=0.002; //f->mを押すとCfがo.o02ずつさがる
aoikoizumi 0:112e277bd7f2 87 pc.printf("duty=%f\r\n",tokusima_Cb);
aoikoizumi 0:112e277bd7f2 88
aoikoizumi 0:112e277bd7f2 89 }else if(sel=='t'){
aoikoizumi 0:112e277bd7f2 90 pc.printf("kouchi_Cf");
aoikoizumi 0:112e277bd7f2 91 if(sel=='i')kouchi_Cf+=0.002; //t->iを押すとCfがo.o02ずつあがる
aoikoizumi 0:112e277bd7f2 92 else if(sel=='m'&&kouchi_Cf>0)kouchi_Cf-=0.002; //t->mを押すとCfがo.o02ずつさがる
aoikoizumi 0:112e277bd7f2 93 pc.printf("duty=%f\r\n",kouchi_Cf);
aoikoizumi 0:112e277bd7f2 94 } else if(sel=='g'){
aoikoizumi 0:112e277bd7f2 95 pc.printf("kouchi_Cb");
aoikoizumi 0:112e277bd7f2 96 if(sel=='i')kouchi_Cb+=0.002; //g->iを押すとCfがo.o02ずつあがる
aoikoizumi 0:112e277bd7f2 97 else if(sel=='m'&&kouchi_Cf>0)kouchi_Cb-=0.002; //g->mを押すとCfがo.o02ずつさがる
aoikoizumi 0:112e277bd7f2 98 pc.printf("duty=%f\r\n",kouchi_Cb);
aoikoizumi 0:112e277bd7f2 99
aoikoizumi 0:112e277bd7f2 100 }else if(sel=='y'){
aoikoizumi 0:112e277bd7f2 101 pc.printf("ehime_Cf");
aoikoizumi 0:112e277bd7f2 102 if(sel=='i')ehime_Cf+=0.002; //t->iを押すとCfがo.o02ずつあがる
aoikoizumi 0:112e277bd7f2 103 else if(sel=='m'&&ehime_Cf>0)ehime_Cf-=0.002; //t->mを押すとCfがo.o02ずつさがる
aoikoizumi 0:112e277bd7f2 104 pc.printf("duty=%f\r\n",ehime_Cf);
aoikoizumi 0:112e277bd7f2 105 } else if(sel=='h'){
aoikoizumi 0:112e277bd7f2 106 pc.printf("ehime_Cb");
aoikoizumi 0:112e277bd7f2 107 if(sel=='i')ehime_Cb+=0.002; //g->iを押すとCfがo.o02ずつあがる
aoikoizumi 0:112e277bd7f2 108 else if(sel=='m'&&ehime_Cf>0)ehime_Cb-=0.002; //g->mを押すとCfがo.o02ずつさがる
aoikoizumi 0:112e277bd7f2 109 pc.printf("duty=%f\r\n",ehime_Cb);
aoikoizumi 0:112e277bd7f2 110 }else if(sel=='p'){
aoikoizumi 0:112e277bd7f2 111
aoikoizumi 0:112e277bd7f2 112 print=!print; //pを押すと表示を停止/再開する
aoikoizumi 0:112e277bd7f2 113 }else if(sel=='i'){
aoikoizumi 0:112e277bd7f2 114 target_kagawa=1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 115 target_tokusima=1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 116 target_kouchi=-1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 117 target_ehime=-1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 118
aoikoizumi 0:112e277bd7f2 119 }else if(sel=='m'){
aoikoizumi 0:112e277bd7f2 120 target_kagawa=1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 121 target_tokusima=1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 122 target_kouchi=-1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 123 target_ehime=-1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 124 }else if(sel=='k'){
aoikoizumi 0:112e277bd7f2 125 target_kagawa=1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 126 target_tokusima=1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 127 target_kouchi=-1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 128 target_ehime=-1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 129 }else if(sel=='j'){
aoikoizumi 0:112e277bd7f2 130 target_kagawa=1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 131 target_tokusima=1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 132 target_kouchi=-1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 133 target_ehime=-1*BASIC_SPEED;
aoikoizumi 0:112e277bd7f2 134 }
aoikoizumi 0:112e277bd7f2 135
aoikoizumi 0:112e277bd7f2 136 }
aoikoizumi 0:112e277bd7f2 137
aoikoizumi 0:112e277bd7f2 138 if(loop_time%1000==0){
aoikoizumi 0:112e277bd7f2 139
aoikoizumi 0:112e277bd7f2 140 if(print) pc.printf("%.2f %.2f %.2f %.2f\r\n",EC_kagawa.getOmega(),EC_tokusima.getOmega(),EC_kouchi.getOmega(),EC_ehime.getOmega());
aoikoizumi 0:112e277bd7f2 141 } //左からエンコーダのカウント、角速度、正方向出力duty比、負方向出力duty比を表示
aoikoizumi 0:112e277bd7f2 142 }
aoikoizumi 0:112e277bd7f2 143 }