ロボステ6期
/
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
Revision 1:798a41fa6515, committed 2019-09-02
- Comitter:
- aoikoizumi
- Date:
- Mon Sep 02 01:26:02 2019 +0000
- Parent:
- 0:112e277bd7f2
- Commit message:
- c
Changed in this revision
| ccheck.cpp | Show annotated file Show diff for this revision Revisions of this file |
| dcheck.cpp | Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ccheck.cpp Mon Sep 02 01:26:02 2019 +0000
@@ -0,0 +1,145 @@
+//モーターの角速度(x)とduty比(y)の関係を
+//y=Cx+Dであると近似した時の、定数Cを決定するためのプログラム
+//別プログラムで求めたdを使い傾きCを決定する
+//入力はTera Termで行う
+#include "mbed.h"
+ #include "EC.h" //Encoderライブラリをインクルード
+#include "SpeedController.h"
+ #define RESOLUTION 2048//分解能
+ #define BASIC_SPEED 20//目標角速度、一番使う速度帯におおよそ合わせるが吉
+// #define TEST_DUTY 0.3
+
+Ec4multi EC_kagawa(PA_5,PC_9,RESOLUTION);
+SpeedControl kagawa(PB_9,PB_8,50,EC_kagawa);
+Ec4multi EC_tokusima(PA_15,PA_14,RESOLUTION);
+SpeedControl tokusima(PC_7,PA_6,50,EC_tokusima);
+Ec4multi EC_kouchi(PC_13,PB_7,RESOLUTION);
+SpeedControl kouchi(PA_9,PB_6,50,EC_kouchi);
+Ec4multi EC_ehime(PC_2,PC_3,RESOLUTION);
+SpeedControl ehime(PB_3,PA_10,50,EC_ehime);
+Serial pc(USBTX,USBRX);
+Ticker motor_tick;
+
+/*void calOmega(){
+ motor.CalOmega(); //角速度計算用
+}
+ */
+int main(void){
+// motor_tick.attach(calOmega,0.05);
+ kagawa.period_us(50);
+ tokusima.period_us(50);
+ kouchi.period_us(50);
+ ehime.period_us(50);
+ double kagawa_Cf=0,kagawa_Cb=0;
+ double tokusima_Cf=0,tokusima_Cb=0;
+ double kouchi_Cf=0,kouchi_Cb=0;
+ double ehime_Cf=0,ehime_Cb=0;
+
+ kagawa.setPDparam(0,0.0); //PIDの係数を設定
+ tokusima.setPDparam(0,0.0); //PIDの係数を設定
+ kouchi.setPDparam(0,0.0); //PIDの係数を設定
+ ehime.setPDparam(0,0.0); //PIDの係数を設定
+ int loop_time=0;
+ double target_kagawa=0,target_tokusima=0,target_kouchi=0,target_ehime=0;
+ bool print=false;
+
+ while(1){
+ loop_time++;
+
+ if(target_kagawa==0) kagawa.stop();
+ else kagawa.Sc(target_kagawa);
+ if(target_tokusima==0) tokusima.stop();
+ else tokusima.Sc(target_tokusima);
+ if(target_kouchi==0) kouchi.stop();
+ else kouchi.Sc(target_kouchi);
+ if(target_ehime==0) ehime.stop();
+ else ehime.Sc(target_ehime);
+
+ kagawa.setEquation(kagawa_Cf,0,kagawa_Cb,0); //求めたDの値を設定
+ tokusima.setEquation(tokusima_Cf,0,tokusima_Cb,0); //求めたDの値を設定
+ kouchi.setEquation(kouchi_Cf,0,kouchi_Cb,0); //求めたDの値を設定
+ ehime.setEquation(ehime_Cf,0,ehime_Cb,0); //求めたDの値を設定
+
+ if(pc.readable()){
+ char sel=pc.getc();
+ if(sel=='s'){
+ kagawa.stop();
+ tokusima.stop();
+ kouchi.stop();
+ ehime.stop();
+ } else if(sel=='e'){
+ pc.printf("kagawa_Cf");
+ if(sel=='i')kagawa_Cf+=0.002; //e->iを押すとCfがo.o02ずつあがる
+ else if(sel=='m'&&kagawa_Cf>0)kagawa_Cf-=0.002; //e->mを押すとCfがo.o02ずつさがる
+ pc.printf("duty=%f\r\n",kagawa_Cf);
+ } else if(sel=='d'){
+ pc.printf("kagawa_Cb");
+ if(sel=='i')kagawa_Cb+=0.002; //e->iを押すとCfがo.o02ずつあがる
+ else if(sel=='m'&&kagawa_Cf>0)kagawa_Cb-=0.002; //e->mを押すとCfがo.o02ずつさがる
+ pc.printf("duty=%f\r\n",kagawa_Cf);
+ }else if(sel=='r'){
+ pc.printf("tokusima_Cf");
+ if(sel=='i')tokusima_Cf+=0.002; //r->iを押すとCfがo.o02ずつあがる
+ else if(sel=='m'&&tokusima_Cf>0)tokusima_Cf-=0.002; //r->mを押すとCfがo.o02ずつさがる
+ pc.printf("duty=%f\r\n",tokusima_Cf);
+ } else if(sel=='f'){
+ pc.printf("tokusima_Cb");
+ if(sel=='i')tokusima_Cb+=0.002; //f->iを押すとCfがo.o02ずつあがる
+ else if(sel=='m'&&tokusima_Cf>0)tokusima_Cb-=0.002; //f->mを押すとCfがo.o02ずつさがる
+ pc.printf("duty=%f\r\n",tokusima_Cb);
+
+ }else if(sel=='t'){
+ pc.printf("kouchi_Cf");
+ if(sel=='i')kouchi_Cf+=0.002; //t->iを押すとCfがo.o02ずつあがる
+ else if(sel=='m'&&kouchi_Cf>0)kouchi_Cf-=0.002; //t->mを押すとCfがo.o02ずつさがる
+ pc.printf("duty=%f\r\n",kouchi_Cf);
+ } else if(sel=='g'){
+ pc.printf("kouchi_Cb");
+ if(sel=='i')kouchi_Cb+=0.002; //g->iを押すとCfがo.o02ずつあがる
+ else if(sel=='m'&&kouchi_Cf>0)kouchi_Cb-=0.002; //g->mを押すとCfがo.o02ずつさがる
+ pc.printf("duty=%f\r\n",kouchi_Cb);
+
+ }else if(sel=='y'){
+ pc.printf("ehime_Cf");
+ if(sel=='i')ehime_Cf+=0.002; //t->iを押すとCfがo.o02ずつあがる
+ else if(sel=='m'&&ehime_Cf>0)ehime_Cf-=0.002; //t->mを押すとCfがo.o02ずつさがる
+ pc.printf("duty=%f\r\n",ehime_Cf);
+ } else if(sel=='h'){
+ pc.printf("ehime_Cb");
+ if(sel=='i')ehime_Cb+=0.002; //g->iを押すとCfがo.o02ずつあがる
+ else if(sel=='m'&&ehime_Cf>0)ehime_Cb-=0.002; //g->mを押すとCfがo.o02ずつさがる
+ pc.printf("duty=%f\r\n",ehime_Cb);
+ }else if(sel=='p'){
+
+ print=!print; //pを押すと表示を停止/再開する
+ }else if(sel=='i'){
+ target_kagawa=1*BASIC_SPEED;
+ target_tokusima=1*BASIC_SPEED;
+ target_kouchi=-1*BASIC_SPEED;
+ target_ehime=-1*BASIC_SPEED;
+
+ }else if(sel=='m'){
+ target_kagawa=1*BASIC_SPEED;
+ target_tokusima=1*BASIC_SPEED;
+ target_kouchi=-1*BASIC_SPEED;
+ target_ehime=-1*BASIC_SPEED;
+ }else if(sel=='k'){
+ target_kagawa=1*BASIC_SPEED;
+ target_tokusima=1*BASIC_SPEED;
+ target_kouchi=-1*BASIC_SPEED;
+ target_ehime=-1*BASIC_SPEED;
+ }else if(sel=='j'){
+ target_kagawa=1*BASIC_SPEED;
+ target_tokusima=1*BASIC_SPEED;
+ target_kouchi=-1*BASIC_SPEED;
+ target_ehime=-1*BASIC_SPEED;
+ }
+
+ }
+
+ if(loop_time%1000==0){
+
+ if(print) pc.printf("%.2f %.2f %.2f %.2f\r\n",EC_kagawa.getOmega(),EC_tokusima.getOmega(),EC_kouchi.getOmega(),EC_ehime.getOmega());
+ }
+ }
+}
--- a/dcheck.cpp Mon Sep 02 01:03:05 2019 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,143 +0,0 @@
-//2,3で使えるサンプルコード
-//入力はTera Termで行う
-#include "mbed.h"
- #include "EC.h" //Encoderライブラリをインクルード
-#include "SpeedController.h"
- #define RESOLUTION 2048
- #define BASIC_SPEED 20
- #define TEST_DUTY 0.3
-
-Ec4multi EC_kagawa(PA_5,PC_9,RESOLUTION);
-SpeedControl kagawa(PB_9,PB_8,50,EC_kagawa);
-Ec4multi EC_tokusima(PA_15,PA_14,RESOLUTION);
-SpeedControl tokusima(PC_7,PA_6,50,EC_tokusima);
-Ec4multi EC_kouchi(PC_13,PB_7,RESOLUTION);
-SpeedControl kouchi(PA_9,PB_6,50,EC_kouchi);
-Ec4multi EC_ehime(PC_2,PC_3,RESOLUTION);
-SpeedControl ehime(PB_3,PA_10,50,EC_ehime);
-Serial pc(USBTX,USBRX);
-Ticker motor_tick;
-
-/*void calOmega(){
- motor.CalOmega(); //角速度計算用
-}
- */
-int main(void){
-// motor_tick.attach(calOmega,0.05);
- kagawa.period_us(50);
- tokusima.period_us(50);
- kouchi.period_us(50);
- ehime.period_us(50);
- double kagawa_Cf=0,kagawa_Cb=0;
- double tokusima_Cf=0,tokusima_Cb=0;
- double kouchi_Cf=0,kouchi_Cb=0;
- double ehime_Cf=0,ehime_Cb=0;
-
- kagawa.setPDparam(0,0.0); //PIDの係数を設定
- tokusima.setPDparam(0,0.0); //PIDの係数を設定
- kouchi.setPDparam(0,0.0); //PIDの係数を設定
- ehime.setPDparam(0,0.0); //PIDの係数を設定
- int loop_time=0;
- double target_kagawa=0,target_tokusima=0,target_kouchi=0,target_ehime=0;
- bool print=false;
-
- while(1){
- loop_time++;
-
- if(target_kagawa==0) kagawa.stop();
- else kagawa.Sc(target_kagawa);
- if(target_tokusima==0) tokusima.stop();
- else tokusima.Sc(target_tokusima);
- if(target_kouchi==0) kouchi.stop();
- else kouchi.Sc(target_kouchi);
- if(target_ehime==0) ehime.stop();
- else ehime.Sc(target_ehime);
-
- kagawa.setEquation(kagawa_Cf,0,kagawa_Cb,0); //求めたDの値を設定
- tokusima.setEquation(tokusima_Cf,0,tokusima_Cb,0); //求めたDの値を設定
- kouchi.setEquation(kouchi_Cf,0,kouchi_Cb,0); //求めたDの値を設定
- ehime.setEquation(ehime_Cf,0,ehime_Cb,0); //求めたDの値を設定
-
- if(pc.readable()){
- char sel=pc.getc();
- if(sel=='s'){
- kagawa.stop();
- tokusima.stop();
- kouchi.stop();
- ehime.stop();
- } else if(sel=='e'){
- pc.printf("kagawa_Cf");
- if(sel=='i')kagawa_Cf+=0.002; //e->iを押すとCfがo.o02ずつあがる
- else if(sel=='m'&&kagawa_Cf>0)kagawa_Cf-=0.002; //e->mを押すとCfがo.o02ずつさがる
- pc.printf("duty=%f\r\n",kagawa_Cf);
- } else if(sel=='d'){
- pc.printf("kagawa_Cb");
- if(sel=='i')kagawa_Cb+=0.002; //e->iを押すとCfがo.o02ずつあがる
- else if(sel=='m'&&kagawa_Cf>0)kagawa_Cb-=0.002; //e->mを押すとCfがo.o02ずつさがる
- pc.printf("duty=%f\r\n",kagawa_Cf);
- }else if(sel=='r'){
- pc.printf("tokusima_Cf");
- if(sel=='i')tokusima_Cf+=0.002; //r->iを押すとCfがo.o02ずつあがる
- else if(sel=='m'&&tokusima_Cf>0)tokusima_Cf-=0.002; //r->mを押すとCfがo.o02ずつさがる
- pc.printf("duty=%f\r\n",tokusima_Cf);
- } else if(sel=='f'){
- pc.printf("tokusima_Cb");
- if(sel=='i')tokusima_Cb+=0.002; //f->iを押すとCfがo.o02ずつあがる
- else if(sel=='m'&&tokusima_Cf>0)tokusima_Cb-=0.002; //f->mを押すとCfがo.o02ずつさがる
- pc.printf("duty=%f\r\n",tokusima_Cb);
-
- }else if(sel=='t'){
- pc.printf("kouchi_Cf");
- if(sel=='i')kouchi_Cf+=0.002; //t->iを押すとCfがo.o02ずつあがる
- else if(sel=='m'&&kouchi_Cf>0)kouchi_Cf-=0.002; //t->mを押すとCfがo.o02ずつさがる
- pc.printf("duty=%f\r\n",kouchi_Cf);
- } else if(sel=='g'){
- pc.printf("kouchi_Cb");
- if(sel=='i')kouchi_Cb+=0.002; //g->iを押すとCfがo.o02ずつあがる
- else if(sel=='m'&&kouchi_Cf>0)kouchi_Cb-=0.002; //g->mを押すとCfがo.o02ずつさがる
- pc.printf("duty=%f\r\n",kouchi_Cb);
-
- }else if(sel=='y'){
- pc.printf("ehime_Cf");
- if(sel=='i')ehime_Cf+=0.002; //t->iを押すとCfがo.o02ずつあがる
- else if(sel=='m'&&ehime_Cf>0)ehime_Cf-=0.002; //t->mを押すとCfがo.o02ずつさがる
- pc.printf("duty=%f\r\n",ehime_Cf);
- } else if(sel=='h'){
- pc.printf("ehime_Cb");
- if(sel=='i')ehime_Cb+=0.002; //g->iを押すとCfがo.o02ずつあがる
- else if(sel=='m'&&ehime_Cf>0)ehime_Cb-=0.002; //g->mを押すとCfがo.o02ずつさがる
- pc.printf("duty=%f\r\n",ehime_Cb);
- }else if(sel=='p'){
-
- print=!print; //pを押すと表示を停止/再開する
- }else if(sel=='i'){
- target_kagawa=1*BASIC_SPEED;
- target_tokusima=1*BASIC_SPEED;
- target_kouchi=-1*BASIC_SPEED;
- target_ehime=-1*BASIC_SPEED;
-
- }else if(sel=='m'){
- target_kagawa=1*BASIC_SPEED;
- target_tokusima=1*BASIC_SPEED;
- target_kouchi=-1*BASIC_SPEED;
- target_ehime=-1*BASIC_SPEED;
- }else if(sel=='k'){
- target_kagawa=1*BASIC_SPEED;
- target_tokusima=1*BASIC_SPEED;
- target_kouchi=-1*BASIC_SPEED;
- target_ehime=-1*BASIC_SPEED;
- }else if(sel=='j'){
- target_kagawa=1*BASIC_SPEED;
- target_tokusima=1*BASIC_SPEED;
- target_kouchi=-1*BASIC_SPEED;
- target_ehime=-1*BASIC_SPEED;
- }
-
- }
-
- if(loop_time%1000==0){
-
- if(print) pc.printf("%.2f %.2f %.2f %.2f\r\n",EC_kagawa.getOmega(),EC_tokusima.getOmega(),EC_kouchi.getOmega(),EC_ehime.getOmega());
- } //左からエンコーダのカウント、角速度、正方向出力duty比、負方向出力duty比を表示
- }
-}