ロボステ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

ccheck.cpp@1:798a41fa6515, 2019-09-02 (annotated)

Committer:
aoikoizumi
Date:
Mon Sep 02 01:26:02 2019 +0000
Revision:
1:798a41fa6515
Parent:
dcheck.cpp@0:112e277bd7f2 
c

Who changed what in which revision?

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