towa ngo / Mbed 2 deprecated okmtjidouki_copy

モータ制御を修正

Dependencies:   QEI mbed

Fork of okmtjidouki by towa ngo

main_new.h@17:b3af0199bcb4, 2015-09-20 (annotated)

Committer:
okmt
Date:
Sun Sep 20 03:50:22 2015 +0000
Revision:
17:b3af0199bcb4
Parent:
15:570a53b61a26 
qq

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okmt 8:834e3af75af8 1 #define DELTA_T 0.004 //処理周期4msec
blanchul96 12:9faf875d184c 2 #define KP 0.4 //比例定数
blanchul96 12:9faf875d184c 3 #define KI 0.4f //積分定数
blanchul96 12:9faf875d184c 4 #define KD 0.4 //微分定数
okmt 8:834e3af75af8 5 #define n 1 //最大値最小値設定
okmt 17:b3af0199bcb4 6 #define arm_time 6.5 //アームの時間
blanchul96 12:9faf875d184c 7 #define speed02 0.8f
okmt 8:834e3af75af8 8
blanchul96 12:9faf875d184c 9 mbed::PwmOut mypwm011(PC_6);
blanchul96 12:9faf875d184c 10 PwmOut mypwm012(PB_15); //モーター１
okmt 8:834e3af75af8 11
blanchul96 12:9faf875d184c 12 PwmOut mypwm021(PB_14);
blanchul96 12:9faf875d184c 13 PwmOut mypwm022(PB_13); //モーター２
okmt 8:834e3af75af8 14
blanchul96 12:9faf875d184c 15 PwmOut mypwm030(PC_9); //アームのモータ？
blanchul96 12:9faf875d184c 16 PwmOut mypwm031(PC_8);
okmt 8:834e3af75af8 17 #define ROTATE_PER_REVOLUTIONS 24
blanchul96 13:4e69f1820e1a 18 QEI wheel01( PA_8,PB_10, //A相のピン、B相のピン
okmt 8:834e3af75af8 19 NC, //回転数数えるピン（任意）
okmt 8:834e3af75af8 20 ROTATE_PER_REVOLUTIONS, //1回転につき何パルスの出力をするか？
okmt 8:834e3af75af8 21 QEI::X2_ENCODING ); //モード設定、X4_ENCODING→分解能が細かくなるが精度低い
okmt 8:834e3af75af8 22
blanchul96 12:9faf875d184c 23 QEI wheel02( D3,D2, //A相のピン、B相のピン
okmt 8:834e3af75af8 24 NC, //回転数数えるピン（任意）
okmt 8:834e3af75af8 25 ROTATE_PER_REVOLUTIONS, //1回転につき何パルスの出力をするか？
okmt 8:834e3af75af8 26 QEI::X2_ENCODING ); //モード設定、X4_ENCODING→分解能が細かくなるが精度低い
okmt 8:834e3af75af8 27
blanchul96 15:570a53b61a26 28 //DigitalOut myled(LED1); //特に必要性のないLED定義
okmt 8:834e3af75af8 29 typedef signed long S32;
okmt 8:834e3af75af8 30 typedef float F32;
okmt 8:834e3af75af8 31 static S32 diff[2];
okmt 8:834e3af75af8 32 static F32 integral;
okmt 8:834e3af75af8 33
okmt 8:834e3af75af8 34 F32 pid_sample(int a,int b)
okmt 8:834e3af75af8 35 {
okmt 8:834e3af75af8 36 float turn;
okmt 8:834e3af75af8 37 F32 p,i,d;
okmt 8:834e3af75af8 38 i += 0.0f;
okmt 8:834e3af75af8 39 diff[0] = diff[1];
okmt 8:834e3af75af8 40 diff[1]= a - b;
okmt 8:834e3af75af8 41 integral += (diff[1]+diff[0])/2.0*DELTA_T+ 0.0f;
okmt 8:834e3af75af8 42
okmt 8:834e3af75af8 43 p = KP*diff[1];
okmt 8:834e3af75af8 44 i = KI * integral;
okmt 8:834e3af75af8 45 d = KD *(diff[1]-diff[0])/DELTA_T;
okmt 8:834e3af75af8 46
blanchul96 10:f048a8bc885a 47 turn = speed02 - (p + i + d);
okmt 8:834e3af75af8 48
okmt 8:834e3af75af8 49 if(n<turn)turn = n;
okmt 8:834e3af75af8 50 if(-n>turn)turn = -n; //最大値最小値の設定
okmt 8:834e3af75af8 51
okmt 8:834e3af75af8 52 return turn;
okmt 8:834e3af75af8 53 }
okmt 3:2fb6302e0cda 54 void Motor01(float a)
okmt 3:2fb6302e0cda 55 {
okmt 3:2fb6302e0cda 56 if(a>0&&a<=1)
okmt 3:2fb6302e0cda 57 {mypwm011 = 1;mypwm012 = 1 - a;} //正転
okmt 3:2fb6302e0cda 58
okmt 3:2fb6302e0cda 59 else if(a<=0&&a>=-1)
okmt 3:2fb6302e0cda 60 {mypwm011 = 1 + a;mypwm012 = 1;} //逆転&a=0ならブレーキ
okmt 3:2fb6302e0cda 61
okmt 3:2fb6302e0cda 62 else
okmt 3:2fb6302e0cda 63 {mypwm011 = 0;mypwm012 = 0;} //ストップ
okmt 3:2fb6302e0cda 64 }
okmt 3:2fb6302e0cda 65
okmt 3:2fb6302e0cda 66 void Motor02(float a)
okmt 3:2fb6302e0cda 67 {
okmt 3:2fb6302e0cda 68 if(a>0&&a<=1)
okmt 3:2fb6302e0cda 69 {mypwm021 = 1;mypwm022 = 1 - a;} //正転
okmt 3:2fb6302e0cda 70
okmt 3:2fb6302e0cda 71 else if(a<=0&&a>=-1)
okmt 3:2fb6302e0cda 72 {mypwm021 = 1 + a;mypwm022 = 1;} //逆転&a=0ならブレーキ
okmt 3:2fb6302e0cda 73
okmt 3:2fb6302e0cda 74 else
okmt 3:2fb6302e0cda 75 {mypwm021 = 0;mypwm022 = 0;} //ストップ
okmt 3:2fb6302e0cda 76 }
okmt 3:2fb6302e0cda 77
okmt 3:2fb6302e0cda 78 void Motor03(float a)
okmt 3:2fb6302e0cda 79 {
okmt 3:2fb6302e0cda 80 if(a>0&&a<=1)
okmt 3:2fb6302e0cda 81 {mypwm030 = 1;mypwm031 = 1 - a;} //正転
okmt 3:2fb6302e0cda 82
okmt 3:2fb6302e0cda 83 else if(a<=0&&a>=-1)
okmt 3:2fb6302e0cda 84 {mypwm030 = 1 + a;mypwm031 = 1;} //逆転&a=0ならブレーキ
okmt 3:2fb6302e0cda 85
okmt 3:2fb6302e0cda 86 else
okmt 3:2fb6302e0cda 87 {mypwm030 = 0;mypwm031 = 0;} //ストップ
okmt 17:b3af0199bcb4 88 }