Revision 0:3d9101dd0061, committed 2018-10-31

Comitter:

yuki0701

Date:

Wed Oct 31 09:56:08 2018 +0000

Commit message:

ver.1

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CruizCore_R1370P.lib	Wed Oct 31 09:56:08 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/teams/ROBOSTEP_LIBRARY/code/CruizCore_R1370P/#b034f6d0b378

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EC.cpp	Wed Oct 31 09:56:08 2018 +0000
@@ -0,0 +1,190 @@
+#include "mbed.h"
+#include "EC.h"
+
+int Ec::defsolution;
+double Ec::deftime;
+//ピン変化割り込み関数の定義
+void Ec::upA()
+{
+    stateA=1;
+    if(stateB==0&&S==0) {
+        S=1;
+    } else if(stateB==1&&S==3) {
+        S=2;
+    }
+}
+void Ec::downA()
+{
+    stateA=0;
+    if(stateB==1&&S==2) {
+        S=3;
+    } else if(stateB==0&&S==1) {
+        S=0;
+        count--;
+    }
+}
+void Ec::upB()
+{
+    stateB=1;
+    if(stateA==1&&S==1) {
+        S=2;
+    } else if(stateA==0&&S==0) {
+        S=3;
+    }
+}
+void Ec::downB()
+{
+    stateB=0;
+    if(stateA==0&&S==3) {
+        count++;
+        S=0;
+    } else if(stateA==1&&S==2) {
+        S=1;
+    }
+}
+
+void Ec::upZ()
+{
+    if(first==false) {
+        static int res_count;
+        rev++;
+        first=true;
+        now_time=timer.read();
+        RPM=60/(now_time-old_time);
+        if((RPM_old-RPM)>RPM_th) {
+            if(res_count >= 5) {
+                printf("\r\n CAUTION : speed downed drastically\r\n");
+                NVIC_SystemReset();
+                res_count=0;
+            } else {
+                res_count++;
+            }
+        } else {
+            res_count=0;
+        }
+        RPM_old=RPM;
+        old_time=timer.read();
+    }
+}
+
+void Ec::downZ()
+{
+    first=false;
+}
+
+
+//コンストラクタの定義
+//main関数の前に必ず一度宣言する
+//第一・第二引数はエンコーダのA・B相のピン名
+//第三院数はエンコーダの分解能
+
+Ec::Ec(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) : signalA_(signalA),signalB_(signalB),signalZ_(signalZ)
+{
+
+    if((signalA!=NC)&&(signalB!=NC)) {
+        S=0;
+        stateA=0;
+        stateB=0;
+        count=0;
+        pre_count=0.0;
+        timer.start();
+        signalA_.rise(callback(this,&Ec::upA));
+        signalA_.fall(callback(this,&Ec::downA));
+        signalB_.rise(callback(this,&Ec::upB));
+        signalB_.fall(callback(this,&Ec::downB));
+    }
+    if(signalZ!=NC) {
+        first=false;
+        rev=0;
+        old_time=0;
+        RPM=0;
+        RPM_old=0;
+        signalZ_.rise(callback(this,&Ec::upZ));
+        signalZ_.fall(callback(this,&Ec::downZ));
+    }
+    dt=t;
+    solution=s;
+    defsolution=s;
+    RPM_th=250;
+    count_to_distance_mm_ = 0;
+    gear_rate_ = 1;
+    is_diameter_set = 0;
+}
+
+int Ec::getCount()
+{
+    return count;
+}
+
+void Ec::CalOmega()
+{
+    omega=(count-pre_count)*2*M_pi/(solution*dt);
+    pre_count=count;
+}
+
+double Ec::getOmega()
+{
+    return omega;
+}
+
+double Ec::getPreCount()
+{
+    precount=count+S/4.0;
+    return precount;
+}
+/*reset関数の定義*/
+/*エンコーダを初期状態に戻すことができる*/
+void Ec::reset()
+{
+    S=0;
+    stateA=0;
+    stateB=0;
+    count=0;
+    pre_count=0.0,omega=0;
+    rev=0;
+    now_time=0;
+    old_time=0;
+    RPM=0;
+    RPM_old=0;
+}
+/*setTime関数の定義*/
+/*自分で好きなように角速度計算の間隔を決めることができる（デフォルトは0.05秒）*/
+void Ec::setTime(double t)
+{
+    dt=t;
+}
+
+double Ec::getRPM()
+{
+    return RPM;
+}
+
+int Ec::getRev()
+{
+    return rev;
+}
+
+void Ec::changeRPM_th(int th)
+{
+    RPM_th=th;
+}
+
+double Ec::getDistance_mm()
+{
+    if(is_diameter_set == 0) printf("please set diameter\r\n");
+    distance_mm_= count * count_to_distance_mm_;
+    return distance_mm_;
+}
+void Ec::setDiameter_mm(double diameter_mm)
+{
+    diameter_mm_ = diameter_mm;
+    setCountToDistance_mm();
+    is_diameter_set = 1;
+}
+void Ec::setGearRate(double gear_rate){
+    gear_rate_ = gear_rate;
+    setCountToDistance_mm();
+}
+void Ec::setCountToDistance_mm(){
+    count_to_distance_mm_ = 1 / (double)solution * M_pi * diameter_mm_ * gear_rate_;//カウント分解能＊2Pi=角度（rad)。角度*直径/2  距離を整理した式
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EC.h	Wed Oct 31 09:56:08 2018 +0000
@@ -0,0 +1,179 @@
+#ifndef __INCLUDED_EC_H_
+#define __INCLUDED_EC_H_
+
+#ifndef M_pi
+#define M_pi 3.141592
+#endif
+#include "mbed.h"
+/**@class Ec
+@brief increment型encoder用class.
+Z相(1周につき1回立ち上がる)の機能あり.
+普通に使う分には不必要な機能ですが、回転回数が欲しい場合や、回転が非常に早い物の回転速度が欲しい場合などで
+A,B相での処理だとマイコンが追いつかない場合などに使ってください
+*/
+class Ec
+{
+protected:
+    int S;  //A相B相のパターン（1~4）
+    bool stateA,stateB; //A・B相の状態
+    int count;  //カウント数 分解能分で一周
+    int pre_count;  //一つ前のカウント
+    double precount;    //4倍精度カウント
+    int solution;   //分解能
+    double dt;  //角速度の計算間隔
+    double distance_mm_;//距離[mm]
+    double diameter_mm_;//測定輪半径
+    double count_to_distance_mm_;//カウントを距離に変換する係数
+    double gear_rate_;//ギア比(エンコーダタイムプーリー/測定輪タイムプーリー)
+    int is_diameter_set;//diameterがセットされていたら1
+    //z相用
+    bool first;
+    int rev;
+    double now_time,old_time;
+    double RPM,RPM_old;
+    int RPM_th;
+
+    InterruptIn signalA_;
+    InterruptIn signalB_;
+    InterruptIn signalZ_;
+
+    void upA();
+    void downA();
+    void upB();
+    void downB();
+    void upZ();
+    void downZ();
+    void setCountToDistance_mm();//countからdistanceに変換する係数を計算する。
+public:
+    double omega;   //角速度
+    /** コンストラクタの定義
+     *
+     *  ***Z相の機能を追加したことで引数が増えました!!!!***
+     *
+     *  main関数の前に必ず一度宣言する
+     *
+     *  使うエンコーダの数だけ設定する必要がある
+     *
+     *  @param signalA エンコーダA相のピン名
+     *  @param signalB エンコーダB相のピン名
+     *  @param signalZ エンコーダZ相のピン名
+     *  @param s エンコーダの分解能(省略可)
+     *  @param t 角速度計算の間隔(省略可)
+     */
+    /** @section CAUTION
+     *  今まで以下のように定義していたものは
+     *  @code
+     *  #include "mbed.h"
+     *  #include "EC.h"
+     *
+     *  Ec Ec1(PA_0,PA_1,1024,0.05);
+     *  @endcode
+     *  次のようにZ相の引数の部分に、NCと入れれば今までの様に使える
+     *  @code
+     *  #include "mbed.h"
+     *  #include "EC.h"
+     *
+     *  Ec Ec1(PA_0,PA_1,NC,1024,0.05);
+     *  @endcode
+     */
+    Ec(PinName signalA,PinName signalB,PinName signalZ,int s,double t);
+
+    ///countの値を返す関数(int型)
+    int getCount();
+
+    /** omega(角速度 rad/s)の値を計算する関数
+    * @section CAUTION
+     *  CalOmega関数は、一定時間ごと（dtごと）に計算される必要があるので、main関数内でタイマー割り込みを設定する必要がある。
+     *  @code
+     *  #include "mbed.h"
+     *  #include "EC.h"        //ライブラリをインクルード
+     *
+     *  Ec Ec1(PA_0,PA_1,NC,1024,0.05); //分解能1024、計算間隔0.05秒に設定、Z相は使わない
+     *  Ticker ticker;
+     *  DigitalIn button(USER_BUTTON);
+     *  Serial pc(USBTX,USBRX);
+     *
+     *  void calOmega(){
+     *      Ec1.CalOmega();
+     *  }
+     *
+     *  int main(){
+     *      ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
+     *     while(1){
+     *          pc.printf(" count1=%d ",Ec1.getCount());
+     *          pc.printf(" omega1=%f\r\n ",Ec1.getOmega());
+     *          if(pc.readable()) {
+     *              char sel=pc.getc();
+     *              if(sel=='r') Ec1.reset(); //rを押したらリセット
+     *          }
+     *      }
+     *  }
+     * @endcode
+     */
+    void CalOmega();
+
+    ///omega(角速度 rad/s)の値を返す関数(double型)
+    double getOmega();
+    ///四倍精度のcountの値を返す関数(double型)
+    double getPreCount();
+    ///エンコーダを初期状態に戻す関数 countやomegaの値を0にする
+    virtual void reset();
+    ///角速度計算の間隔dtを決めることができる（デフォルトは0.05秒）
+    void setTime(double t);
+    ///(Z相を使用する場合)回転回数を返す関数(int型)
+    int getRev();
+
+    ///(Z相を使用する場合)回転速度(rpm)の値を返す関数(double型)
+    /** @section SAMPLE
+     *  z相を使う場合のプログラムの例
+     *  @code
+     *  #include "mbed.h"
+     *  #include "EC.h"        //ライブラリをインクルード
+     *
+     *  Ec Ec1(NC,NC,PA_0,1024,0.05); //A相B相が不必要な場合も、このようにNCと入れればよい
+     *  DigitalIn button(USER_BUTTON);
+     *  Serial pc(USBTX,USBRX);
+     *
+     *  int main(){
+     *     while(1){
+     *          pc.printf(" rev1=%d ",Ec1.getRev());
+     *          pc.printf(" rpm1=%f\r\n ",Ec1.getRPM());
+     *          if(!button) Ec1.reset();     //USERボタンを押したらリセット
+     *      }
+     *  }
+     * @endcode
+     */
+    double getRPM();
+    void changeRPM_th(int th);
+
+    /** @caution 先にsetDiameter_mmをしないと正しい値とならない
+    @return 現在距離.
+    @code
+#include <mbed.h>
+#include "EC.h"
+Ec ec(PB_5, PB_4, NC, 500, 0.5);
+int main()
+{
+    ec.setDiameter_mm(48);
+    while(1) {
+        printf("%f\r\n", ec.getDistance_mm());
+        wait(0.1);
+    }
+}
+@endcode
+    */
+    double getDistance_mm();
+
+    /** @param diameter_mm 測定輪半径
+    */
+    void setDiameter_mm(double diameter_mm);
+    /** @param gear比(エンコーダタイムプーリー/測定輪タイムプーリー)
+    */
+    void setGearRate(double gear_rate);
+    static double deftime;//角速度計算の間隔
+    static int defsolution;//エンコーダの分解能のデフォルト値
+    Timer timer;
+};
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PathFollowing1.cpp	Wed Oct 31 09:56:08 2018 +0000
@@ -0,0 +1,85 @@
+#include <PathFollowing1.h>
+#include <mbed.h>
+#include <math.h>
+
+
+namespace{
+    R1370P gyro(PC_6,PC_7);
+    }
+
+//初期座標：A, 目標座標：B、機体位置：C、点Cから直線ABに下ろした垂線の足：H
+void pathfollowing1::XYRmotorout(double plot_x1, double plot_y1, double plot_x2, double plot_y2, double *ad_x_out, double *ad_y_out, double *ad_r_out){ //プログラム使用時、now_x,now_yはグローバル変数として定義する必要あり
+
+    double Vector_P[2] = {(plot_x2 - plot_x1), (plot_y2 - plot_y1)}; //ベクトルAB
+    double A_Vector_P = hypot(Vector_P[0], Vector_P[1]); //ベクトルABの大きさ（hypot(a,b)で√（a^2+b^2）を計算できる <math.h>））
+    double UnitVector_P[2] = {Vector_P[0]/A_Vector_P, Vector_P[1]/A_Vector_P}; //ベクトルABの単位ベクトル
+    double UnitVector_Q[2] = {UnitVector_P[1], -UnitVector_P[0]}; //ベクトルCHの単位ベクトル
+    double Vector_R[2] = {(now_x - plot_x1), (now_y - plot_y1)}; //ベクトルAC
+    double diff = UnitVector_P[0]*Vector_R[1] - UnitVector_P[1]*Vector_R[0]; //機体位置と直線ABの距離（外積を用いて計算）
+
+    double VectorOut_P[2] = {p_out*UnitVector_P[0], p_out*UnitVector_P[1]}; //ベクトルABに平行方向の出力をx軸方向、y軸方向の出力に分解
+
+
+///////////////////＜XYRmotorout関数内＞以下、ベクトルABに垂直な方向の誤差を埋めるPD制御（ベクトルABに垂直方向の出力を求め、x軸方向、y軸方向の出力に分解）//////////////////////
+
+
+    now_timeQ=timer.read();
+    out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定
+    diff_old=diff;
+    
+    if(out_dutyQ>0.1)out_dutyQ=0.1;
+    if(out_dutyQ<-0.1)out_dutyQ=-0.1;
+    
+    old_timeQ=now_timeQ;
+    
+    double VectorOut_Q[2] = {out_dutyQ*UnitVector_Q[0], out_dutyQ*UnitVector_Q[1]}; //ベクトルABに垂直方向の出力をx軸方向、y軸方向の出力に分解
+    
+    
+///////////////////////////////<XYRmotorout関数内＞以下、機体角度と目標角度の誤差を埋めるPD制御（旋回のための出力値を決定）//////////////////////////////////
+    
+    now_angle=gyro.getAngle();
+    now_timeR=timer.read();
+    diffangle=target_angle-now_angle;
+    out_dutyR=Kvr_p*diff+Kvr_d*(diffangle-diffangle_old)/(now_timeR-old_timeR);
+    diffangle_old=diffangle;
+    
+    if(out_dutyR>r_out)out_dutyR=r_out;
+    if(out_dutyR<-r_out)out_dutyR=-r_out;
+    
+    old_timeR=now_timeR;
+
+//////////////////////////<XYRmotorout関数内＞以下、x軸方向、y軸方向、旋回の出力をそれぞれad_x_out,ad_y_out,ad_r_outの指すアドレスに書き込む/////////////////////////////
+
+    *ad_x_out = VectorOut_P[0]+VectorOut_Q[0];
+    *ad_y_out = VectorOut_P[1]+VectorOut_Q[1];
+    *ad_r_out = out_dutyR;
+   
+}
+
+////////////////////////////////////////////////////////////<XYRmotorout関数は以上>////////////////////////////////////////////////////////////////
+
+
+
+    
+void pathfollowing1::set_p_out(double p){ //ベクトルABに平行方向の出力値設定関数
+    p_out = p;
+    }
+    
+void pathfollowing1::q_setPDparam(double q_p,double q_d){ //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
+    Kvq_p=q_p;
+    Kvq_d=q_d;
+}
+   
+void pathfollowing1::r_setPDparam(double r_p,double r_d){ //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
+    Kvr_p=r_p;
+    Kvr_d=r_d;
+}
+ 
+void pathfollowing1::set_r_out(double r){ //旋回時の最大出力値設定関数
+    r_out = r;
+    }
+    
+void pathfollowing1::set_target_angle(double t){ //機体の目標角度設定関数
+    target_angle = t;
+    }
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PathFollowing1.h	Wed Oct 31 09:56:08 2018 +0000
@@ -0,0 +1,50 @@
+#ifndef HARUROBO2019_PATHFOLLOWING_H
+#define HARUROBO2019_PATHFOLLOWING_H
+#include <EC.h>
+#include <R1370P.h>
+
+class pathfollowing1 : public Ec
+{
+private:
+    double p_out,r_out;
+    double Kvq_p,Kvq_d,Kvr_p,Kvr_d;
+    double diff,diff_old,diffangle,diffangle_old;
+    double out_dutyQ,out_dutyR;
+    double now_angle,target_angle;
+    double now_timeQ,old_timeQ,now_timeR,old_timeR;
+    
+protected:
+
+public:
+
+
+
+double now_x, now_y;
+
+void XYRmotorout(double plot_x1, double plot_y1, double plot_x2, double plot_y2, double *ad_x_out, double *ad_y_out, double *ad_r_out);
+//出発地点、目標地点の座標から機体のx軸方向、y軸方向、旋回の出力を算出する関数
+/*
+ *⒈main分内でx_out,y_out,r_outをdouble型で定義
+ *⒉XYRmotorout関数使用時は、num番目のx、y座標、num+1番目のx,y座標に加えx_out,y_out,r_outのアドレス（&x_out,&y_out,&r_out）を渡す→（XYRmotorout(num,&x_out,&y_out,&r_out)）
+*/
+
+
+
+void set_p_out(double p);
+//ベクトルABに平行方向の出力値設定関数
+
+void q_setPDparam(double q_p,double q_d);
+//ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 
+
+void r_setPDparam(double r_p,double r_d);
+//機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
+
+void set_r_out(double r);
+//旋回時の最大出力値設定関数
+
+void set_target_angle(double t);
+//機体目標角度設定関数
+
+
+};
+#endif
\ No newline at end of file