ysak okmt / Mbed 2 deprecated Jidouki

kk

Dependencies:   QEI mbed

Fork of Jidouki by towa ngo

main.cpp@3:2fb6302e0cda, 2015-09-14 (annotated)

Committer:
okmt
Date:
Mon Sep 14 07:58:08 2015 +0000
Revision:
3:2fb6302e0cda
Parent:
2:dcd12798a336 
ouou

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