GPC =
4.4 まとめと考察
発表論文
[1] Shigeki Goka, Seiji Hashimoto, Kiyoshi Ohishi, Takeo Ishikawa, Shuichi Adachi, Koji Kosaka,Hiroshi Kubota and Tadahiro Ohmi : Variable Forgetting Factor-Based Friction Compensation Method of Precision Stage, SICE-ICASE International Conference 2006, Busan, Korea (2006.10)
[2] 五箇・橋本・大石・小坂・石川：「可変忘却要素を用いた適応同定法による超精密ステ ージの摩擦補償法」, 電気学会･産業応用部門大会, No.2, 2-35, (2007)
参照文献
[1] 樋口 龍雄 : 自動制御理論, 森北出版株式会社 (1989)
[2] International Technology Roadmap for Semiconductors:Lithography (2004)
[3] S. Hashimoto, Y. Okada, H. Okuda, S. Adachi, S. Niwa, M. Kajitani: ”Modeling and Deterioration Diagnosis of Catalyst on Vehicle by Variable Forgetting Factor- Based On-Line Identification Method,” Proc. of the 14th IFAC Symposium on System Identification (SYSID-2006), pp. 1364-1369, 2006
[4] Y. Egashira, et al.:“Sub-Nanometer Resolution Ultrasonic Motor for 300 mm Wafer Lithography Precision Stage”, Jpn. J. Appl. Phys.,Vol.41, No.9, pp.5858-5863 2002-9
[5] 橋本・大石・小坂・石川・久保田・大見：「スティックスリップ補償と連続軌跡追 従制 御 に 基 づ く 超 音 波 ア ク チ ュ エ ー タ 駆 動 精 密 ス テ ー ジ の 一 制 御 法 」, 電 気 学 会 論,Vol.
125-D, No.6, pp.582-589, 2005
[6] B. Armstrong-Helouvry, Control of Machines with Friction, Kluwer Academic Publishers 1991
[7] 足立修一：MATLABによる制御のためのシステム同定，東京電機大学出版局(1996) [8] L. Ljung, System Identification – Theory for the user (2nd edition), Englewood Cliffs, Nj:
Prentice Hall PTR 1999
[9] 岩崎誠・柴田和宏・松井信行：「外乱オブザーバによる非線形性摩擦補償とテーブル駆 動系への適用」, 電学論 D, 118,1,pp.51-57 (1998-1)
[10] 大嶋, 小河 : モデル予測制御－Ⅰ－基礎編 : 発展の歴史と現状, システム/制御/情報, Vol.46, No.5, pp.286-293 (2002)
[11] 加納, 大嶋 : モデル予測制御－Ⅱ－線形モデル予測制御, システム/制御/情報, Vol.46, No.7, pp.418-424 (2002)
[12] 増 田, 山 本, 大 嶋 : モ デ ル 予 測制 御 － Ⅲ－ 一般 化 予 測 制御 (GPC) とそ の 周 辺, シス テム/制御/情報, Vol.46, No.9, pp.578-584 (2002)
[13] 江 上, 土 谷, 愛 田, 北 森 : 予 見 サ ーボ 系 の 設計 と 一 般 化予 測 制 御 (GPC) シ ス テム に ついて, 計測自動制御学会論文集, Vol.28, No.8, 956 / 963 (1992)
[14] 増田, 矢納, 井上, 平嶋 : 多項式代数法と等価な状態空間法による一般化予測制御系
の構成とその等価性の証明, 計測自動制御学会論文集, Vol.35, No.2, 221 / 230 (1999) [15] 小郷, 美多 : システム制御理論入門, 実教出版株式会社 (1979)
[16] 岡田 康志 : 排出ガス浄化用触媒特性を考慮したエンジン制御システムの設計に関す
る基礎研究, 平成16年度宇都宮大学修士学位論文 (2005)
[17] 足立 修一 : MATLABによる制御工学, 東京電機大学出版局 (1999)
[18] 奥田 裕之 : 一般化 予測制御による触媒特性を考慮したエンジン制御系の設計法, 平
成16年度宇都宮大学卒業論文 (2005)
[19] H.S.Gandhi, A.G.Piken, M.Shelef and R.G.Delosh : Laboratory Evaluation of Three-way Catalysts, SAE Paper 760201
[20] E.F.Camacho and C.bordons : Model Predictive Control in the Process Industry, Springer-Verlag (1996)
[21] Jan M. Maciejowski 著, 足立, 管野 訳 : モデル予測制御－制約のもとでの最適制御
‐, 東京電機大学出版局 (2005)
[22] 野波, 西村, 平田 : MATLABによる制御系設計, 東京電機大学出版局 (1998)
[23] T.R.FORTESCUE, L.S.KERSHENBAUM and B.E.YDSTIE : Implementation of Self-tuning Regulators with Variable Forgetting Factor, Automatica, Vol.17, No.6, pp.831-835, 1981
[24] 五箇 成輝 : ロバスト性を考慮したモデ ル予測制御によるエンジ ン制御系設計法, 平
成17年度卒業論文（2006）
[25] 安部 広則 : モデル予測制御の精密ステージ駆動システムへの応用, 平成18年度群馬
大学卒業論文 （2007）
謝辞
最 後に, 本 論文 を まとめ る にあ た り, 多 く の方に お 世話 に なっ た こと をこ の 場を 借 りて 感謝いたします。
ま ず本 研 究を 進 める にあ た り, 多大 なる ご 指導, ご 鞭 撻を 頂 きま した 橋本 誠 司准 教 授に 厚く御礼を申し上げます。また, 本研究において多くの有益な御助言を頂いた石川赴夫教授, 松波道夫技官に深く感謝いたします。ならびに実験装置の提供を頂くとともに的確なアド バイスをいただいたテック･コンシェルジェ熊本の小坂光二氏に感謝いたします。
そして, 副査としてご指導いただきました藤井雄作教授に深く感謝いたします。
最 後 に, 長 年に わ たり 経済 的 に 支援 し てい た だいた 両 親, な らび に 苦楽 を共 に 過 ごし た 石川研究室及び橋本研究室のみなさんに心より感謝いたします。
付録 C 言語プログラムと M ファイル
ここでは本研究にて作成した C 言語プログラムを簡単に説明し使用したとプログラムを 以下に示す。
・ gokapro1.ccp
このプログラムはモデル予測制御を行う時の基本プログラムである。各種パラメータを 変更することなどにより制御法の変更が可能となっている。
ここでは本研究にて作成したMファイルについて簡単に説明し使用したMファイルを以 下に示す。
・ m_cata_vff2.m
このプログラムは得られた入出力信号から可変忘却要素を使用したRLS法のオンライン 同定を行うプラグラムである。
・ dsmpccom.m
このプログラムはモデル予測制御を行う時の設計パラメータを導出するプラグラムであ る。
・ gmpc_simulation.m
このプログラムは上記で導出した設計パラメータを用いてモデル予測制御のシミュレー ションを実行するプログラムである。
最後にモデル予測制御で使用したSimlinkを掲載する。
///// gokapro1////
// timertest0520_2Dlg.cpp : インプリメンテーション ファイル //
#include "stdafx.h"
#include "timertest0520_2.h"
#include "timertest0520_2Dlg.h"
#include "FbiDio.h"
#include "servoio.h"
//#include "servoiov2.h"
#include <process.h>
#include <math.h>
// ここから servoio.c から持ってきた
#include <windows.h>
#include <stdio.h>
//#include <math.h>
#include <dos.h>
//#include "FbiDio.h" //Header file for GPC-2000
//#include "servoio.h" //Header file for
Servoio
HANDLE ghDeviceHandle_X = INVALID_HANDLE_VALUE;
// Device handle
HANDLE ghDeviceHandle_Y = INVALID_HANDLE_VALUE;
// Device handle
//HANDLE ghDeviceHandle = INVALID_HANDLE_VALUE;
// Device handle
//char gszDeviceName[24]=SERVOIO_DEVICENAME;
char gszDeviceName_X[24]=SERVOIO_DEVICENAME_X;
char gszDeviceName_Y[24]=SERVOIO_DEVICENAME_Y;
int gnUsmfreqini;
int gnUsmpoffsetini;
BYTE bSetCount;
extern HWND ghWnd;
// ここまで
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
BOOL DsplyErrMessage( DWORD, char * );
float Gene_ref(int Num_Rep, char profile);
void Gene_ref_XY(int Num_Rep, char profile, float *pos_refx, float *pos_refy);
// ここからプロトタイプ宣言
int Servo_open(int fvalue, char xyaxis);
int Servo_flush(char xyaxis);
//int Servo_cmdset(int cmd,unsigned long* datap);
int Servo_cmdset(int cmd,int* datap, char xyaxis);
int Servo_reset(char xyaxis);
int Servo_scalereset(char xyaxis);
int Servo_usmsetamp(int avalue, char xyaxis);
int Servo_usmsetfreq(int fvalue, char xyaxis);
int Servo_usmsetpoffset(int pvalue,char xyaxis);
int Servo_usmseteoffset(int evalue,char xyaxis);
int Servo_usmenable(char xyaxis);
int Servo_usmdisable(char xyaxis);
int Servo_setledbits(int bits,char xyaxis);
int Servo_changecondition(int bits,char xyaxis);
int Servo_getlimit(int * ptr,char xyaxis);
int Servo_getstatus(int * ptr,char xyaxis);
int Servo_getdipsw(int * ptr,char xyaxis);
int Servo_getposition(int * ptr,char xyaxis);
int Servo_getstatposition(int * dptr, int * sptr,char xyaxis);
int Servo_tmstart10(BYTE count,char xyaxis);
int Servo_tmstart100(BYTE count,char xyaxis);
BOOL Servo_tmcheck(BYTE count,char xyaxis);
int Servo_tmstop(char xyaxis);
// ここまでプロトタイプ宣言
int nod = 0;
float pos_datax[5000];
float u_datax[5000];
float ref_datax[5000];
float pos_datay[5000];
float u_datay[5000];
float ref_datay[5000];
float tmp_1[5000];
float tmp_2[5000];
float flag_bang[5000];
/*float ev1_datay[20][1000];
float ev2_datay[20][1000];
float ev3_datay[20][1000];
float ev4_datay[20][1000];
float ev5_datay[20][1000];
float ev6_datay[20][1000];
float ev7_datay[20][1000];*/
//int check_a_data[10], check_b_data[10], check_c_data[10], check_d_data[10], check_e_data[10];
int SampleOfData = 5000;
int k;
//int SampleOfData = 1024;
// サーボの状態 int Servo_Stat = 0;
// プロファイル用
#define STP 2
#define TPZD 3
#define SINE 4
#define S_CURVE 5
#define CIRC 6
float ref_amp_stp, ref_amp_sine, ref_amp_scv, ref_vel_tpzd, ref_vel_scv, ref_time_stp, ref_time_tpzd, ref_time_scv, ref_freq_sine, ref_acc_scv;
float ref_amp_circ, ref_freq_circ, ref_phase_circ;
// for s-curve
float t1_scv, t2_scv, t3_scv, t4_scv, t5_scv, t6_scv, t7_scv;
float pos1_scv, pos2_scv, pos3_scv, pos4_scv, pos5_scv, pos6_scv;
float t_acc_scv, t_vel_scv, t_setl_scv;
/////////////////////////////////////////////////////////////////////////////
// アプリケーションのバージョン情報で使われている CAboutDlg ダイアログ
class CAboutDlg : public CDialog {
public:
CAboutDlg();
// ダイアログ データ
//{{AFX_DATA(CAboutDlg)
enum { IDD = IDD_ABOUTBOX };
//}}AFX_DATA
// ClassWizard は仮想関数のオーバーライドを生成します
//{{AFX_VIRTUAL(CAboutDlg) protected:
virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV のサポート //}}AFX_VIRTUAL
// インプリメンテーション protected:
//{{AFX_MSG(CAboutDlg) //}}AFX_MSG
DECLARE_MESSAGE_MAP() };
CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD) {
//{{AFX_DATA_INIT(CAboutDlg) //}}AFX_DATA_INIT
}
void CAboutDlg::DoDataExchange(CDataExchange* pDX) {
CDialog::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CAboutDlg) //}}AFX_DATA_MAP
}
BEGIN_MESSAGE_MAP(CAboutDlg, CDialog) //{{AFX_MSG_MAP(CAboutDlg)
// メッセージ ハンドラがありません。
//}}AFX_MSG_MAP END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CTimertest0520_2Dlg ダイアログ
CTimertest0520_2Dlg::CTimertest0520_2Dlg(CWnd* pParent /*=NULL*/) : CDialog(CTimertest0520_2Dlg::IDD, pParent)
{
//{{AFX_DATA_INIT(CTimertest0520_2Dlg)
// メモ: この位置に ClassWizard によってメンバの初期化が追加されます。
//}}AFX_DATA_INIT
// メモ: LoadIcon は Win32 の DestroyIcon のサブシーケンスを要求しません。
m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}
void CTimertest0520_2Dlg::DoDataExchange(CDataExchange* pDX) {
CDialog::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CTimertest0520_2Dlg)
// メモ: この場所には ClassWizard によって DDX と DDV の呼び出しが追加されます。
//}}AFX_DATA_MAP }
BEGIN_MESSAGE_MAP(CTimertest0520_2Dlg, CDialog) //{{AFX_MSG_MAP(CTimertest0520_2Dlg) ON_WM_SYSCOMMAND()
ON_WM_PAINT()
ON_WM_QUERYDRAGICON()
ON_BN_CLICKED(IDC_OPEN, OnOpen)
ON_BN_CLICKED(IDC_TIMERSTART, OnTimerstart) ON_BN_CLICKED(IDC_GOTEST, OnGotest)
ON_BN_CLICKED(IDC_BUTTON1, OnButton1) ON_WM_HSCROLL()
ON_BN_CLICKED(IDC_BUTTON2, On_Open2) ON_EN_CHANGE(IDC_FILE, OnChangeFile) //}}AFX_MSG_MAP
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CTimertest0520_2Dlg メッセージ ハンドラ
BOOL CTimertest0520_2Dlg::OnInitDialog() {
CDialog::OnInitDialog();
// "バージョン情報..." メニュー項目をシステム メニューへ追加します。
// IDM_ABOUTBOX はコマンド メニューの範囲でなければなりません。
ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
ASSERT(IDM_ABOUTBOX < 0xF000);
CMenu* pSysMenu = GetSystemMenu(FALSE);
if (pSysMenu != NULL) {
CString strAboutMenu;
strAboutMenu.LoadString(IDS_ABOUTBOX);
if (!strAboutMenu.IsEmpty()) {
pSysMenu->AppendMenu(MF_SEPARATOR);
pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
} }
// このダイアログ用のアイコンを設定します。フレームワークはアプリケーションのメイ ン
// ウィンドウがダイアログでない時は自動的に設定しません。
SetIcon(m_hIcon, TRUE); // 大きいアイコンを設定
SetIcon(m_hIcon, FALSE); // 小さいアイコンを設定
// TODO: 特別な初期化を行う時はこの場所に追加してください。
//---スクロールバーの利用---
CSliderCtrl* pSlide1=(CSliderCtrl *)GetDlgItem(IDC_SLIDER1);
pSlide1->SetRange(-100,100); // Slider_Gain の変化幅 50% -> 200%
pSlide1->SetPos(0); // Slider_Gain の初期値 //---
return TRUE; // TRUE を返すとコントロールに設定したフォーカスは失われません。
}
void CTimertest0520_2Dlg::OnSysCommand(UINT nID, LPARAM lParam) {
if ((nID & 0xFFF0) == IDM_ABOUTBOX) {
CAboutDlg dlgAbout;
dlgAbout.DoModal();
} else {
CDialog::OnSysCommand(nID, lParam);
} }
// もしダイアログボックスに最小化ボタンを追加するならば、アイコンを描画する // コードを以下に記述する必要があります。MFC アプリケーションは document/view // モデルを使っているので、この処理はフレームワークにより自動的に処理されます。
void CTimertest0520_2Dlg::OnPaint() {
if (IsIconic()) {
CPaintDC dc(this); // 描画用のデバイス コンテキスト
SendMessage(WM_ICONERASEBKGND,(WPARAM)dc.GetSafeHdc(), 0);
// クライアントの矩形領域内の中央
int cxIcon = GetSystemMetrics(SM_CXICON);
int cyIcon = GetSystemMetrics(SM_CYICON);
CRect rect;
GetClientRect(&rect);
int x = (rect.Width() - cxIcon + 1) / 2;
int y = (rect.Height() - cyIcon + 1) / 2;
// アイコンを描画します。
dc.DrawIcon(x, y, m_hIcon);
} else {
CDialog::OnPaint();
} }
// システムは、ユーザーが最小化ウィンドウをドラッグしている間、
// カーソルを表示するためにここを呼び出します。
HCURSOR CTimertest0520_2Dlg::OnQueryDragIcon() {
return (HCURSOR) m_hIcon;
}
void CTimertest0520_2Dlg::OnOpen() {
// TODO: この位置にコントロール通知ハンドラ用のコードを追加してください
// オープンのクリックチェック Servo_Stat = 1 ;
//
// Servo_open(25); // オープンと駆動周波数の設定
Servo_open(25,'Y'); // オープンと駆動周波数の設定 Servo_open(25,'X'); // オープンと駆動周波数の設定
// Servo_open(20); // オープンと駆動周波数の設定
// Servo_open(39); // オープンと駆動周波数の設定
// 以下，デフォルト値の設定
SetDlgItemText(IDC_TIMERCFG, "10"); // サンプリングレイト(Ts×rate) // SetDlgItemText(IDC_FILE, "./dat/dh0"); // 出力データ保存用ファイル
SetDlgItemText(IDC_FILE, "D:/usr/goka/matdata/dat/dgoka1"); // 出 力 デ ー タ 保存用ファイル
// SetDlgItemText(IDC_REF, "1"); // プロファイル
CButton* radio1=(CButton*)GetDlgItem(IDC_RADIO1);
CButton* radio2=(CButton*)GetDlgItem(IDC_RADIO2);
CButton* radio3=(CButton*)GetDlgItem(IDC_RADIO3);
CButton* radio4=(CButton*)GetDlgItem(IDC_RADIO4);
CButton* radio5=(CButton*)GetDlgItem(IDC_RADIO5);
radio1->SetCheck(1);
radio2->SetCheck(0);
radio3->SetCheck(0);
radio4->SetCheck(0);
radio5->SetCheck(0);
SetDlgItemText(IDC_COUNT, "5000"); // カウント数
SetDlgItemText(IDC_REFAMP_STP, "0.05"); // ステップ振幅 [mm]
SetDlgItemText(IDC_REFTIME_STP, "2"); // ステップ周期 [s]
SetDlgItemText(IDC_REFVEL_TPZD, "5"); // 台形波最大速度 [mm/s]
SetDlgItemText(IDC_REFTIME_TPZD, "2"); // 台形波周期 [s]
SetDlgItemText(IDC_REFAMP_SINE, "1"); // 正弦波振幅 [mm]
SetDlgItemText(IDC_REFFRQ_SINE, "1"); // 正弦波周波数 [Hz]
SetDlgItemText(IDC_REFAMP_SCV, "10"); // Sカーブ振幅 [mm]
SetDlgItemText(IDC_REFVEL_SCV, "20"); // Sカーブ最大速度 [mm/s]
SetDlgItemText(IDC_REFACC_SCV, "80"); // Sカーブ最大加速度 [mm/s/s]
SetDlgItemText(IDC_REFTIME_SCV, "2"); // Sカーブ信号の周期 [s]
SetDlgItemText(IDC_CIRC_AMP, "1"); // 円振幅 [mm]
SetDlgItemText(IDC_CIRC_FREQ, "1"); // 円周波数 [Hz]
SetDlgItemText(IDC_CIRC_PHASE, "90"); // 円位相 [deg]
// For Slider
CSliderCtrl* pSlide1=(CSliderCtrl *)GetDlgItem(IDC_SLIDER1);
CString strText;
strText.Format("%d %c",pSlide1->GetPos(),'%');
SetDlgItemText(IDC_SLIDER, strText);
// For Postion Error CRect myRECT;
CWnd* myPICT2 = GetDlgItem(IDC_PICT2);
CClientDC myDC2(myPICT2);
myPICT2->GetClientRect(myRECT);
myDC2.FillSolidRect(myRECT,RGB(255,255,255));
CWnd* myPICT1 = GetDlgItem(IDC_PICT1);
CClientDC myDC1(myPICT1);
myPICT1->GetClientRect(myRECT);
myDC1.FillSolidRect(myRECT,RGB(255,255,255));
CWnd* myPICT3 = GetDlgItem(IDC_PICT3);
CClientDC myDC3(myPICT3);
myPICT3->GetClientRect(myRECT);
myDC3.FillSolidRect(myRECT,RGB(255,255,255));
}
BOOL DsplyErrMessage( DWORD dwErrCode ,char * msg) {
char pszErrCode[] = "Return value";
char szTemp[128];
wsprintf(szTemp, "Return value: %08ld¥n¥t", dwErrCode);
// wsprintf(szTemp, "Return value: %08lx¥n¥t", dwErrCode);
lstrcat(szTemp, msg);
lstrcat(szTemp, ": ");
switch(dwErrCode) {
case FBIDIO_ERROR_SUCCESS: // Completed successfully.
lstrcat(szTemp, "Completed successfully.");
break;
// case SERVOIO_ERR_PARAM:
// lstrcat(szTemp, "The command parameter is invalid.");
// break;
case FBIDIO_ERROR_NOT_DEVICE:
lstrcat(szTemp, "The device handle is invalid.");
break;
case FBIDIO_ERROR_NOT_OPEN:
lstrcat(szTemp, "The device could not be opened.");
break;
case FBIDIO_ERROR_INVALID_HANDLE:
lstrcat(szTemp, "The device handle is invalid.");
break;
case FBIDIO_ERROR_ALREADY_OPEN:
lstrcat(szTemp, "The device is already open.");
break;
case FBIDIO_ERROR_INSUFFICIENT_BUFFER:
lstrcat(szTemp, "Data area passed to the system call is too small.");
break;
case FBIDIO_ERROR_IO_PENDING:
lstrcat(szTemp, "An asynchronous I/O operation is in progress.");
break;
case FBIDIO_ERROR_NOT_SUPPORTED:
lstrcat(szTemp, "The feature is not supported.");
break;
case FBIDIO_ERROR_MEMORY_NOTALLOCATED:
lstrcat(szTemp, "Allocating work area failed.");
break;
case FBIDIO_ERROR_PARAMETER:
lstrcat(szTemp, "Parameters passed to the function are invalid.");
break;
case FBIDIO_ERROR_INVALID_CALL:
lstrcat(szTemp, "Invalid function call was occurred.");
break;
case FBIDIO_ERROR_DRVCAL:
lstrcat(szTemp, "The driver could not be called out.");
break;
case FBIDIO_ERROR_NULL_POINTER:
lstrcat(szTemp, "NULL pointer is passed between the driver and the DLL.");
break;
default:
lstrcat(szTemp, "Not-expected error");
break;
}
AfxMessageBox( szTemp, (MB_ICONINFORMATION | MB_OK));
return(TRUE);
}
void CTimertest0520_2Dlg::OnTimerstart() {
char szName[32];
int bits;
int nRet;
int count;
BYTE bTimerCount;
// TODO: この位置にコントロール通知ハンドラ用のコードを追加してください
GetDlgItemText(IDC_TIMERCFG, szName,16);
sscanf(szName,"%d",&bits);
nRet = Servo_tmstart10( (BYTE) bits , 'Y');
if (nRet != FBIDIO_ERROR_SUCCESS){
DsplyErrMessage( nRet, "SetTimerConfig");
}
count=20;
while(count-- >0){
nRet = Servo_tmcheck(2,'Y');
if (nRet != FBIDIO_ERROR_SUCCESS){
DsplyErrMessage( nRet, "tmcheck");
}
}
// SetDlgItemText(IDC_TIMERCOUNT, szTemp);
}
void CTimertest0520_2Dlg::OnGotest() {
char szName[32];
char szTemp[32];
int bits;
int nRet;
int count;
int loopd;
// unsigned long data;
int posx, statx;
int posy, staty;
int started =0;
float enc_posx, pos_refx, vel_refx, u_floatx, u_float_inx , vel_float_tustinx, vel_float_zohx;
float enc_posy, pos_refy, vel_refy, u_floaty, u_float_iny , vel_float_tustiny, vel_float_zohy;
float u_float1x, u_float2x, u_1_maxx, e_ux, e_newx, e_new_ipx, e_u_ipx;
float u_float1y, u_float2y, u_1_maxy, e_uy, e_newy, e_new_ipy, e_u_ipy;
float pos_ref_pastx=0;
float pos_ref_pasty=0;
int data_outx;
int data_outy;
float pos_data_int[1000];
float u_data_int[1000];
// float ref_data[nod]
#define C_LIMIT 10 // 80Hz Ts=0.5ms
float y_ny=0 ;
// float yk_pny1, yk_pny2;
// For Y-axis velocity feedback // float vel_float_zohy=0;
// wsp:200 Hz : stable
float k_vely = 0.245435;
float k_fy = 2.42594;
//
float enc_pos_px=0 ; float enc_pos_py=0 ;
int mk_count = 4 ;
int u_ofset = 3 ;
int check_freq = 0;
int Drive_freq_base = 20;
int Drive_freq = 20 ;
float Scale_Res = 0.0001 ; // for 100 um , unit=mm float Scale_Res_Inv = 10000 ; // for 100 um , unit=mm // float Scale_Res = 0.00001 ; // for 10 nm , unit=mm // float Scale_Res_Inv = 100000 ; // for 10 nm , unit=mm
char profile ;
int tmp1,tmp2;
float Slider_Gain;
// bang-bang
/* float s_fricy = 0.3;
float s_fric_ry = -0.3;
float ery ;*/
float s_fricy = 0.58;
float s_fric_ry = -0.83;
float ery ;
// MPC Controller Y-Parameters(0.1ms)
/* float CC11 = -1.8998;
float CC12 = 0.80654;
float CC13 = 0;
float DC11 = 497.92;
float DC12 = -497.92;
float phiiIKC11 = 1.8065;
float phiiIKC12 = -0.8065;
float phiiIKC13 = 0;
float phiiIKC21 = 1.0000;
float phiiIKC22 = 0;
float phiiIKC23 = 0;
float phiiIKC31 = 0.0017;
float phiiIKC32 = -0.0005;
float phiiIKC33 = 0;
float phiiKest11 = 0;
float phiiKest21 = 0;
float phiiKest31 = 1;
float gami11 = 1;
float gami21 = 0;
float gami31 = 0.0006;
float GAM11 = 1;
float GAM21 = 0;
float GAM31 = 0.00063564;
float PHI11 = 1.8065;
float PHI12 = -0.80654;
float PHI13 = 0;
float PHI21 = 1.0000;
float PHI22 = 0;
float PHI23 = 0;
float PHI31 = 0.00174;
float PHI32 = -0.00051267;
float PHI33 = 1;
float C11 = 0;
float C12 = 0;
float C13 = 1;
float L = 0.8;
float delta_u = 0;
float delta_u_past1 = 0;
float u_past1 = 0 ; float u_past2 = 0;
float u_1y;
float enc_pos_past1y = 0;
float enc_pos_past2y = 0;
float Xc1;
float Xc2;
float Xc3;
float Xc_past1 = 0;
float Xc_past2 = 0;
float Xc_past3 = 0;
float dest=0;
float xkx1 = 0;
float xkx2 = 0;
float xkx3 = 0;
float d_est;
float y_est = 0;
float xkx_past1 = 0;
float xkx_past2 = 0;
float xkx_past3 = 0;
float xk_inte = 0;
float xk_inte_past = 0;
float flag_bang_bang=0; */
// MPC Controller Y-Parameters(0.5ms)
float CC11 = -1.9749;
float CC12 = 0.89808;
float CC13 = 0;
float DC11 = 1856.1;
float DC12 = -1856.1;
float phiiIKC11 = 1.8981;
float phiiIKC12 = -0.8981;
float phiiIKC13 = 0;
float phiiIKC21 = 1.0000;
float phiiIKC22 = 0;
float phiiIKC23 = 0;
float phiiIKC31 = 0.0005;
float phiiIKC32 = -0.0001;
float phiiIKC33 = 0;
float phiiKest11 = 0;
float phiiKest21 = 0;
float phiiKest31 = 1;
float gami11 = 1;
float gami21 = 0;
float gami31 = 0.00016455;
float GAM11 = 1;
float GAM21 = 0;
float GAM31 = 0.00016455;
float PHI11 = 1.8981;
float PHI12 = -0.89808;
float PHI13 = 0;
float PHI21 = 1.0000;
float PHI22 = 0;
float PHI23 = 0;
float PHI31 = 0.00047109;
float PHI32 = -0.00014778;
float PHI33 = 1;
float C11 = 0;
float C12 = 0;
float C13 = 1;
float L = 1.8;
float delta_u = 0;
float delta_u_past1 = 0;
float u_past1 = 0 ; float u_past2 = 0;
float u_1y;
float enc_pos_past1y = 0;
float enc_pos_past2y = 0;
float Xc1;
float Xc2;
float Xc3;
float Xc_past1 = 0;
float Xc_past2 = 0;
float Xc_past3 = 0;
float dest=0;
float xkx1 = 0;
float xkx2 = 0;
float xkx3 = 0;
float d_est;
float y_est = 0;
float xkx_past1 = 0;