riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

EC.h@5:4abba4f54406, 2016-11-12 (annotated)

Committer:
jack0325suzu
Date:
Sat Nov 12 06:50:56 2016 +0000
Revision:
5:4abba4f54406
Parent:
3:65ecbd28545c
Child:
7:87c135463de7
ttt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jack0325suzu 0:20fc96400ca3 1 #ifndef __INCLUDED_EC_H_
jack0325suzu 0:20fc96400ca3 2 #define __INCLUDED_EC_H_
jack0325suzu 0:20fc96400ca3 3
jack0325suzu 0:20fc96400ca3 4 #ifndef M_pi
jack0325suzu 0:20fc96400ca3 5 #define M_pi 3.141592
jack0325suzu 0:20fc96400ca3 6 #endif
jack0325suzu 0:20fc96400ca3 7 /**increment型encoder用class*/
jack0325suzu 0:20fc96400ca3 8
jack0325suzu 0:20fc96400ca3 9 class Ec{
jack0325suzu 0:20fc96400ca3 10 private:
jack0325suzu 0:20fc96400ca3 11 int S; //A相B相のパターン（1~4）
jack0325suzu 0:20fc96400ca3 12 bool stateA,stateB; //A・B相の状態
jack0325suzu 0:20fc96400ca3 13 int count; //カウント数 分解能分で一周
jack0325suzu 0:20fc96400ca3 14 int pre_count; //一つ前のカウント
jack0325suzu 0:20fc96400ca3 15 double precount; //4倍精度カウント
jack0325suzu 0:20fc96400ca3 16 int solution; //分解能
jack0325suzu 0:20fc96400ca3 17 double dt; //角速度の計算間隔
jack0325suzu 0:20fc96400ca3 18
jack0325suzu 5:4abba4f54406 19 //z相用
jack0325suzu 5:4abba4f54406 20 bool first;
jack0325suzu 5:4abba4f54406 21 int rev;
jack0325suzu 5:4abba4f54406 22 double now_time,old_time;
jack0325suzu 5:4abba4f54406 23 double RPM,RPM_old;
jack0325suzu 0:20fc96400ca3 24
jack0325suzu 0:20fc96400ca3 25 InterruptIn signalA_;
jack0325suzu 0:20fc96400ca3 26 InterruptIn signalB_;
jack0325suzu 5:4abba4f54406 27 InterruptIn signalZ_;
jack0325suzu 5:4abba4f54406 28
jack0325suzu 5:4abba4f54406 29 Timer timer;
jack0325suzu 0:20fc96400ca3 30
jack0325suzu 0:20fc96400ca3 31 void upA();
jack0325suzu 0:20fc96400ca3 32 void downA();
jack0325suzu 0:20fc96400ca3 33 void upB();
jack0325suzu 0:20fc96400ca3 34 void downB();
jack0325suzu 5:4abba4f54406 35 void upZ();
jack0325suzu 5:4abba4f54406 36 void downZ();
jack0325suzu 0:20fc96400ca3 37
jack0325suzu 0:20fc96400ca3 38 public:
jack0325suzu 0:20fc96400ca3 39 double omega; //角速度
jack0325suzu 0:20fc96400ca3 40 /** コンストラクタの定義
jack0325suzu 0:20fc96400ca3 41 *
jack0325suzu 0:20fc96400ca3 42 * main関数の前に必ず一度宣言する
jack0325suzu 0:20fc96400ca3 43 *
jack0325suzu 0:20fc96400ca3 44 * 使うエンコーダの数だけ設定する必要がある
jack0325suzu 0:20fc96400ca3 45 *
jack0325suzu 0:20fc96400ca3 46 * @param signalA エンコーダA相のピン名
jack0325suzu 0:20fc96400ca3 47 * @param signalB エンコーダB相のピン名
jack0325suzu 0:20fc96400ca3 48 * @param s エンコーダの分解能(省略可)
jack0325suzu 0:20fc96400ca3 49 * @param t 角速度計算の間隔(省略可)
jack0325suzu 0:20fc96400ca3 50 */
jack0325suzu 5:4abba4f54406 51 Ec(PinName signalA,PinName signalB,PinName signalZ,int s,double t);
jack0325suzu 5:4abba4f54406 52 Ec(PinName signalZ);
jack0325suzu 0:20fc96400ca3 53
jack0325suzu 0:20fc96400ca3 54 ///countの値を返す関数(int型)
jack0325suzu 0:20fc96400ca3 55 int getCount();
jack0325suzu 0:20fc96400ca3 56 ///omega(角速度 rad/s)の値を計算する関数
jack0325suzu 0:20fc96400ca3 57 /** @section CAUTION
jack0325suzu 0:20fc96400ca3 58 * CalOmega関数は、一定時間ごと（dtごと）に計算される必要があるので、main関数内でタイマー割り込みを設定する必要がある。
jack0325suzu 0:20fc96400ca3 59 * @code
jack0325suzu 0:20fc96400ca3 60 * #include "mbed.h"
jack0325suzu 2:a9216df32be6 61 * #include "EC.h" //ライブラリをインクルード
jack0325suzu 0:20fc96400ca3 62 *
jack0325suzu 0:20fc96400ca3 63 * Ec Ec1(PA_0,PA_1,1024,0.05); //分解能1024、計算間隔0.05秒に設定
jack0325suzu 0:20fc96400ca3 64 * Ticker ticker;
jack0325suzu 0:20fc96400ca3 65 * DigitalIn button(USER_BUTTON);
jack0325suzu 0:20fc96400ca3 66 * Serial pc(USBTX,USBRX);
jack0325suzu 0:20fc96400ca3 67 *
jack0325suzu 0:20fc96400ca3 68 * void calOmega(){
jack0325suzu 0:20fc96400ca3 69 * Ec1.CalOmega();
jack0325suzu 0:20fc96400ca3 70 * }
jack0325suzu 0:20fc96400ca3 71 *
jack0325suzu 0:20fc96400ca3 72 * int main(){
jack0325suzu 0:20fc96400ca3 73 * ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
jack0325suzu 0:20fc96400ca3 74 * while(1){
jack0325suzu 0:20fc96400ca3 75 * pc.printf(" count1=%d ",Ec1.getCount());
jack0325suzu 0:20fc96400ca3 76 * pc.printf(" omega1=%f\r\n ",Ec1.getOmega());
jack0325suzu 1:d10ca555a103 77 * if(!button) Ec1.reset(); //USERボタンを押したらリセット
jack0325suzu 0:20fc96400ca3 78 * }
jack0325suzu 0:20fc96400ca3 79 * }
jack0325suzu 0:20fc96400ca3 80 * @endcode
jack0325suzu 0:20fc96400ca3 81 */
jack0325suzu 0:20fc96400ca3 82 void CalOmega();
jack0325suzu 0:20fc96400ca3 83 ///omega(角速度 rad/s)の値を返す関数(double型)
jack0325suzu 0:20fc96400ca3 84 double getOmega();
jack0325suzu 0:20fc96400ca3 85 ///四倍精度のcountの値を返す関数(double型)
jack0325suzu 0:20fc96400ca3 86 double getPreCount();
jack0325suzu 0:20fc96400ca3 87 ///エンコーダを初期状態に戻す関数 countやomegaの値を0にする
jack0325suzu 0:20fc96400ca3 88 void reset();
jack0325suzu 0:20fc96400ca3 89 ///角速度計算の間隔dtを決めることができる（デフォルトは0.05秒）
jack0325suzu 0:20fc96400ca3 90 void setTime(double t);
jack0325suzu 0:20fc96400ca3 91
jack0325suzu 5:4abba4f54406 92 double getRPM();
jack0325suzu 5:4abba4f54406 93 int getrev();
jack0325suzu 5:4abba4f54406 94
jack0325suzu 0:20fc96400ca3 95 ///角速度計算の間隔
jack0325suzu 0:20fc96400ca3 96 static double deftime;
jack0325suzu 0:20fc96400ca3 97 ///エンコーダの分解能のデフォルト値
jack0325suzu 0:20fc96400ca3 98 static int defsolution;
jack0325suzu 0:20fc96400ca3 99
jack0325suzu 0:20fc96400ca3 100
jack0325suzu 0:20fc96400ca3 101
jack0325suzu 0:20fc96400ca3 102 };
jack0325suzu 0:20fc96400ca3 103
jack0325suzu 0:20fc96400ca3 104 ///速度制御用class
jack0325suzu 0:20fc96400ca3 105 ///Ec classを継承している
jack0325suzu 0:20fc96400ca3 106 /** @section Example
jack0325suzu 0:20fc96400ca3 107 * @code
jack0325suzu 0:20fc96400ca3 108 * #include "mbed.h"
jack0325suzu 0:20fc96400ca3 109 * #include "EC.h" //ヘッダファイルをインクルード
jack0325suzu 0:20fc96400ca3 110 *
jack0325suzu 0:20fc96400ca3 111 * SpeedControl motor1(PA_0,PA_1,1024,0.05,PB_8,PB_9); //エンコーダ分解能1024、角速度計算間隔を0.05秒に設定
jack0325suzu 0:20fc96400ca3 112 * Ticker ticker; //角速度計算用ticker
jack0325suzu 0:20fc96400ca3 113 * DigitalIn button(USER_BUTTON);
jack0325suzu 0:20fc96400ca3 114 * Serial pc(USBTX,USBRX);
jack0325suzu 0:20fc96400ca3 115 *
jack0325suzu 0:20fc96400ca3 116 * void calOmega() //角速度計算関数
jack0325suzu 0:20fc96400ca3 117 * {
jack0325suzu 0:20fc96400ca3 118 * motor1.CalOmega();
jack0325suzu 0:20fc96400ca3 119 * }
jack0325suzu 0:20fc96400ca3 120 *
jack0325suzu 0:20fc96400ca3 121 * int main()
jack0325suzu 0:20fc96400ca3 122 * {
jack0325suzu 0:20fc96400ca3 123 * ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
jack0325suzu 0:20fc96400ca3 124 * motor1.setPIDparam(0.15,0.0,0.15); //PIDパラメータを設定
jack0325suzu 0:20fc96400ca3 125 * motor1.setDOconstant(46.3); //duty比と角速度の変換係数を46.3に設定
jack0325suzu 0:20fc96400ca3 126 *
jack0325suzu 0:20fc96400ca3 127 * int kai=0;
jack0325suzu 0:20fc96400ca3 128 *
jack0325suzu 0:20fc96400ca3 129 * while(1) {
jack0325suzu 0:20fc96400ca3 130 * motor1.Sc(-10); //角速度10rad/sで逆回転
jack0325suzu 0:20fc96400ca3 131 * if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
jack0325suzu 0:20fc96400ca3 132 * pc.printf("omega=%f duty=%f \r\n",motor1.getOmega(),motor1.duty);
jack0325suzu 0:20fc96400ca3 133 * kai=0;
jack0325suzu 0:20fc96400ca3 134 * }
jack0325suzu 0:20fc96400ca3 135 * if(!button) break; //ボタンを押したら停止
jack0325suzu 0:20fc96400ca3 136 * kai++;
jack0325suzu 0:20fc96400ca3 137 * }
jack0325suzu 0:20fc96400ca3 138 * motor1.stop();
jack0325suzu 0:20fc96400ca3 139 * }
jack0325suzu 0:20fc96400ca3 140 * @endcode
jack0325suzu 0:20fc96400ca3 141 */
jack0325suzu 0:20fc96400ca3 142
jack0325suzu 0:20fc96400ca3 143
jack0325suzu 0:20fc96400ca3 144 class SpeedControl : public Ec{
jack0325suzu 0:20fc96400ca3 145 private:
jack0325suzu 0:20fc96400ca3 146 double Kv_p,Kv_d,Kv_i;
jack0325suzu 0:20fc96400ca3 147 double diff,diff_old,integral;
jack0325suzu 0:20fc96400ca3 148 double out_duty,out;
jack0325suzu 5:4abba4f54406 149 //Serial pc(USBTX,USBRX);
jack0325suzu 0:20fc96400ca3 150 protected:
jack0325suzu 3:65ecbd28545c 151
jack0325suzu 3:65ecbd28545c 152 public:
jack0325suzu 0:20fc96400ca3 153 PwmOut pwm_F_;
jack0325suzu 0:20fc96400ca3 154 PwmOut pwm_B_;
jack0325suzu 3:65ecbd28545c 155
jack0325suzu 0:20fc96400ca3 156 /** constractorの定義
jack0325suzu 0:20fc96400ca3 157 * @param signalA encoderA相のピン名
jack0325suzu 0:20fc96400ca3 158 * @param signalB encoderB相のピン名
jack0325suzu 0:20fc96400ca3 159 * @param s encoderの分解能(省略可)
jack0325suzu 0:20fc96400ca3 160 * @param t 角速度計算の間隔(省略可)
jack0325suzu 0:20fc96400ca3 161 * @param pwm_F motorを正転させるpwmピン名
jack0325suzu 0:20fc96400ca3 162 * @param pwm_B motorを後転させるpwmピン名
jack0325suzu 0:20fc96400ca3 163 * motorを正転させるとencoderのcountが正のほうに加算されるようにencoderとmotorを設置する
jack0325suzu 0:20fc96400ca3 164 */
jack0325suzu 5:4abba4f54406 165 SpeedControl( PinName signalA , PinName signalB , PinName signalZ , int s, double t, PinName pwm_F , PinName pwm_B);
jack0325suzu 5:4abba4f54406 166 //SpeedControl( PinName signalZ , PinName pwm_F , PinName pwm_B);
jack0325suzu 0:20fc96400ca3 167 /** duty比を角速度に変換させるための定数
jack0325suzu 0:20fc96400ca3 168 *
jack0325suzu 0:20fc96400ca3 169 * 使うモーターやギア比や供給電圧によって変化するので一度duty比と角速度の関係を調べてグラフの傾きから求める
jack0325suzu 0:20fc96400ca3 170 *
jack0325suzu 0:20fc96400ca3 171 * 多少ずれてもPIDが何とかしてくれるのでそこまでの精度は必要ない
jack0325suzu 0:20fc96400ca3 172 *
jack0325suzu 0:20fc96400ca3 173 * デフォルトは45 これは380モータとエンコーダを直結して12V電圧で動かしたときの結果
jack0325suzu 0:20fc96400ca3 174 *
jack0325suzu 0:20fc96400ca3 175 *
jack0325suzu 0:20fc96400ca3 176 */
jack0325suzu 0:20fc96400ca3 177 double C;
jack0325suzu 0:20fc96400ca3 178 /** 速度制御関数、引数は目標速度
jack0325suzu 0:20fc96400ca3 179 *
jack0325suzu 0:20fc96400ca3 180 * この目標角加速度になるようにモーターを回転させることができる
jack0325suzu 0:20fc96400ca3 181 *
jack0325suzu 0:20fc96400ca3 182 * 負の数を代入すれば逆回転することができる
jack0325suzu 0:20fc96400ca3 183 *
jack0325suzu 0:20fc96400ca3 184 * 出力できるduty比は最大で0.5までに設定してある
jack0325suzu 0:20fc96400ca3 185 * @param target_omega 目標角速度
jack0325suzu 0:20fc96400ca3 186 *
jack0325suzu 0:20fc96400ca3 187 * @section CAUTION（printfについて）
jack0325suzu 0:20fc96400ca3 188 *
jack0325suzu 0:20fc96400ca3 189 * 上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。
jack0325suzu 0:20fc96400ca3 190 *
jack0325suzu 0:20fc96400ca3 191 * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
jack0325suzu 0:20fc96400ca3 192 *
jack0325suzu 0:20fc96400ca3 193 * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
jack0325suzu 0:20fc96400ca3 194 *
jack0325suzu 0:20fc96400ca3 195 * （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
jack0325suzu 0:20fc96400ca3 196 *
jack0325suzu 0:20fc96400ca3 197 */
jack0325suzu 0:20fc96400ca3 198 void Sc(double target_omega);
jack0325suzu 0:20fc96400ca3 199 /** PIDパラメータ設定関数
jack0325suzu 0:20fc96400ca3 200 *
jack0325suzu 0:20fc96400ca3 201 * 引数はそれぞれp,i,dパラメータ
jack0325suzu 0:20fc96400ca3 202 *
jack0325suzu 0:20fc96400ca3 203 * デフォルトは全部0に設定してある
jack0325suzu 0:20fc96400ca3 204 *
jack0325suzu 0:20fc96400ca3 205 * だいたいp=0.15くらいでなんとかなる（知らんけど）
jack0325suzu 0:20fc96400ca3 206 * @param p p制御パラメータ
jack0325suzu 0:20fc96400ca3 207 * @param i i制御パラメータ
jack0325suzu 0:20fc96400ca3 208 * @param d d制御パラメータ
jack0325suzu 0:20fc96400ca3 209 */
jack0325suzu 5:4abba4f54406 210
jack0325suzu 0:20fc96400ca3 211 void setPIDparam(double p,double i,double d);
jack0325suzu 0:20fc96400ca3 212 /** 角速度・duty比変換定数(C)の設定関数
jack0325suzu 0:20fc96400ca3 213 *
jack0325suzu 0:20fc96400ca3 214 * 文字通りである
jack0325suzu 0:20fc96400ca3 215 * @param c duty比を角速度に変換させるための定数
jack0325suzu 0:20fc96400ca3 216 */
jack0325suzu 0:20fc96400ca3 217 void setDOconstant(double c);
jack0325suzu 0:20fc96400ca3 218
jack0325suzu 0:20fc96400ca3 219 /** モーター停止用関数 */
jack0325suzu 0:20fc96400ca3 220 void stop();
jack0325suzu 0:20fc96400ca3 221
jack0325suzu 0:20fc96400ca3 222 /** モーター正回転用関数
jack0325suzu 0:20fc96400ca3 223 @param duty 回転duty比
jack0325suzu 0:20fc96400ca3 224 */
jack0325suzu 0:20fc96400ca3 225 void turnF(double duty);
jack0325suzu 0:20fc96400ca3 226
jack0325suzu 0:20fc96400ca3 227 /** モーター逆回転用関数
jack0325suzu 0:20fc96400ca3 228 @param duty 回転duty比
jack0325suzu 0:20fc96400ca3 229 */
jack0325suzu 0:20fc96400ca3 230 void turnB(double duty);
jack0325suzu 0:20fc96400ca3 231
jack0325suzu 0:20fc96400ca3 232 /** 出力duty比
jack0325suzu 0:20fc96400ca3 233 */
jack0325suzu 0:20fc96400ca3 234 double duty;
jack0325suzu 5:4abba4f54406 235
jack0325suzu 5:4abba4f54406 236 void ScZ(double target_RPM);
jack0325suzu 5:4abba4f54406 237 void Accelarate(double target_duty);
jack0325suzu 0:20fc96400ca3 238 };
jack0325suzu 0:20fc96400ca3 239 #endif