Yudai Suzuki / Mbed 2 deprecated sekkeiseisaku_week4

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 QEI

main.cpp@0:67b5faeb3720, 2018-11-09 (annotated)

Committer:
uchikimi
Date:
Fri Nov 09 03:56:27 2018 +0000
Revision:
0:67b5faeb3720
Child:
1:2d5ccfddbe36
????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uchikimi 0:67b5faeb3720 1 #include "QEI.h"
uchikimi 0:67b5faeb3720 2 #include "mbed.h"
uchikimi 0:67b5faeb3720 3 #include "RawSerial.h"
uchikimi 0:67b5faeb3720 4 RawSerial pc(P5_0,P5_1); // tx, rx (ポートの指定)
uchikimi 0:67b5faeb3720 5 //RawSerial pc(USBTX,USBRX); // tx, rx(USBシリアルポートの指定)
uchikimi 0:67b5faeb3720 6
uchikimi 0:67b5faeb3720 7
uchikimi 0:67b5faeb3720 8 #define ROTATE_PER_REVOLUTIONS 360 //1回転でのパルス数
uchikimi 0:67b5faeb3720 9 #define JJ 0.006637 //慣性モーメント
uchikimi 0:67b5faeb3720 10 #define KT 0.184088 //モータ定数
uchikimi 0:67b5faeb3720 11 #define KD 0.001 //微分ゲイン
uchikimi 0:67b5faeb3720 12 #define KP 0.1 //比例ゲイン
uchikimi 0:67b5faeb3720 13 #define KI 0 //積分ゲイン
uchikimi 0:67b5faeb3720 14 #define T 1000 //割り込み時間(u_sec)
uchikimi 0:67b5faeb3720 15
uchikimi 0:67b5faeb3720 16 QEI wheel(P1_0, P1_1, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); //チャンネルAとBの初期状態を決める
uchikimi 0:67b5faeb3720 17 PwmOut Servo1(D7); //Pwm出力Servo1の初期化
uchikimi 0:67b5faeb3720 18 PwmOut Servo2(D6); //Pwm出力Servo2の初期化
uchikimi 0:67b5faeb3720 19 //BusOut input(D6,D5);
uchikimi 0:67b5faeb3720 20
uchikimi 0:67b5faeb3720 21 Ticker msec; //繰り返しタイマ割り込みmsecを定義
uchikimi 0:67b5faeb3720 22
uchikimi 0:67b5faeb3720 23 double r=0; //目標値
uchikimi 0:67b5faeb3720 24 double y,e,old_e,u; //現在値，エラー値，1つ前のエラー値，
uchikimi 0:67b5faeb3720 25 double t=0.001; //割り込み周期
uchikimi 0:67b5faeb3720 26
uchikimi 0:67b5faeb3720 27
uchikimi 0:67b5faeb3720 28 void pid() //PID制御部
uchikimi 0:67b5faeb3720 29 {
uchikimi 0:67b5faeb3720 30
uchikimi 0:67b5faeb3720 31 y=(double)wheel.getPulses()*360/(ROTATE_PER_REVOLUTIONS); //エンコーダの値(角度)の取得
uchikimi 0:67b5faeb3720 32 pc.printf("(%-10f)¥0",y); //エンコーダ値(角度)の送信(serial)
uchikimi 0:67b5faeb3720 33
uchikimi 0:67b5faeb3720 34 e=r-y; //誤差値の計算
uchikimi 0:67b5faeb3720 35 //u=KP*e; //P制御*******ここ書かせる*******
uchikimi 0:67b5faeb3720 36 //u=KP*e+KD*(e-old_e)/t; //PD制御*******ここ書かせる*******
uchikimi 0:67b5faeb3720 37 u=KP*e+KD*(e-old_e)/t+KI*(e+old_e)*t; //PID計算(入力値の計算)*******ここ書かせる*******
uchikimi 0:67b5faeb3720 38 old_e=e; //誤差値の保存
uchikimi 0:67b5faeb3720 39
uchikimi 0:67b5faeb3720 40 /***PWM値と回転方向の決定***/
uchikimi 0:67b5faeb3720 41 if(u<0) {
uchikimi 0:67b5faeb3720 42 Servo1=-u;
uchikimi 0:67b5faeb3720 43 Servo2=0;
uchikimi 0:67b5faeb3720 44 //input=0x02;
uchikimi 0:67b5faeb3720 45 } else {
uchikimi 0:67b5faeb3720 46 Servo1=0;
uchikimi 0:67b5faeb3720 47 Servo2=u;
uchikimi 0:67b5faeb3720 48 //input=0x01;
uchikimi 0:67b5faeb3720 49 }
uchikimi 0:67b5faeb3720 50
uchikimi 0:67b5faeb3720 51 }
uchikimi 0:67b5faeb3720 52
uchikimi 0:67b5faeb3720 53 int main()
uchikimi 0:67b5faeb3720 54 {
uchikimi 0:67b5faeb3720 55 //pc.baud(115200); //ボーレートの指定(通信速度)
uchikimi 0:67b5faeb3720 56 pc.baud(921600); //ボーレートの指定
uchikimi 0:67b5faeb3720 57 Servo1.period(0.00001); //PWMの周期の指定
uchikimi 0:67b5faeb3720 58 Servo2.period(0.00001); //PWMの周期の指定
uchikimi 0:67b5faeb3720 59 msec.attach_us(&pid,T); //タイマ割り込みの指定
uchikimi 0:67b5faeb3720 60
uchikimi 0:67b5faeb3720 61 while(1) {
uchikimi 0:67b5faeb3720 62
uchikimi 0:67b5faeb3720 63 //pc.printf("(%-10f)¥0",y); //エンコーダ値(角度)の送信(serial)
uchikimi 0:67b5faeb3720 64
uchikimi 0:67b5faeb3720 65
uchikimi 0:67b5faeb3720 66
uchikimi 0:67b5faeb3720 67 /********************************************************************/
uchikimi 0:67b5faeb3720 68 /************************シリアル通信受信部*****************************/
uchikimi 0:67b5faeb3720 69 /********************************************************************/
uchikimi 0:67b5faeb3720 70 char cmd,check_cmd;
uchikimi 0:67b5faeb3720 71 char code[13];
uchikimi 0:67b5faeb3720 72 int i;
uchikimi 0:67b5faeb3720 73
uchikimi 0:67b5faeb3720 74
uchikimi 0:67b5faeb3720 75 i =0;
uchikimi 0:67b5faeb3720 76
uchikimi 0:67b5faeb3720 77 while(1) {
uchikimi 0:67b5faeb3720 78 cmd =pc.getc();
uchikimi 0:67b5faeb3720 79 check_cmd ='m';
uchikimi 0:67b5faeb3720 80 //pc.printf("%c",cmd);
uchikimi 0:67b5faeb3720 81 if(cmd=='(') {
uchikimi 0:67b5faeb3720 82 i =0;
uchikimi 0:67b5faeb3720 83 while(1) {
uchikimi 0:67b5faeb3720 84 cmd =pc.getc();
uchikimi 0:67b5faeb3720 85 if(check_cmd!=cmd) {
uchikimi 0:67b5faeb3720 86 pc.printf("%c",cmd);
uchikimi 0:67b5faeb3720 87 code[i] =cmd;
uchikimi 0:67b5faeb3720 88 check_cmd ='m';
uchikimi 0:67b5faeb3720 89 i++;
uchikimi 0:67b5faeb3720 90 }
uchikimi 0:67b5faeb3720 91 if(cmd==')') {
uchikimi 0:67b5faeb3720 92 i=12;
uchikimi 0:67b5faeb3720 93 break;
uchikimi 0:67b5faeb3720 94
uchikimi 0:67b5faeb3720 95 }
uchikimi 0:67b5faeb3720 96 }
uchikimi 0:67b5faeb3720 97 }
uchikimi 0:67b5faeb3720 98 if(i>10) {
uchikimi 0:67b5faeb3720 99 break;
uchikimi 0:67b5faeb3720 100 }
uchikimi 0:67b5faeb3720 101 }
uchikimi 0:67b5faeb3720 102 // pc.printf("¥n%s¥n",code);
uchikimi 0:67b5faeb3720 103 r =atof(code);
uchikimi 0:67b5faeb3720 104 i=0;
uchikimi 0:67b5faeb3720 105 while(1) {
uchikimi 0:67b5faeb3720 106 i++;
uchikimi 0:67b5faeb3720 107 if(code[i]==')') {
uchikimi 0:67b5faeb3720 108 break;
uchikimi 0:67b5faeb3720 109 }
uchikimi 0:67b5faeb3720 110 }
uchikimi 0:67b5faeb3720 111 /********************************************************************/
uchikimi 0:67b5faeb3720 112 /*********************シリアル通信受信部ここまで*************************/
uchikimi 0:67b5faeb3720 113 /********************************************************************/
uchikimi 0:67b5faeb3720 114
uchikimi 0:67b5faeb3720 115 }
uchikimi 0:67b5faeb3720 116 }
uchikimi 0:67b5faeb3720 117
uchikimi 0:67b5faeb3720 118