riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

EC.h@27:72711b6cbe2a, 2017-08-10 (annotated)

Committer:
jack0325suzu
Date:
Thu Aug 10 05:27:10 2017 +0000
Revision:
27:72711b6cbe2a
Parent:
26:45a53e3c81b1
Child:
28:172a316ab863
uoo

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 8:833757a1df66 7 /**increment型encoder用class
jack0325suzu 8:833757a1df66 8
jack0325suzu 8:833757a1df66 9 Z相(1周につき1回立ち上がる)の機能を追加しました!!!!
jack0325suzu 8:833757a1df66 10
jack0325suzu 8:833757a1df66 11 普通に使う分には不必要な機能ですが、回転回数が欲しい場合や、回転が非常に早い物の回転速度が欲しい場合などでA,B相での処理だとマイコンが追いつかない場合などに使ってください
jack0325suzu 8:833757a1df66 12 */
jack0325suzu 0:20fc96400ca3 13
jack0325suzu 0:20fc96400ca3 14 class Ec{
jack0325suzu 12:530f6184830a 15 protected:
jack0325suzu 0:20fc96400ca3 16 int S; //A相B相のパターン（1~4）
jack0325suzu 0:20fc96400ca3 17 bool stateA,stateB; //A・B相の状態
jack0325suzu 0:20fc96400ca3 18 int count; //カウント数 分解能分で一周
jack0325suzu 0:20fc96400ca3 19 int pre_count; //一つ前のカウント
jack0325suzu 0:20fc96400ca3 20 double precount; //4倍精度カウント
jack0325suzu 0:20fc96400ca3 21 int solution; //分解能
jack0325suzu 0:20fc96400ca3 22 double dt; //角速度の計算間隔
jack0325suzu 0:20fc96400ca3 23
jack0325suzu 5:4abba4f54406 24 //z相用
jack0325suzu 5:4abba4f54406 25 bool first;
jack0325suzu 5:4abba4f54406 26 int rev;
jack0325suzu 5:4abba4f54406 27 double now_time,old_time;
jack0325suzu 5:4abba4f54406 28 double RPM,RPM_old;
jack0325suzu 10:216d5a573dc7 29 int RPM_th;
jack0325suzu 0:20fc96400ca3 30
jack0325suzu 0:20fc96400ca3 31 InterruptIn signalA_;
jack0325suzu 0:20fc96400ca3 32 InterruptIn signalB_;
jack0325suzu 5:4abba4f54406 33 InterruptIn signalZ_;
jack0325suzu 5:4abba4f54406 34
jack0325suzu 0:20fc96400ca3 35 void upA();
jack0325suzu 0:20fc96400ca3 36 void downA();
jack0325suzu 0:20fc96400ca3 37 void upB();
jack0325suzu 0:20fc96400ca3 38 void downB();
jack0325suzu 5:4abba4f54406 39 void upZ();
jack0325suzu 5:4abba4f54406 40 void downZ();
jack0325suzu 0:20fc96400ca3 41
jack0325suzu 0:20fc96400ca3 42 public:
jack0325suzu 0:20fc96400ca3 43 double omega; //角速度
jack0325suzu 0:20fc96400ca3 44 /** コンストラクタの定義
jack0325suzu 7:87c135463de7 45 *
jack0325suzu 8:833757a1df66 46 * ***Z相の機能を追加したことで引数が増えました!!!!***
jack0325suzu 8:833757a1df66 47 *
jack0325suzu 0:20fc96400ca3 48 * main関数の前に必ず一度宣言する
jack0325suzu 0:20fc96400ca3 49 *
jack0325suzu 0:20fc96400ca3 50 * 使うエンコーダの数だけ設定する必要がある
jack0325suzu 0:20fc96400ca3 51 *
jack0325suzu 0:20fc96400ca3 52 * @param signalA エンコーダA相のピン名
jack0325suzu 0:20fc96400ca3 53 * @param signalB エンコーダB相のピン名
jack0325suzu 8:833757a1df66 54 * @param signalZ エンコーダZ相のピン名
jack0325suzu 0:20fc96400ca3 55 * @param s エンコーダの分解能(省略可)
jack0325suzu 0:20fc96400ca3 56 * @param t 角速度計算の間隔(省略可)
jack0325suzu 0:20fc96400ca3 57 */
jack0325suzu 8:833757a1df66 58 /** @section CAUTION
jack0325suzu 8:833757a1df66 59 * 今まで以下のように定義していたものは
jack0325suzu 8:833757a1df66 60 * @code
jack0325suzu 8:833757a1df66 61 * #include "mbed.h"
jack0325suzu 8:833757a1df66 62 * #include "EC.h"
jack0325suzu 8:833757a1df66 63 *
jack0325suzu 8:833757a1df66 64 * Ec Ec1(PA_0,PA_1,1024,0.05);
jack0325suzu 8:833757a1df66 65 * @endcode
jack0325suzu 8:833757a1df66 66 * 次のようにZ相の引数の部分に、NCと入れれば今までの様に使える
jack0325suzu 8:833757a1df66 67 * @code
jack0325suzu 8:833757a1df66 68 * #include "mbed.h"
jack0325suzu 8:833757a1df66 69 * #include "EC.h"
jack0325suzu 8:833757a1df66 70 *
jack0325suzu 8:833757a1df66 71 * Ec Ec1(PA_0,PA_1,NC,1024,0.05);
jack0325suzu 8:833757a1df66 72 * @endcode
jack0325suzu 8:833757a1df66 73 */
jack0325suzu 5:4abba4f54406 74 Ec(PinName signalA,PinName signalB,PinName signalZ,int s,double t);
jack0325suzu 0:20fc96400ca3 75
jack0325suzu 0:20fc96400ca3 76 ///countの値を返す関数(int型)
jack0325suzu 0:20fc96400ca3 77 int getCount();
jack0325suzu 0:20fc96400ca3 78 ///omega(角速度 rad/s)の値を計算する関数
jack0325suzu 0:20fc96400ca3 79 /** @section CAUTION
jack0325suzu 0:20fc96400ca3 80 * CalOmega関数は、一定時間ごと（dtごと）に計算される必要があるので、main関数内でタイマー割り込みを設定する必要がある。
jack0325suzu 0:20fc96400ca3 81 * @code
jack0325suzu 0:20fc96400ca3 82 * #include "mbed.h"
jack0325suzu 2:a9216df32be6 83 * #include "EC.h" //ライブラリをインクルード
jack0325suzu 0:20fc96400ca3 84 *
jack0325suzu 8:833757a1df66 85 * Ec Ec1(PA_0,PA_1,NC,1024,0.05); //分解能1024、計算間隔0.05秒に設定、Z相は使わない
jack0325suzu 0:20fc96400ca3 86 * Ticker ticker;
jack0325suzu 0:20fc96400ca3 87 * DigitalIn button(USER_BUTTON);
jack0325suzu 0:20fc96400ca3 88 * Serial pc(USBTX,USBRX);
jack0325suzu 0:20fc96400ca3 89 *
jack0325suzu 0:20fc96400ca3 90 * void calOmega(){
jack0325suzu 0:20fc96400ca3 91 * Ec1.CalOmega();
jack0325suzu 0:20fc96400ca3 92 * }
jack0325suzu 0:20fc96400ca3 93 *
jack0325suzu 0:20fc96400ca3 94 * int main(){
jack0325suzu 0:20fc96400ca3 95 * ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
jack0325suzu 0:20fc96400ca3 96 * while(1){
jack0325suzu 0:20fc96400ca3 97 * pc.printf(" count1=%d ",Ec1.getCount());
jack0325suzu 0:20fc96400ca3 98 * pc.printf(" omega1=%f\r\n ",Ec1.getOmega());
jack0325suzu 25:d73c40fd4b55 99 * if(pc.readable()) {
jack0325suzu 25:d73c40fd4b55 100 * char sel=pc.getc();
jack0325suzu 25:d73c40fd4b55 101 * if(sel=='r') Ec1.reset(); //rを押したらリセット
jack0325suzu 25:d73c40fd4b55 102 * }
jack0325suzu 0:20fc96400ca3 103 * }
jack0325suzu 0:20fc96400ca3 104 * }
jack0325suzu 0:20fc96400ca3 105 * @endcode
jack0325suzu 0:20fc96400ca3 106 */
jack0325suzu 0:20fc96400ca3 107 void CalOmega();
jack0325suzu 0:20fc96400ca3 108 ///omega(角速度 rad/s)の値を返す関数(double型)
jack0325suzu 0:20fc96400ca3 109 double getOmega();
jack0325suzu 0:20fc96400ca3 110 ///四倍精度のcountの値を返す関数(double型)
jack0325suzu 0:20fc96400ca3 111 double getPreCount();
jack0325suzu 0:20fc96400ca3 112 ///エンコーダを初期状態に戻す関数 countやomegaの値を0にする
jack0325suzu 12:530f6184830a 113 virtual void reset();
jack0325suzu 0:20fc96400ca3 114 ///角速度計算の間隔dtを決めることができる（デフォルトは0.05秒）
jack0325suzu 0:20fc96400ca3 115 void setTime(double t);
jack0325suzu 8:833757a1df66 116 ///(Z相を使用する場合)回転回数を返す関数(int型)
jack0325suzu 8:833757a1df66 117 int getRev();
jack0325suzu 8:833757a1df66 118 ///(Z相を使用する場合)回転速度(rpm)の値を返す関数(double型)
jack0325suzu 8:833757a1df66 119 /** @section SAMPLE
jack0325suzu 8:833757a1df66 120 * z相を使う場合のプログラムの例
jack0325suzu 8:833757a1df66 121 * @code
jack0325suzu 8:833757a1df66 122 * #include "mbed.h"
jack0325suzu 8:833757a1df66 123 * #include "EC.h" //ライブラリをインクルード
jack0325suzu 8:833757a1df66 124 *
jack0325suzu 8:833757a1df66 125 * Ec Ec1(NC,NC,PA_0,1024,0.05); //A相B相が不必要な場合も、このようにNCと入れればよい
jack0325suzu 8:833757a1df66 126 * DigitalIn button(USER_BUTTON);
jack0325suzu 8:833757a1df66 127 * Serial pc(USBTX,USBRX);
jack0325suzu 8:833757a1df66 128 *
jack0325suzu 8:833757a1df66 129 * int main(){
jack0325suzu 8:833757a1df66 130 * while(1){
jack0325suzu 8:833757a1df66 131 * pc.printf(" rev1=%d ",Ec1.getRev());
jack0325suzu 8:833757a1df66 132 * pc.printf(" rpm1=%f\r\n ",Ec1.getRPM());
jack0325suzu 8:833757a1df66 133 * if(!button) Ec1.reset(); //USERボタンを押したらリセット
jack0325suzu 8:833757a1df66 134 * }
jack0325suzu 8:833757a1df66 135 * }
jack0325suzu 8:833757a1df66 136 * @endcode
jack0325suzu 8:833757a1df66 137 */
jack0325suzu 5:4abba4f54406 138 double getRPM();
jack0325suzu 10:216d5a573dc7 139
jack0325suzu 10:216d5a573dc7 140 void changeRPM_th(int th);
jack0325suzu 8:833757a1df66 141
jack0325suzu 5:4abba4f54406 142
jack0325suzu 0:20fc96400ca3 143 ///角速度計算の間隔
jack0325suzu 0:20fc96400ca3 144 static double deftime;
jack0325suzu 0:20fc96400ca3 145 ///エンコーダの分解能のデフォルト値
jack0325suzu 0:20fc96400ca3 146 static int defsolution;
jack0325suzu 0:20fc96400ca3 147
jack0325suzu 10:216d5a573dc7 148 Timer timer;
jack0325suzu 0:20fc96400ca3 149
jack0325suzu 0:20fc96400ca3 150 };
jack0325suzu 0:20fc96400ca3 151
jack0325suzu 0:20fc96400ca3 152 ///速度制御用class
jack0325suzu 0:20fc96400ca3 153 ///Ec classを継承している
jack0325suzu 0:20fc96400ca3 154 /** @section Example
jack0325suzu 0:20fc96400ca3 155 * @code
jack0325suzu 0:20fc96400ca3 156 * #include "mbed.h"
jack0325suzu 0:20fc96400ca3 157 * #include "EC.h" //ヘッダファイルをインクルード
jack0325suzu 0:20fc96400ca3 158 *
jack0325suzu 25:d73c40fd4b55 159 * SpeedControl motor1(PA_0,PA_1,NC,1024,0.05,PB_8,PB_9); //エンコーダ分解能1024、角速度計算間隔を0.05秒に設定
jack0325suzu 0:20fc96400ca3 160 * Ticker ticker; //角速度計算用ticker
jack0325suzu 0:20fc96400ca3 161 * DigitalIn button(USER_BUTTON);
jack0325suzu 0:20fc96400ca3 162 * Serial pc(USBTX,USBRX);
jack0325suzu 0:20fc96400ca3 163 *
jack0325suzu 0:20fc96400ca3 164 * void calOmega() //角速度計算関数
jack0325suzu 0:20fc96400ca3 165 * {
jack0325suzu 0:20fc96400ca3 166 * motor1.CalOmega();
jack0325suzu 0:20fc96400ca3 167 * }
jack0325suzu 0:20fc96400ca3 168 *
jack0325suzu 0:20fc96400ca3 169 * int main()
jack0325suzu 0:20fc96400ca3 170 * {
jack0325suzu 0:20fc96400ca3 171 * ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
jack0325suzu 27:72711b6cbe2a 172 * motor1.setPDparam(0.01,0); //PDパラメータを設定
jack0325suzu 0:20fc96400ca3 173 * motor1.setDOconstant(46.3); //duty比と角速度の変換係数を46.3に設定
jack0325suzu 0:20fc96400ca3 174 *
jack0325suzu 0:20fc96400ca3 175 * int kai=0;
jack0325suzu 0:20fc96400ca3 176 *
jack0325suzu 0:20fc96400ca3 177 * while(1) {
jack0325suzu 0:20fc96400ca3 178 * motor1.Sc(-10); //角速度10rad/sで逆回転
jack0325suzu 0:20fc96400ca3 179 * if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
jack0325suzu 0:20fc96400ca3 180 * pc.printf("omega=%f duty=%f \r\n",motor1.getOmega(),motor1.duty);
jack0325suzu 0:20fc96400ca3 181 * kai=0;
jack0325suzu 0:20fc96400ca3 182 * }
jack0325suzu 25:d73c40fd4b55 183 * if(pc.readable()) {
jack0325suzu 25:d73c40fd4b55 184 * char sel=pc.getc();
jack0325suzu 25:d73c40fd4b55 185 * if(sel=='q') break; //rを押したらリセット
jack0325suzu 25:d73c40fd4b55 186 * }
jack0325suzu 0:20fc96400ca3 187 * kai++;
jack0325suzu 0:20fc96400ca3 188 * }
jack0325suzu 0:20fc96400ca3 189 * motor1.stop();
jack0325suzu 0:20fc96400ca3 190 * }
jack0325suzu 0:20fc96400ca3 191 * @endcode
jack0325suzu 0:20fc96400ca3 192 */
jack0325suzu 0:20fc96400ca3 193
jack0325suzu 0:20fc96400ca3 194
jack0325suzu 0:20fc96400ca3 195 class SpeedControl : public Ec{
jack0325suzu 0:20fc96400ca3 196 private:
jack0325suzu 26:45a53e3c81b1 197 double Kv_p,Kv_d;
jack0325suzu 26:45a53e3c81b1 198 double diff,diff_old;
jack0325suzu 0:20fc96400ca3 199 double out_duty,out;
jack0325suzu 10:216d5a573dc7 200 double now_omega,now_RPM;
jack0325suzu 10:216d5a573dc7 201 double now_time_,old_time_;
jack0325suzu 5:4abba4f54406 202 //Serial pc(USBTX,USBRX);
jack0325suzu 0:20fc96400ca3 203 protected:
jack0325suzu 3:65ecbd28545c 204
jack0325suzu 3:65ecbd28545c 205 public:
jack0325suzu 0:20fc96400ca3 206 PwmOut pwm_F_;
jack0325suzu 0:20fc96400ca3 207 PwmOut pwm_B_;
jack0325suzu 3:65ecbd28545c 208
jack0325suzu 0:20fc96400ca3 209 /** constractorの定義
jack0325suzu 8:833757a1df66 210 * @param signalA エンコーダA相のピン名
jack0325suzu 8:833757a1df66 211 * @param signalB エンコーダB相のピン名
jack0325suzu 8:833757a1df66 212 * @param signalZ エンコーダZ相のピン名
jack0325suzu 8:833757a1df66 213 * @param s エンコーダの分解能
jack0325suzu 8:833757a1df66 214 * @param t 角速度計算の間隔
jack0325suzu 0:20fc96400ca3 215 * @param pwm_F motorを正転させるpwmピン名
jack0325suzu 0:20fc96400ca3 216 * @param pwm_B motorを後転させるpwmピン名
jack0325suzu 8:833757a1df66 217 * モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する
jack0325suzu 0:20fc96400ca3 218 */
jack0325suzu 5:4abba4f54406 219 SpeedControl( PinName signalA , PinName signalB , PinName signalZ , int s, double t, PinName pwm_F , PinName pwm_B);
jack0325suzu 8:833757a1df66 220
jack0325suzu 0:20fc96400ca3 221 /** duty比を角速度に変換させるための定数
jack0325suzu 0:20fc96400ca3 222 *
jack0325suzu 0:20fc96400ca3 223 * 使うモーターやギア比や供給電圧によって変化するので一度duty比と角速度の関係を調べてグラフの傾きから求める
jack0325suzu 0:20fc96400ca3 224 *
jack0325suzu 0:20fc96400ca3 225 * 多少ずれてもPIDが何とかしてくれるのでそこまでの精度は必要ない
jack0325suzu 0:20fc96400ca3 226 *
jack0325suzu 0:20fc96400ca3 227 * デフォルトは45 これは380モータとエンコーダを直結して12V電圧で動かしたときの結果
jack0325suzu 0:20fc96400ca3 228 *
jack0325suzu 0:20fc96400ca3 229 *
jack0325suzu 0:20fc96400ca3 230 */
jack0325suzu 0:20fc96400ca3 231 double C;
jack0325suzu 0:20fc96400ca3 232 /** 速度制御関数、引数は目標速度
jack0325suzu 0:20fc96400ca3 233 *
jack0325suzu 0:20fc96400ca3 234 * この目標角加速度になるようにモーターを回転させることができる
jack0325suzu 0:20fc96400ca3 235 *
jack0325suzu 0:20fc96400ca3 236 * 負の数を代入すれば逆回転することができる
jack0325suzu 0:20fc96400ca3 237 *
jack0325suzu 0:20fc96400ca3 238 * 出力できるduty比は最大で0.5までに設定してある
jack0325suzu 0:20fc96400ca3 239 * @param target_omega 目標角速度
jack0325suzu 0:20fc96400ca3 240 *
jack0325suzu 0:20fc96400ca3 241 * @section CAUTION（printfについて）
jack0325suzu 0:20fc96400ca3 242 *
jack0325suzu 0:20fc96400ca3 243 * 上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。
jack0325suzu 0:20fc96400ca3 244 *
jack0325suzu 0:20fc96400ca3 245 * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
jack0325suzu 0:20fc96400ca3 246 *
jack0325suzu 0:20fc96400ca3 247 * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
jack0325suzu 0:20fc96400ca3 248 *
jack0325suzu 0:20fc96400ca3 249 * （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
jack0325suzu 0:20fc96400ca3 250 *
jack0325suzu 0:20fc96400ca3 251 */
jack0325suzu 0:20fc96400ca3 252 void Sc(double target_omega);
jack0325suzu 0:20fc96400ca3 253 /** PIDパラメータ設定関数
jack0325suzu 0:20fc96400ca3 254 *
jack0325suzu 26:45a53e3c81b1 255 * 引数はそれぞれp,dパラメータ
jack0325suzu 0:20fc96400ca3 256 *
jack0325suzu 0:20fc96400ca3 257 * デフォルトは全部0に設定してある
jack0325suzu 0:20fc96400ca3 258 *
jack0325suzu 26:45a53e3c81b1 259 * パラメーター値は実験的に頑張って求めるしかないです
jack0325suzu 0:20fc96400ca3 260 * @param p p制御パラメータ
jack0325suzu 0:20fc96400ca3 261 * @param d d制御パラメータ
jack0325suzu 0:20fc96400ca3 262 */
jack0325suzu 26:45a53e3c81b1 263 void setPDparam(double p,double d);
jack0325suzu 26:45a53e3c81b1 264
jack0325suzu 0:20fc96400ca3 265 /** 角速度・duty比変換定数(C)の設定関数
jack0325suzu 0:20fc96400ca3 266 *
jack0325suzu 0:20fc96400ca3 267 * 文字通りである
jack0325suzu 0:20fc96400ca3 268 * @param c duty比を角速度に変換させるための定数
jack0325suzu 0:20fc96400ca3 269 */
jack0325suzu 0:20fc96400ca3 270 void setDOconstant(double c);
jack0325suzu 0:20fc96400ca3 271
jack0325suzu 0:20fc96400ca3 272 /** モーター停止用関数 */
jack0325suzu 0:20fc96400ca3 273 void stop();
jack0325suzu 0:20fc96400ca3 274
jack0325suzu 0:20fc96400ca3 275 /** モーター正回転用関数
jack0325suzu 0:20fc96400ca3 276 @param duty 回転duty比
jack0325suzu 0:20fc96400ca3 277 */
jack0325suzu 0:20fc96400ca3 278 void turnF(double duty);
jack0325suzu 0:20fc96400ca3 279
jack0325suzu 0:20fc96400ca3 280 /** モーター逆回転用関数
jack0325suzu 0:20fc96400ca3 281 @param duty 回転duty比
jack0325suzu 0:20fc96400ca3 282 */
jack0325suzu 0:20fc96400ca3 283 void turnB(double duty);
jack0325suzu 0:20fc96400ca3 284
jack0325suzu 0:20fc96400ca3 285 /** 出力duty比
jack0325suzu 0:20fc96400ca3 286 */
jack0325suzu 0:20fc96400ca3 287 double duty;
jack0325suzu 5:4abba4f54406 288
jack0325suzu 26:45a53e3c81b1 289 /** Z相を用いた速度制御
jack0325suzu 26:45a53e3c81b1 290 @param target_RPM 目標RPM
jack0325suzu 26:45a53e3c81b1 291 */
jack0325suzu 9:a919aa92e65e 292 void ScZ2(double target_RPM);
jack0325suzu 26:45a53e3c81b1 293
jack0325suzu 26:45a53e3c81b1 294 //RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ
jack0325suzu 26:45a53e3c81b1 295 //void ScZ(double target_RPM);
jack0325suzu 26:45a53e3c81b1 296 //void Accelarate(double target_duty);
jack0325suzu 26:45a53e3c81b1 297
jack0325suzu 12:530f6184830a 298 virtual void reset();
jack0325suzu 0:20fc96400ca3 299 };
jack0325suzu 0:20fc96400ca3 300 #endif