towa ngo / Mbed 2 deprecated okmtjidouki_copy

モータ制御を修正

Dependencies:   QEI mbed

Fork of okmtjidouki by towa ngo

main_new.h@8:834e3af75af8, 2015-09-14 (annotated)

Committer:
okmt
Date:
Mon Sep 14 09:41:37 2015 +0000
Revision:
8:834e3af75af8
Parent:
3:2fb6302e0cda
Child:
10:f048a8bc885a
a

Who changed what in which revision?

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