ysak okmt / Mbed 2 deprecated Jidouki

kk

Dependencies:   QEI mbed

Fork of Jidouki by towa ngo

main.cpp@2:dcd12798a336, 2015-09-11 (annotated)

Committer:
blanchul96
Date:
Fri Sep 11 09:38:43 2015 +0000
Revision:
2:dcd12798a336
Parent:
1:65415bf8a01a
Child:
3:2fb6302e0cda
oo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
blanchul96 0:6ea9b36d7630 1 #include "mbed.h"
blanchul96 0:6ea9b36d7630 2 #include "QEI.h"
blanchul96 0:6ea9b36d7630 3 #define DELTA_T 0.004 //処理周期4msec
blanchul96 0:6ea9b36d7630 4 #define KP 0.0 //比例定数
blanchul96 0:6ea9b36d7630 5 #define KI 0.0 //積分定数
blanchul96 0:6ea9b36d7630 6 #define KD 0.0 //微分定数
blanchul96 1:65415bf8a01a 7 #define n 1 //最大値最小値設定
blanchul96 0:6ea9b36d7630 8
blanchul96 0:6ea9b36d7630 9 mbed::PwmOut mypwm011(D11);
blanchul96 0:6ea9b36d7630 10 PwmOut mypwm012(D10); //モーター１
blanchul96 0:6ea9b36d7630 11
blanchul96 0:6ea9b36d7630 12 PwmOut mypwm021(D6);
blanchul96 0:6ea9b36d7630 13 PwmOut mypwm022(D5); //モーター２
blanchul96 0:6ea9b36d7630 14
blanchul96 0:6ea9b36d7630 15 PwmOut mypwm030(D3); //アームのモータ？
blanchul96 0:6ea9b36d7630 16 PwmOut mypwm031(D4);
blanchul96 0:6ea9b36d7630 17
blanchul96 0:6ea9b36d7630 18 #define ROTATE_PER_REVOLUTIONS 24
blanchul96 0:6ea9b36d7630 19 QEI wheel01( D7,D8, //A相のピン、B相のピン
blanchul96 0:6ea9b36d7630 20 NC, //回転数数えるピン（任意）
blanchul96 0:6ea9b36d7630 21 ROTATE_PER_REVOLUTIONS, //1回転につき何パルスの出力をするか？
blanchul96 0:6ea9b36d7630 22 QEI::X2_ENCODING ); //モード設定、X4_ENCODING→分解能が細かくなるが精度低い
blanchul96 0:6ea9b36d7630 23
blanchul96 0:6ea9b36d7630 24 QEI wheel02( D9,D10, //A相のピン、B相のピン
blanchul96 0:6ea9b36d7630 25 NC, //回転数数えるピン（任意）
blanchul96 0:6ea9b36d7630 26 ROTATE_PER_REVOLUTIONS, //1回転につき何パルスの出力をするか？
blanchul96 0:6ea9b36d7630 27 QEI::X2_ENCODING ); //モード設定、X4_ENCODING→分解能が細かくなるが精度低い
blanchul96 0:6ea9b36d7630 28
blanchul96 0:6ea9b36d7630 29 DigitalOut myled(LED1); //特に必要性のないLED定義
blanchul96 0:6ea9b36d7630 30 typedef signed long S32;
blanchul96 0:6ea9b36d7630 31 typedef float F32;
blanchul96 0:6ea9b36d7630 32 static S32 diff[2];
blanchul96 0:6ea9b36d7630 33 static F32 integral;
blanchul96 0:6ea9b36d7630 34
blanchul96 0:6ea9b36d7630 35 F32 pid_sample(int a,int b)
blanchul96 0:6ea9b36d7630 36 {
blanchul96 0:6ea9b36d7630 37 int turn;
blanchul96 0:6ea9b36d7630 38 F32 p,i,d;
blanchul96 0:6ea9b36d7630 39
blanchul96 0:6ea9b36d7630 40 diff[0] = diff[1];
blanchul96 0:6ea9b36d7630 41 diff[1]= a - b;
blanchul96 0:6ea9b36d7630 42 integral += (diff[1]+diff[0])/2.0*DELTA_T;
blanchul96 0:6ea9b36d7630 43
blanchul96 0:6ea9b36d7630 44 p = KP*diff[1];
blanchul96 0:6ea9b36d7630 45 i = KI*integral;
blanchul96 0:6ea9b36d7630 46 d = KD *(diff[1]-diff[0])/DELTA_T;
blanchul96 0:6ea9b36d7630 47
blanchul96 0:6ea9b36d7630 48 turn = p + i + d;
blanchul96 0:6ea9b36d7630 49
blanchul96 0:6ea9b36d7630 50 if(n<turn)turn = n;
blanchul96 1:65415bf8a01a 51 if(-n>turn)turn = -n; //最大値最小値の設定
blanchul96 0:6ea9b36d7630 52
blanchul96 0:6ea9b36d7630 53 return turn;
blanchul96 0:6ea9b36d7630 54 }
blanchul96 0:6ea9b36d7630 55
blanchul96 0:6ea9b36d7630 56 int main() { while(true){
blanchul96 0:6ea9b36d7630 57 int c =0; //cはスイッチによって値うける、ｃの値を３パターン規定し、n,mに加算減算することで使えるものにする（または単純に定義してもいいか）
blanchul96 0:6ea9b36d7630 58 int s = c;
blanchul96 0:6ea9b36d7630 59 int m = c;
blanchul96 0:6ea9b36d7630 60 mypwm011 = 1.0;
blanchul96 0:6ea9b36d7630 61 mypwm012 = 0.0;
blanchul96 0:6ea9b36d7630 62
blanchul96 0:6ea9b36d7630 63 mypwm021 = 1.0;
blanchul96 0:6ea9b36d7630 64 mypwm022 = 0.0;
blanchul96 0:6ea9b36d7630 65
blanchul96 0:6ea9b36d7630 66 /* mypwm.period_ms(10);
blanchul96 0:6ea9b36d7630 67 mypwm.pulsewidth_ms(1); */
blanchul96 0:6ea9b36d7630 68 for(int i = s; i < wheel01.getPulses();)
blanchul96 0:6ea9b36d7630 69 { if(wheel01.getPulses()>wheel02.getPulses())
blanchul96 1:65415bf8a01a 70 {mypwm012 = pid_sample(wheel01.getPulses(),wheel02.getPulses());}
blanchul96 2:dcd12798a336 71 else
blanchul96 2:dcd12798a336 72 {mypwm022 = pid_sample(wheel02.getPulses(),wheel01.getPulses());}
blanchul96 2:dcd12798a336 73
blanchul96 2:dcd12798a336 74 if(mypwm012<=0.1&&mypwm022<=0.1)
blanchul96 2:dcd12798a336 75 {mypwm012=mypwm012-0.1;mypwm022=mypwm022-0.1;}
blanchul96 0:6ea9b36d7630 76 }
blanchul96 0:6ea9b36d7630 77 mypwm030 = 1.0;
blanchul96 0:6ea9b36d7630 78 mypwm031 = 1.0;
blanchul96 0:6ea9b36d7630 79
blanchul96 0:6ea9b36d7630 80 //アーム部の駆動
blanchul96 0:6ea9b36d7630 81
blanchul96 0:6ea9b36d7630 82 wheel01.reset();
blanchul96 0:6ea9b36d7630 83 wheel02.reset(); //パルス値の初期化
blanchul96 0:6ea9b36d7630 84
blanchul96 0:6ea9b36d7630 85 mypwm011 = 0.0;
blanchul96 0:6ea9b36d7630 86 mypwm012 = 1.0; //逆転
blanchul96 0:6ea9b36d7630 87
blanchul96 0:6ea9b36d7630 88 mypwm021 = 0.0;
blanchul96 0:6ea9b36d7630 89 mypwm022 = 1.0; //逆転
blanchul96 0:6ea9b36d7630 90
blanchul96 0:6ea9b36d7630 91
blanchul96 0:6ea9b36d7630 92 for(int i = m; i > wheel01.getPulses();)
blanchul96 0:6ea9b36d7630 93 {if(wheel01.getPulses()>wheel02.getPulses())
blanchul96 2:dcd12798a336 94 {mypwm011 = pid_sample(wheel01.getPulses(),wheel02.getPulses());}
blanchul96 2:dcd12798a336 95 else
blanchul96 2:dcd12798a336 96 {mypwm021 = pid_sample(wheel02.getPulses(),wheel01.getPulses());}
blanchul96 2:dcd12798a336 97
blanchul96 2:dcd12798a336 98 if(mypwm011>=0.1&&mypwm021>=0.1)
blanchul96 2:dcd12798a336 99 {mypwm011=mypwm011-0.1;mypwm022=mypwm022-0.1;}
blanchul96 0:6ea9b36d7630 100 }
blanchul96 0:6ea9b36d7630 101
blanchul96 2:dcd12798a336 102 myled = 1; //パルスがmの値にいくまで
blanchul96 0:6ea9b36d7630 103
blanchul96 0:6ea9b36d7630 104 mypwm011 = 1.0;
blanchul96 0:6ea9b36d7630 105 mypwm012 = 1.0;
blanchul96 0:6ea9b36d7630 106
blanchul96 0:6ea9b36d7630 107 mypwm021 = 1.0;
blanchul96 0:6ea9b36d7630 108 mypwm022 = 1.0;
blanchul96 0:6ea9b36d7630 109
blanchul96 0:6ea9b36d7630 110
blanchul96 0:6ea9b36d7630 111
blanchul96 0:6ea9b36d7630 112 wheel01.reset();
blanchul96 0:6ea9b36d7630 113 wheel02.reset(); //パルス値の初期化
blanchul96 0:6ea9b36d7630 114
blanchul96 0:6ea9b36d7630 115
blanchul96 0:6ea9b36d7630 116 }
blanchul96 0:6ea9b36d7630 117 }