File content as of revision 10:21ea2a241025:

/* 
外乱推定→走行面の検出を行う
やること
・100Hzで離散化したプラントの逆モデルの実装
・推定外乱から磁気吸着力変化の検出
リビジョン
mbed 127:25aea2a3f4e3
*/

#include "mbed.h"
#include "USBSerial.h"
#include "math.h"

// Board settings
#define LED1 P0_29
#define LED2 P0_28
#define LED3 P0_27
#define LED4 P0_26

// CANopen settings
#define RxPDO1 0x220
#define RxPDO2 0x320
#define RxPDO3 0x420
#define Halt 1
#define QuickStop 2
#define ShutDown 3
#define SYNC_Enable 0
#define SYNC_Disable 1
#define node 2

// Disturbance Observer settings
#define tfA 193.5
#define tfB 0.7529
#define tfC 1
#define tfD 0.5219

// 定数
#define Kv 0.092421
#define Km  0.0092
#define samplimgrate 0.01
#define startdetect 20.0
#define enddetect 200.0
#define Kn  116

USBSerial pc;           //PCとのシリアル通信
char Serialdata;
CANMessage canmsgTx;    //送信データ処理用
CANMessage canmsgRx;    //受信データ処理用
Ticker flipper;         //汎用タイマー

DigitalOut ENsig(P0_10);
BusOut myled(LED1, LED2, LED3,LED4);
CAN canPort(P0_13, P0_18);  //CAN name(PinName rd, PinName td)

//グローバル変数
//char Serialdata;    //シリアル受信データ
float Current_demand=0, Velocity_actual=0;  //電流入力値，速度出力値
float Torque_estimate_tf=0;                    //トルク入力推定値
float Distterbance_estimate_tf=0;              //外乱トルク推定値
float Velocity_estimate_z1;                 //推定時保持データ
float Velocity_estimate=0;                  //推定時現在速度
float Current_estimate=0;                   //推定時現在電流
int Velocity_Trend=0, Current_Trend=0;      //除去するトレンド
float distance = 0;                         //走行距離[mm]
float Velocity_meter;                       // 走行速度[mm/s]
bool run_eternal = false;
int check = 0;
bool decimal = false;

// 判別用
float Distterbance_estimate_tf_abs = 0;    // 外乱推定値nの絶対値
float Dist_error = 0;                      // 低下割合
float baseDist = 0;                        // 判別基準値
int cnt_ditect = 0;                        // しきい値を超えてからのカウント
int cnt_find = 0;                          // 非磁性面の検出からのカウント
int findnonmagnet =0;                      // 非磁性面検出信号
int cnt_run = 0;                           // 走行時間カウント

//プロトタイプ宣言
// CANopen関連
void NMTPreOpn(void);       //NMT PreOperationalへ移行
void NMTOpn(void);          //NMT Operationalへ移行
void CtrlWord(int);         //RxPDO1
void ModesOfOperation(void);//RxPDO2
void TgtVelCtrl(int);       //RxPDO3
void sendCtrlRS(int);       //SDO Reset
void sendCtrlSD(int);       //SDO Shutdown
void sendCtrlEN(int);       //SDO Enable
//-------------------その他--------------------
void init(void);            //マイコン初期化
void CANinit(void);         //CANノード初期化
void SerialRX(void);        //Serial受信処理
void flip(void);            //タイマー割り込み
void printPlantData(void);  //速度指令値、出力値をPCに表示
void EstimateDisterbance(void);//指令値を推定
void detectNonmagnet(void);

int main(){
    init();
    myled = 0b0101;
    while(1){
        if(Serialdata == 's'){
            run_eternal = false;
            break;
        }else if(Serialdata == 'e'){
            run_eternal = true;
            break;
        }
        myled=~myled;
        wait(1);
    }
    Serialdata = 0;
    //node初期化
    CANinit();
    int count=0;
    myled=0;
    while(1){
        if(!ENsig){
            myled=count;
            count++;
        }
        else{
            myled =~ myled;
        }
        wait(0.5);
    }
}
//マイコン初期化
void init(void){
    //I/O設定
    ENsig=SYNC_Disable;
    //CAN設定
    canPort.frequency(1000000); //Bit Rate:1Mbps
    //割り込み設定
    flipper.attach_us(&flip,5000);  //汎用タイマー割り込み周期:5ms(5*10^3 us)
    pc.attach(SerialRX);        //Serial受信割り込み開始
}
//can node初期化
void CANinit(void){
    //SDOコマンドで各nodeをリセット
    for(int nodeID=1;nodeID <= node;nodeID++){
        sendCtrlRS(nodeID);
        sendCtrlSD(nodeID);
        sendCtrlEN(nodeID);
    }
    //NMTコマンドで全nodeをオペレーショナルに
    NMTPreOpn();
    NMTOpn();
}
//Serial受信割り込み処理
void SerialRX(void){
    Serialdata = pc.getc();
    //-------------送信コマンドを選択--------------
    if (decimal) {
        if(Serialdata == '1'){
            Velocity_Trend = 100;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '2'){
            Velocity_Trend = 200;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '3'){
            Velocity_Trend = 300;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '4'){
            Velocity_Trend = 400;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '5'){
            Velocity_Trend = 500;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '6'){
            Velocity_Trend = 600;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '7'){
            Velocity_Trend = 700;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '8'){
            Velocity_Trend = 800;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '9'){
            Velocity_Trend = 900;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        decimal = false;
    } else {
        if(Serialdata == '1'){
            Velocity_Trend = 1000;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '2'){
            Velocity_Trend = 2000;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '3'){
            Velocity_Trend = 3000;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
        else if(Serialdata == '4'){
            Velocity_Trend = 4000;
            TgtVelCtrl(Velocity_Trend);
            ENsig = SYNC_Enable;
            Serialdata = 0;
        }
    }
    
    if(Serialdata == 'q'){
        //Haltコマンド送信
        ENsig = SYNC_Disable;
        CtrlWord(QuickStop);
        Serialdata = 0;
    }
    else if(Serialdata == '0'){
        decimal = true;
        ENsig = SYNC_Disable;
        Serialdata = 0;
    }
    else if(Serialdata == 'h'){
        //Haltコマンド送信
        ENsig = SYNC_Disable;
        CtrlWord(Halt);
        Serialdata = 0;
    }
    else if(Serialdata == 'm'){
        ENsig = SYNC_Disable;
        ModesOfOperation();
        Serialdata = 0;
    }

}
//汎用タイマー
void flip(void){
    if(canPort.read(canmsgRx)){
        cnt_run++;
        if(canmsgRx.id == 0x4a0) printPlantData();  // 左モータの情報を表示
    }
    // 走行4分後に停止
    if (cnt_run >= 24000 && run_eternal == false) {
        //Haltコマンド送信
        ENsig = SYNC_Disable;
        CtrlWord(QuickStop);
    }
    
    
    
}
//速度出力値、指令値の表示
void printPlantData(void){
    short Current_raw, Velocity_raw;
    
    Current_raw = canmsgRx.data[1]*0x100+canmsgRx.data[0];   //指令値の取得(電流)
    if (Current_raw > 10000) Current_demand = ((Current_raw - 1) ^ 0xffff) * -1;
    else Current_demand = Current_raw;
    
    //Velocity_actual = canmsgRx.data[5]*0x100+canmsgRx.data[4];  //出力値の取得(速度)
    Velocity_raw = canmsgRx.data[5]*0x100+canmsgRx.data[4];
    //if (Velocity_raw > 10000) Velocity_actual = ((Velocity_raw -1) ^ 0xffff) * -1;
    //else Velocity_actual = Velocity_raw;
    Velocity_actual = Velocity_raw;
    
    EstimateDisterbance();  //外乱の推定
    detectNonmagnet();      //非磁性面の判別
    pc.printf("%f,%f,%f,%f,%f,%f,%d\r\n",Current_demand,Velocity_actual,Torque_estimate_tf,Distterbance_estimate_tf,baseDist,Dist_error,findnonmagnet);
    
    // ロボット停止処理
    if ( findnonmagnet == 1 && run_eternal == false) {
        /*cnt_find++;
        if (cnt_find >= 200) {
            //Haltコマンド送信
            ENsig = SYNC_Disable;
            CtrlWord(QuickStop);
        }*/
        ENsig = SYNC_Disable;
        CtrlWord(QuickStop);
    }
    findnonmagnet = 0;
}

void EstimateDisterbance(void){
    //出力値のトレンド除去
    Velocity_estimate = (Velocity_actual-Velocity_Trend)*Kv;
    //電流値トレンドの計算
    Current_Trend = int((Current_Trend + Current_demand)/2);
    Current_estimate = (Current_demand-Current_Trend)*Km;
    /*
    伝達関数
              (tfAz - tfB)
        tf = ------------
             (tfCz - tfD)
    */
    Torque_estimate_tf = (tfA*Velocity_estimate - tfB*Velocity_estimate_z1 + tfD*Torque_estimate_tf)/tfC;
    Velocity_estimate_z1 = Velocity_estimate;
    Distterbance_estimate_tf = Torque_estimate_tf - Current_estimate;
}

void detectNonmagnet(void) {
    Velocity_meter = Velocity_actual * Kv;    //回転数から速度に変換[m/s]
    distance += Velocity_meter * samplimgrate;  //走行距離を計算
    Distterbance_estimate_tf_abs = fabsf(Distterbance_estimate_tf);
    if (distance <= enddetect && distance >= startdetect) {
        check = 0;
        //外乱推定値の最大値を確認
        if(Distterbance_estimate_tf_abs > baseDist) baseDist = Distterbance_estimate_tf_abs;
    } else if (distance > enddetect) {
        // 低下割合算出
        Dist_error = Distterbance_estimate_tf_abs / baseDist;
        
        // しきい値判断
        if (Dist_error < 0.5) {
            check = 1;
            cnt_ditect++;
            // 低下時間判断
            if(cnt_ditect >= 50) {
                findnonmagnet = 1;
            }
        } else {
            cnt_ditect = 0;
        }
    }
}
//NMT
void NMTPreOpn(void){
    //COB-ID:0 0x01-00-//-//-//-//-//-//
    canmsgTx.id = 0x0;
    canmsgTx.len = 2;
    canmsgTx.data[0] = 0x80;//0x01:enter NMT state "PreOperational"
    canmsgTx.data[1] = 0x00;//send All nodes
    canPort.write(canmsgTx);
    wait(0.2);
}
void NMTOpn(void){
    //COB-ID:0 0x01-00-//-//-//-//-//-//
    canmsgTx.id = 0x0;
    canmsgTx.len = 2;
    canmsgTx.data[0] = 0x01;//0x01:enter NMT state "Operational"
    canmsgTx.data[1] = 0x00;//send All nodes
    canPort.write(canmsgTx);
    wait(0.2);
}
//PDO
void CtrlWord(int type){
    canmsgTx.id = RxPDO1;
    canmsgTx.len = 2;       //Data Length
    if(type==Halt){
        canmsgTx.data[0] = 0x0F;//data:0x01"0F"
        canmsgTx.data[1] = 0x01;//data:0x"01"0F
    }
    else if(type==QuickStop){
        canmsgTx.data[0] = 0x0B;//data:0x00"0B"
        canmsgTx.data[1] = 0x00;//data:0x"00"0B
    }
    else if(type==ShutDown){
        canmsgTx.data[0] = 0x06;//data:0x00"06"
        canmsgTx.data[1] = 0x00;//data:0x"00"06
    }
    canPort.write(canmsgTx);//CANでデータ送信
}
void ModesOfOperation(void){
    canmsgTx.id = RxPDO2;
    canmsgTx.len = 1;       //Data Length
    canmsgTx.data[0] = 0x03;//data:0x03 = "Profile Velocity Mode"
    canPort.write(canmsgTx);//CANでデータ送信
}
void TgtVelCtrl(int rpm){
    //pc.printf("%drpm|0x%08x\r\n",rpm,rpm);
    canmsgTx.id = RxPDO3;
    canmsgTx.len = 6;       //Data Length
    //Target Velocity
    if (rpm > 0) {
        for(char cnt=0;cnt<4;cnt++){
            canmsgTx.data[cnt] = rpm % 256;
            rpm = rpm / 256;
        }
    } else {
        canmsgTx.data[0] = 0x17;
        canmsgTx.data[1] = 0xFC;
        canmsgTx.data[2] = 0xFF;
        canmsgTx.data[3] = 0xFF;
        /*
        rpm = (rpm ^ 0xffffffff) + 1;
        //printf("%x",rpm);
        for(char cnt=0;cnt<4;cnt++){
            canmsgTx.data[cnt] = rpm & 0x000000FF;
            //printf("%x",rpm);
            rpm = rpm >> 2;
        }
        */
    }
    //CtrlWord Enable
    canmsgTx.data[4] = 0x0F;//data:0x00"0F"
    canmsgTx.data[5] = 0x00;//data:0x"00"0F
    canPort.write(canmsgTx);//CANでデータ送信
}
//SDO
void sendCtrlRS(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x40;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Reset)"
    canmsgTx.data[5] = 0x00;//data:0x"00"80
    canPort.write(canmsgTx);//CANでデータ送信
    wait(0.2);
}
void sendCtrlSD(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x40;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)"
    canmsgTx.data[5] = 0x00;//data:0x"00"06
    canPort.write(canmsgTx);//CANでデータ送信
    wait(0.2);
}
void sendCtrlEN(int nodeID){
    canmsgTx.id = 0x600+nodeID;
    canmsgTx.len = 6;       //Data Length
    canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
    canmsgTx.data[1] = 0x40;//Index LowByte
    canmsgTx.data[2] = 0x60;//Index HighByte
    canmsgTx.data[3] = 0x00;//sub-Index
    canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)"
    canmsgTx.data[5] = 0x00;//data:0x"00"0F
    canPort.write(canmsgTx);//CANでデータ送信
    wait(0.2);
}