＜３軸モータコントロールプログラム＞
割
り込みルーチンはここをクリックしてください。
＜３軸モータコントロールプログラム＞
///////////////////////////////////////////////////////////////
// moz80.c
///////////////////////////////////////////////////////////////
//
// 2000 1/20
//
// Ver 0.93 相対的な移動追加
// Ver 0.94 相対移動と絶対移動でX_objが干渉している
// 不具合修正。相対用のXr_objを使用する。
//
// Ver 0.95 X_obj = X_cur追加
// Ver 0.96 dda_move不具合
// Ver 0.97 dda_move不具合 Ｘ＝Ｙ＃Ｚの時Ｘが移動せず
//
// ＜定義＞
// ホスト：ＦｒｅｅＢＳＤのＣＮＣホスト
// コントローラ：Ｚ８０のモータコントローラ
//
// ＜ハード残件＞
// 緊急停止-＞コントローラリセットでＢＳＤフリーズ
//
// ＜ソフト残件＞
// キャリブレートが不動
//
// ＜割り込み要因＞
// １：ＴＵ−ＣＬＫ１（エンコーダＡ相のラッチ）
// ２：ライトＦＦ−（ライトＦＩＦＯのフルフラグの解除のラッチ）
// ３：リードＥＦＦ−（リードＦＩＦＯのエンプティフラグの解除のラッチ）
// ４：ＣＨＫ＿ＥＲＲ（コントローラからのエラー）
// ５：ＬＩＭＩＴ＿ＥＲＲ（コントローラからリミットエラー）
//
// ＜割り込みクリア方法＞
// １と２は割り込みクリアポートを０、１にしてラッチをクリアする。
// ３はコントローラとの通信の不具合なのでコントローラをリセットする。
// ４はＸ、Ｙ、Ｚいずれかのリミットエラーなのでアプリ上に警告する。
//
// ＜割り込み処理＞
// １：ロータリエンコーダの回転パルス、ティーチング時にポイント指示
// ２：ライトＦＩＦＯのフルフラグの解除、ＦＩＦＯライト
// ３：リードＦＩＦＯのエンプティフラグの解除、ＦＩＦＯリード
//
//
// ＜コマンドフォーマット＞
//
// シーケンス番号：１バイト０−ＦＦ
// コマンド：１バイト
// データ列：可変長
// ＣＨＫ：サムチェック
//
// シーケンス番号、コマンド番号、データ、データ、・・・、ＣＨＫ
//
// ＜応答データフォーマット＞
// シーケンス番号、コマンド番号、ＣＨＫ
// （応答データのシーケンス番号はコマンドのシーケンス番号を使う）
// ＣＨＫ：シーケンス番号から、ＣＨＫ直前の総和を８ビットで計算し
// その２の補数をＣＨＫとする.検査はすべてを加えると０になる.
//
// 絶対移動コマンドの座標の最大値は２４ビット（３バイト）符号なし
// 相対移動コマンドの座標の最大値は１６ビット（２バイト）符号あり
//
//
// ＜ホスト下位処理＞
// ライト：コマンド列をＦＩＦＯにライトする。１バイト毎にＦＦをチェックする。
//
フルでライトできなければｓｗａｉｔする。フルフラグの解除割り込み
// ルーチンでｗａｋｅｕｐする。
//
// リード：ＦＩＦＯをリードする。１バイト毎にＥＦをチェックする。
//
エンプティならばｓｗａｉｔする。エンプティフラグの解除割り込み
// ルーチンでｗａｋｅｕｐする。
//
// ＜ＪＯＹ＞
// Ｘ軸、Ｙ軸、Ｚ軸
// ボタン１（ポイント設定）２（ＴＯＵＲＢＯ），３，４
// Ｘ、Ｙ、Ｚと４つのボタンはサウンドブラスタ経由で入力
// デバイスはjoy0,joy1の２つで入力する。
//
//
// ＜ホストポート＞
// Ｐ０：入力ポート／エンコーダ、割り込みステータス
// ０：ＲＥ−ＩＮＴ（エンコーダ割り込みステータス、ＨＩＧＨ）
// １：ＲＥ−ＤＩＲ（エンコーダ方向）
// ２：ＦＦ−ＩＮＴ（ライトＦＩＦＯフルフラグ解除割り込みステータス、ＨＩＧＨ）
// ３：ＥＦ−ＩＮＴ（リードＦＩＦＯエンプティ解除割り込みステータス、ＨＩＧＨ）
// ４：ＣＨＫ＿ＥＲＲ（コントローラエラー、ＨＩＧＨ）
// ５：ＬＩＭ＿ＥＲＲ（リミットエラー、ＨＩＧＨ）
// ６：Ｇ１＿ＥＲＲ（ジェネラルエラー１）
// ７：Ｇ２＿ＥＲＲ（ジェネラルエラー２）
//
// Ｐ１：入力ポート ティーチングユニットＳＷ
// ０：ＴＵ−ＳＷ０（Ｘ軸＋）
// １：ＴＵ−ＳＷ１（Ｘ軸−）
// ２：ＴＵ−ＳＷ２（Ｙ軸＋）
// ３：ＴＵ−ＳＷ０（Ｙ軸−）
// ４：ＴＵ−ＳＷ１（Ｚ軸＋）
// ５：ＴＵ−ＳＷ２（Ｚ軸−）
// ６：Ｘ
// ７：Ｘ
//
// Ｐ２下位：ＦＩＦＯステータスリードポート
// ０：ＦＦ（ライトＦＩＦＯフルフラグ、ＬＯＷ）
// １：ＥＦ（リードＦＩＦＯエンプティフラグ、ＬＯＷ）
// ２：ＨＦ（リードＦＩＦＯハーフフラグ、ＬＯＷ）
// ３：Ｘ
// Ｐ２上位：制御ライトポート
// ４：割り込みステータスＦＦクリア（ＬＯＷ―ＨＩＧＨでクリア）
// ５：
// ６：
// ７：コントローラリセット（ＬＯＷでリセット）
//
//
// ＜コントローラポート＞
// Ｐ０：出力ポート／ステッパモータクロック、回転方向、主軸、パトライト
// ０：Ｘ軸ＣＬＫ（立ち上がり）
// １：Ｘ軸ＤＩＲ
// ２：Ｙ軸ＣＬＫ（立ち上がり）
// ３：Ｙ軸ＤＩＲ
// ４：Ｚ軸ＣＬＫ（立ち上がり）
// ５：Ｚ軸ＤＩＲ
// ６：パトライト制御（ＬＯＷで回転）
// ７：主軸モータ制御（ＬＯＷで回転）
//
// Ｐ１：入力ポート、リミットポート
// ０：Ｘ軸ＬＩＭ１（左、ＬＯＷでリミット）
// １：Ｘ軸ＬＩＭ２（右、ＬＯＷでリミット）
// ２：Ｙ軸ＬＩＭ１（前、ＬＯＷでリミット）
// ３：Ｙ軸ＬＩＭ２（奥、ＬＯＷでリミット）
// ４：Ｚ軸ＬＩＭ１（下、ＬＯＷでリミット）
// ５：Ｚ軸ＬＩＭ２（上、ＬＯＷでリミット）
// ６：Ｘ
// ７：ステッパドライバ汎用入力
//
// Ｐ２下位：ＦＩＦＯステータスリードポート
// ０：ＦＦ（ライトＦＩＦＯフルフラグ、ＬＯＷ）
// １：ＥＦ（リードＦＩＦＯエンプティフラグ、ＬＯＷ）
// ２：ＨＦ（リードＦＩＦＯハーフフラグ、ＬＯＷ）
// ３：Ｘ
// Ｐ２上位：イネーブルポート
// ４：Ｘ軸ＥＮＡＢＬＥ（ＬＯＷ）
// ５：Ｙ軸ＥＮＡＢＬＥ（ＬＯＷ）
// ６：Ｚ軸ＥＮＡＢＬＥ（ＬＯＷ）
// ７：ステッパドライバ汎用出力
//
//
// ＰＯＵＴ：エラー出力
// ０：Ｃ８２５４＿ＧＡＴＥ０
// １：Ｘ
// ２：Ｘ
// ３：Ｘ
// ４：ＣＨＫ＿ＥＲＲ（コントローラエラー、ＨＩＧＨ）
// ５：ＬＩＭ＿ＥＲＲ（リミットエラー、ＨＩＧＨ）
// ６：Ｇ１＿ＥＲＲ
// ７：Ｇ２＿ＥＲＲ
//
//
///////////////////////////////////////////////////////////////
// コマンド定義
///////////////////////////////////////////////////////////////
//
// ＜リセット＞：００
// 動作：コントローラをリセットする
//
// データ数／コマンド総数：０／３
//
// 例：ＳＱ、００、ＣＨＫ
//
// 応答データ：無し
//
#define COM_RESET_NUM 0x0
#define COM_RESET_COML 3
#define COM_RESET_RESL 0
//
//
//
//
// ＜自己診断＞：１０
// 動作：自己診断を行ない結果を返す
//
// データ数／コマンド総数：０／３
//
// 例：ＳＱ、１０、ＣＨＫ
//
// 応答データ：ＳＱ、１０、Ｒｅｓ、ＣＨＫ
// （Ｒｅｓ：正常＝１、異常＝０）
//
// 応答データ数：４
//
#define COM_DIAG_NUM 0x10
#define COM_DIAG_COML 3
#define COM_DIAG_RESL 4
//
//
//
//
// ＜ソフトウェアバージョン通知＞：１１
//
// 動作：ソフトウェアのバージョンを返す
//
// データ数／コマンド総数：０／３
//
// 例：ＳＱ、１１、ＣＨＫ
//
// 応答データ：ＳＱ、１１、”Ｖ１．００”、ＣＨＫ
//
// 応答データ数：８
//
#define COM_VER_NUM 0x11
#define COM_VER_COML 3
#define COM_VER_RESL 8
//
//
//
//
// ＜テスト＞：１２
//
// 動作：テストする
//
// データ数／コマンド総数：０／３
//
// 例：ＳＱ、１２、ＣＨＫ
//
// 応答データ：ＳＱ、１２、ＣＨＫ
//
// 応答データ数：３
//
#define COM_TEST_NUM 0x12
#define COM_TEST_COML 3
#define COM_TEST_RESL 3
//
//
//
//
// ＜ループバック＞：２０
// 動作：入力されたデータを４バイトそのまま返す。
//
// データ数／コマンド総数：４／７
//
// フォーマット：２０、Ｄ０、Ｄ１、Ｄ２、Ｄ３
//
// 例：ＳＱ、２０、００、５５、ａａ、ｆｆ、ＣＨＫ
//
// 応答データ：ＳＱ、２０、００、５５、ａａ、ｆｆ、ＣＨＫ
//
// 応答データ数：７
//
#define COM_LOOPB_NUM 0x20
#define COM_LOOPB_COML 7
#define COM_LOOPB_RESL 7
//
//
//
//
// ＜シンク＞：４０
// 動作：移動の同期を取るために、このシーケンスの１つ前のコマンドの終了時にホストに応答する。
//
// データ数／コマンド総数：０／３
//
// フォーマット：４０
//
// 例：ＳＱ、４０、ＣＨＫ
//
// 応答データ：ＳＱ、４０、ＣＨＫ
//
// 応答データ数：３
//
#define COM_SYNC_NUM 0x40
#define COM_SYNC_COML 3
#define COM_SYNC_RESL 3
//
//
//
//
// ＜現在位置を通知する＞：４１
// 動作：現在の位置を通知する
//
// データ数／コマンド総数：０／３
//
// フォーマット：４１
//
// 例：ＳＱ、４１、ＣＨＫ
//
// 応答データ：ＳＱ、４１、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ数：１２
//
#define COM_RCURR_NUM 0x41
#define COM_RCURR_COML 3
#define COM_RCURR_RESL 12
//
//
//
//
// ＜現在位置を登録する＞：４２
// 動作：現在の位置を登録する
//
// データ数／コマンド総数：９／１２
//
// フォーマット：４２、Ｘｈ，Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、４２、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ数：なし
//
#define COM_WCURR_NUM 0x42
#define COM_WCURR_COML 12
#define COM_WCURR_RESL 0
//
//
//
//
// ＜最高速度通知＞：４３
// 動作：移動最高速度を通知する
//
// データ数／コマンド総数：０／３
//
// フォーマット：４３
//
// 例：ＳＱ、４３、ＣＨＫ
//
// 応答データ数：ＳＱ、４３、MAX_H, MAX_L,ＣＨＫ
//
#define COM_RMAX_NUM 0x43
#define COM_RMAX_COML 3
#define COM_RMAX_RESL 5
//
//
//
//
// ＜最高速度登録＞：４４
// 動作：移動最高速度を登録する
//
// データ数／コマンド総数：２／５
//
// フォーマット：４４、MAX_H，MAX_L
//
// 例：ＳＱ、４４、１２、３４、ＣＨＫ
//
// 応答データ数：無し
//
#define COM_WMAX_NUM 0x44
#define COM_WMAX_COML 5
#define COM_WMAX_RESL 0
//
//
//
// ＜ステップ数通知＞：４５
// 動作：ステップ数を通知する
//
// データ数／コマンド総数：０／３
//
// フォーマット：４５
//
// 例：ＳＱ、４５、ＣＨＫ
//
// 応答データ数：ＳＱ、４５、STEP_H, STEP_L,ＣＨＫ
//
#define COM_RSTEP_NUM 0x45
#define COM_RSTEP_COML 3
#define COM_RSTEP_RESL 5
//
//
//
//
// ＜ステップ数登録＞：４６
// 動作：ステップ数を登録する
//
// データ数／コマンド総数：２／５
//
// フォーマット：４６、STEP_h，STEP_L
//
// 例：ＳＱ、４６、１２、３４、ＣＨＫ
//
// 応答データ数：無し
//
#define COM_WSTEP_NUM 0x46
#define COM_WSTEP_COML 5
#define COM_WSTEP_RESL 0
//
//
//
// ＜タイマー待ち＞：４７
// 動作：指定したタイマーを待って応答データを返す
//
// データ数／コマンド総数：２／５
//
// フォーマット：４７、Ｔｈ，Ｔｌ
//
// 例：ＳＱ、４７、５５、ａａ、ＣＨＫ
//
// 応答データ数：ＳＱ、４７、ＣＨＫ
//
#define COM_TIMER_RES_NUM 0x47
#define COM_TIMER_RES_COML 5
#define COM_TIMER_RES_RESL 3
//
//
//
//
// ＜タイマー待ち＞：４８
// 動作：指定したタイマーを待つ（応答データ無し）
//
// データ数／コマンド総数：２／５
//
// フォーマット：４８、Ｔｈ，Ｔｌ
//
// 例：ＳＱ、４８、５５、ａａ、ＣＨＫ
//
// 応答データ数：無し
//
#define COM_TIMER_NRES_NUM 0x48
#define COM_TIMER_NRES_COML 5
#define COM_TIMER_NRES_RESL 0
//
//
//
//
//
// ＜ダイレクトポート出力＞：５０
// 動作：出力ポートＰ０、Ｐ２Ｈ、ＥＲＲ＿ＰＯＵＴに直接データを出力する
// Ｐ０：出力ポート／ステッパモータクロック、回転方向、主軸、パトライト
// Ｐ２上位：イネーブルポート
// ＥＲＲ＿ＰＯＵＴ：ＣＨＫ、ＬＩＭエラー、ＮＭＩ＿ＥＮ
//
// データ数／コマンド総数：６／９
//
// フォーマット：５０、Ｐ０、ＭＡＳＫ、Ｐ２Ｈ、ＭＡＳＫ、ＥＲＲ＿ＰＯＵＴ、ＭＡＳＫ
//
// 例：ＳＱ、５０、５５、ｆ０、ａａ、０ｆ、ｆ０、ｆ０、ＣＨＫ
//
// 応答データ数：無し
//
#define COM_OPORT_NUM 0x50
#define COM_OPORT_COML 9
#define COM_OPORT_RESL 0
//
//
//
//
//
// ＜ダイレクトポート入力＞：５１
// 動作：ポートから直接データを入力する
//
// データ数／コマンド総数：０／３
//
// フォーマット：５１
//
// 例：ＳＱ、５１、ＣＨＫ
//
// 応答データ数：ＳＱ、５１、Ｐ０、Ｐ１、Ｐ２、ＥＲＲ＿ＰＯＵＴ、ＣＨＫ
//
#define COM_IPORT_NUM 0x51
#define COM_IPORT_COML 3
#define COM_IPORT_RESL 7
//
//
//
//
// ＜相対ステップ加速３軸移動コマンド＞：７１
// 動作：指定された座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：６／９
//
// フォーマット：７１、 Ｘｈ、Ｘｌ、Ｙｈ、Ｙｌ、Ｚｈ、Ｚｌ
//
// 例：ＳＱ、７１、００、１０、００、２０、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3RMOVE_AU_NUM 0x71
#define COM_3RMOVE_AU_COML 9
#define COM_3RMOVE_AU_RESL 0
//
//
//
//
// ＜相対ステップ加速Ｘ軸移動コマンド＞：７２
// 動作：指定されたＸ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：２／５
//
// フォーマット：７２、Ｘｈ、Ｘｌ
//
// 例：ＳＱ、７２、００、１０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_XRMOVE_AU_NUM 0x72
#define COM_XRMOVE_AU_COML 5
#define COM_XRMOVE_AU_RESL 0
//
//
//
//
// ＜相対ステップ加速Ｙ軸移動コマンド＞：７３
// 動作：指定されたＹ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：２／５
//
// フォーマット：７３、Ｙｈ、Ｙｌ
//
// 例：
// ＳＱ、７３、００、２０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_YRMOVE_AU_NUM 0x73
#define COM_YRMOVE_AU_COML 5
#define COM_YRMOVE_AU_RESL 0
//
//
//
//
// ＜相対ステップ加速Ｚ軸移動コマンド＞：７４
// 動作：指定されたＺ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：２／５
//
// フォーマット：７４、Ｚｈ、Ｚｌ
//
// 例：
// ＳＱ、８１、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_ZRMOVE_AU_NUM 0x74
#define COM_ZRMOVE_AU_COML 5
#define COM_ZRMOVE_AU_RESL 0
//
//
//
//
//
// ＜相対ステップ加速３軸移動コマンド＞：７５
// 動作：指定された座標に速度０からステップで加速しながら最高速度で移動する
//
// データ数／コマンド総数：７／９
//
// フォーマット：７５、Ｘｈ、Ｘｌ、Ｙｈ、Ｙｌ、Ｚｈ、Ｚｌ
//
// 例：ＳＱ、７５、００、１０、００、２０、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3RMOVE_A_NUM 0x75
#define COM_3RMOVE_A_COML 9
#define COM_3RMOVE_A_RESL 0
//
//
//
//
//
// ＜相対定速度３軸移動コマンド＞：７６
// 動作：指定された座標に現在の速度で移動する
//
// データ数／コマンド総数：７／９
//
// フォーマット：７６、Ｘｈ、Ｘｌ、Ｙｈ、Ｙｌ、Ｚｈ、Ｚｌ
//
// 例：ＳＱ、７６、００、１０、００、２０、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3RMOVE_NUM 0x76
#define COM_3RMOVE_COML 9
#define COM_3RMOVE_RESL 0
//
//
//
//
// ＜相対定速度Ｘ軸移動コマンド＞：７７
//
// 動作：指定されたＸ座標に現在の速度で移動する
//
// データ数／コマンド総数：２／５
//
// フォーマット：Ｘｈ、Ｘｌ
//
// 例：ＳＱ、７７、００、１０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_XRMOVE_NUM 0x77
#define COM_XRMOVE_COML 5
#define COM_XRMOVE_RESL 0
//
//
//
//
// ＜相対定速度Ｙ軸移動コマンド＞：７８
//
// 動作：指定されたＹ座標に現在の速度で移動する
//
// データ数／コマンド総数：２／５
//
// フォーマット：Ｙｈ、Ｙｌ
//
// 例：
// ＳＱ、７８、００、２０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_YRMOVE_NUM 0x78
#define COM_YRMOVE_COML 5
#define COM_YRMOVE_RESL 0
//
//
//
//
// ＜相対定速度Ｚ軸移動コマンド＞：７９
//
// 動作：指定されたＺ座標に現在の速度で移動する
//
// データ数／コマンド総数：２／５
//
// フォーマット：７９、Ｚｈ、Ｚｌ
//
// 例：
// ＳＱ、７９、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_ZRMOVE_NUM 0x79
#define COM_ZRMOVE_COML 5
#define COM_ZRMOVE_RESL 0
//
//
//
//
// ＜相対ステップ減速３軸移動コマンド＞：７ａ
//
// 動作：指定された座標に現在の速度からステップで減速しながら移動し停止する
//
// データ数／コマンド総数：７／９
//
// フォーマット：７ａ、Ｘｈ、Ｘｌ、Ｙｈ、Ｙｌ、Ｚｈ、Ｚｌ
//
// 例：ＳＱ、７ａ、００、１０、００、２０、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3RMOVE_U_NUM 0x7a
#define COM_3RMOVE_U_COML 9
#define COM_3RMOVE_U_RESL 0
//
//
//
//
// ＜キャリブレート＞：８０
// 動作：Ｘ、Ｙ、Ｚ軸とも原点に移動する。（現在不動）
// 原点は最左、手前、下のリミットＳＷがＯＮの位置
//
// データ数／コマンド総数：０／３
//
// フォーマット：８０
//
// 例：ＳＱ、８０、ＣＨＫ
//
// 応答データ：ＳＱ、８０、ＣＨＫ
// 原点に移動後、応答データを送信
//
// 応答データ数：３
//
#define COM_CARYB_NUM 0x80
#define COM_CARYB_COML 3
#define COM_CARYB_RESL 3
//
//
//
//
// ＜絶対ステップ加速３軸移動コマンド＞：８１
// 動作：指定された座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：９／１２
//
// フォーマット：８１、Ｘｈ、Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、８１、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3AMOVE_AU_NUM 0x81
#define COM_3AMOVE_AU_COML 12
#define COM_3AMOVE_AU_RESL 0
//
//
//
//
// ＜絶対ステップ加速Ｘ軸移動コマンド＞：８２
// 動作：指定されたＸ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：８２、Ｘｈ，Ｘｍ、Ｘｌ
//
// 例：ＳＱ、８２、００、００、１０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_XAMOVE_AU_NUM 0x82
#define COM_XAMOVE_AU_COML 6
#define COM_XAMOVE_AU_RESL 0
//
//
//
//
// ＜絶対ステップ加速Ｙ軸移動コマンド＞：８３
// 動作：指定されたＹ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：８３、Ｙｈ，Ｙｍ、Ｙｌ
//
// 例：
// ＳＱ、８３、００、００、２０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_YAMOVE_AU_NUM 0x83
#define COM_YAMOVE_AU_COML 6
#define COM_YAMOVE_AU_RESL 0
//
//
//
//
// ＜絶対ステップ加速Ｚ軸移動コマンド＞：８４
// 動作：指定されたＺ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：８４、Ｚｈ，Ｚｍ、Ｚｌ
//
// 例：
// ＳＱ、８１、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_ZAMOVE_AU_NUM 0x84
#define COM_ZAMOVE_AU_COML 6
#define COM_ZAMOVE_AU_RESL 0
//
//
//
//
//
// ＜絶対ステップ加速３軸移動コマンド＞：８５
// 動作：指定された座標に速度０からステップで加速しながら最高速度で移動する
//
// データ数／コマンド総数：９／１２
//
// フォーマット：８５、Ｘｈ、Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、８５、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3AMOVE_A_NUM 0x85
#define COM_3AMOVE_A_COML 12
#define COM_3AMOVE_A_RESL 0
//
//
//
//
//
// ＜絶対定速度３軸移動コマンド＞：８６
// 動作：指定された座標に現在の速度で移動する
//
// データ数／コマンド総数：９／１２
//
// フォーマット：８６、Ｘｈ、Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、８６、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3AMOVE_NUM 0x86
#define COM_3AMOVE_COML 12
#define COM_3AMOVE_RESL 0
//
//
//
//
// ＜絶対定速度Ｘ軸移動コマンド＞：８７
//
// 動作：指定されたＸ座標に現在の速度で移動する
//
// データ数／コマンド総数：３／６
//
// フォーマット：Ｘｈ、Ｘｍ、Ｘｌ
//
// 例：ＳＱ、８７、００、００、１０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_XAMOVE_NUM 0x87
#define COM_XAMOVE_COML 6
#define COM_XAMOVE_RESL 0
//
//
//
//
// ＜絶対定速度Ｙ軸移動コマンド＞：８８
//
// 動作：指定されたＹ座標に現在の速度で移動する
//
// データ数／コマンド総数：３／５
//
// フォーマット：Ｙｈ、Ｙｍ、Ｙｌ
//
// 例：
// ＳＱ、８８、００、００、２０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_YAMOVE_NUM 0x88
#define COM_YAMOVE_COML 6
#define COM_YAMOVE_RESL 0
//
//
//
//
// ＜絶対定速度Ｚ軸移動コマンド＞：８９
//
// 動作：指定されたＺ座標に現在の速度で移動する
//
// データ数／コマンド総数：３／６
//
// フォーマット：８９、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：
// ＳＱ、８９、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_ZAMOVE_NUM 0x89
#define COM_ZAMOVE_COML 6
#define COM_ZAMOVE_RESL 0
//
//
//
//
// ＜絶対ステップ減速３軸移動コマンド＞：８ａ
//
// 動作：指定された座標に現在の速度からステップで減速しながら移動し停止する
//
// データ数／コマンド総数：９／１３
//
// フォーマット：８ａ、Ｘｈ、Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ）
//
// 例：ＳＱ、８ａ、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3AMOVE_U_NUM 0x8a
#define COM_3AMOVE_U_COML 12
#define COM_3AMOVE_U_RESL 0
//
//
//
//
// ＜主軸モータ開始コマンド＞：９０
//
// 動作：主軸モータを回転させる。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９０
//
// 例：ＳＱ、９０、ＣＨＫ
//
// 応答データ：無し
//
#define COM_MOON_NUM 0x90
#define COM_MOON_COML 3
#define COM_MOON_RESL 0
//
//
//
//
// ＜主軸モータ停止コマンド＞：９１
//
// 動作：主軸モータを回転させる。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９１
//
// 例：ＳＱ、９１、ＣＨＫ
//
// 応答データ：無し
//
#define COM_MOOFF_NUM 0x91
#define COM_MOOFF_COML 3
#define COM_MOOFF_RESL 0
//
//
//
//
// ＜警告回転灯開始コマンド＞：９２
//
// 動作：警告回転灯をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９２
//
// 例：ＳＱ、９２、ＣＨＫ
//
// 応答データ：無し
//
#define COM_PTON_NUM 0x92
#define COM_PTON_COML 3
#define COM_PTON_RESL 0
//
//
//
//
// ＜警告回転灯停止コマンド＞：９３
//
// 動作：警告回転灯をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９３
//
// 例：ＳＱ、９３、ＣＨＫ
//
// 応答データ：無し
//
#define COM_PTOFF_NUM 0x93
#define COM_PTOFF_COML 3
#define COM_PTOFF_RESL 0
//
//
//
//
// ＜Ｘ軸イネーブルコマンド＞：９４
//
// 動作：Ｘ軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９４
//
// 例：ＳＱ、９４、ＣＨＫ
//
// 応答データ：無し
//
#define COM_XON_NUM 0x94
#define COM_XON_COML 3
#define COM_XON_RESL 0
//
//
//
//
// ＜Ｘ軸ディゼーブルコマンド＞：９５
//
// 動作：Ｘ軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９５
//
// 例：ＳＱ、９５、ＣＨＫ
//
// 応答データ：無し
//
#define COM_XOFF_NUM 0x95
#define COM_XOFF_COML 3
#define COM_XOFF_RESL 0
//
//
//
//
// ＜Ｙ軸イネーブルコマンド＞：９６
//
// 動作：Ｙ軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９６
//
// 例：ＳＱ、９６、ＣＨＫ
//
// 応答データ：無し
//
#define COM_YON_NUM 0x96
#define COM_YON_COML 3
#define COM_YON_RESL 0
//
//
//
//
// ＜Ｙ軸ディゼーブルコマンド＞：９７
//
// 動作：Ｙ軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９７
//
// 例：ＳＱ、９７、ＣＨＫ
//
// 応答データ：無し
//
#define COM_YOFF_NUM 0x97
#define COM_YOFF_COML 3
#define COM_YOFF_RESL 0
//
//
//
//
// ＜Ｚ軸イネーブルコマンド＞：９８
//
// 動作：Ｚ軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９８
//
// 例：ＳＱ、９８、ＣＨＫ
//
// 応答データ：無し
//
#define COM_ZON_NUM 0x98
#define COM_ZON_COML 3
#define COM_ZON_RESL 0
//
//
//
//
// ＜Ｚ軸ディゼーブルコマンド＞：９９
//
// 動作：Ｚ軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９９
//
// 例：ＳＱ、９９、ＣＨＫ
//
// 応答データ：無し
//
#define COM_ZOFF_NUM 0x99
#define COM_ZOFF_COML 3
#define COM_ZOFF_RESL 0
//
//
//
//
// ＜３軸イネーブルコマンド＞：９ａ
//
// 動作：３軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ａ
//
// 例：ＳＱ、９ａ、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3ON_NUM 0x9a
#define COM_3ON_COML 3
#define COM_3ON_RESL 0
//
//
//
//
// ＜３軸ディゼーブルコマンド＞：９ｂ
//
// 動作：３軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ｂ
//
// 例：ＳＱ、９ｂ、ＣＨＫ
//
// 応答データ：無し
//
#define COM_3OFF_NUM 0x9b
#define COM_3OFF_COML 3
#define COM_3OFF_RESL 0
//
//
//
//
// ＜汎用出力ポートネーブルコマンド＞：９ｃ
//
// 動作：３軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ｃ
//
// 例：ＳＱ、９ｃ、ＣＨＫ
//
// 応答データ：無し
//
#define COM_GON_NUM 0x9c
#define COM_GON_COML 3
#define COM_GON_RESL 0
//
//
//
//
// ＜汎用出力ポートディゼーブルコマンド＞：９ｄ
//
// 動作：３軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ｄ
//
// 例：ＳＱ、９ｄ、ＣＨＫ
//
// 応答データ：無し
//
#define COM_GOFF_NUM 0x9d
#define COM_GOFF_COML 3
#define COM_GOFF_RESL 0
//
//
//
// ＜汎用入力ポート入力コマンド＞：９ｅ
//
// 動作：３軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ｅ
//
// 例：ＳＱ、９ｄ、ＣＨＫ
//
// 応答データ：無し
//
#define COM_GIN_NUM 0x9e
#define COM_GIN_COML 3
#define COM_GIN_RESL 4
//
// moz80.c
//
// ＣＮＣ用Ｚ８０モータドライバ制御プログラム
//
// 1999 7/1
//
// コンパイルコマンドは以下
// bin/start.sofをスタートアップとしてリンク
// コンパイル用バッチファイル LC.BAT
// lcc80 -B -c -g2 moz80.c
// knil #CODE=0 #DATA=8000 main.hex
// ..\lib\start.sof main.sof -F..\lib\romlib.sof
// -mmain.map -nmain.lin -gmain.dbg
// lsimap main
//
// Ｉ／Ｏのアドレス
//
//
#include <stdrom.h>
#include <machine.h>
#include <z80.h>
typedef unsigned int UINT;
typedef unsigned char UCHAR;
// リセット番地（ｓｔａｒｔ．ａｓｚのリセットベクター）
extern START();
// 基本動作
// １．ＦＩＦＯを読み、コマンド単位でバッファに格納する.
// ２．コマンドを解析して、分岐する.
// ３．システム系
// 処理後、応答データをＦＩＦＯに書く.
// ４．移動系
// タイマ割り込み処理で1回ループを回るＤＤＡ処理の中で
// ３軸に対して、ＣＬＫをＤＩＲを制御する.
// タイマに設定する値で移動速度を制御する.
// ステップ加速と減速は変更しながらループする.
// ステップ減速はあらかじめ減速点を計算しておく必要あり.
// 移動系のコマンドは基本的には以下のようなシーケンスに
// なると思われる.
// ＴＵによる移動：
// ＴＵによる移動はステップ
//
// リセットベクタ番地
extern START();
// Ｉ／Ｏポートの定義
// ８２５５
#define P8255_0 0x00
#define P8255_1 0x01
#define P8255_2 0x02
#define P8255_C 0x03
#define P8255_INIT 0x83
#define P8255_0_INIT 0x83
#define P8255_1_X_L1 0x01
#define P8255_1_X_L2 0x02
#define P8255_1_Y_L1 0x04
#define P8255_1_Y_L2 0x08
#define P8255_1_Z_L1 0x10
#define P8255_1_Z_L2 0x20
#define P8255_1_3L_MASK 0x3f
#define P8255_1_NO_LIM 0x3f
// 主軸イネーブルビット
#define P8255_MOON_BIT 0x80
// パトライトイネーブルビット
#define P8255_PTON_BIT 0x40
// 各軸のイネーブルビットセット
#define P8255_C_XON 0x08
#define P8255_C_YON 0x0a
#define P8255_C_ZON 0x0c
#define P8255_C_GON 0x0e
// 各軸ディゼーブルビットセット
#define P8255_C_XOFF 0x09
#define P8255_C_YOFF 0x0b
#define P8255_C_ZOFF 0x0d
#define P8255_C_GOFF 0x0f
// ３軸イネーブルビット
#define P8255_3ON_BIT 0x70
// ８２５４
#define C8254_0 0x40
#define C8254_1 0x41
#define C8254_2 0x42
#define C8254_C 0x43
// 上位・下位マスク
#define C_MASK_HIGH 0xff00
#define C_MASK_LOW 0x00ff
// カウンタ＃０（出力間隔用ＮＭＩに反転して接続）
// クロック７５０Ｋ
// アドレス
// モード０、ＧＡＴＥ制御付き（ＥＲＲ＿ＯＰＯＲＴ）
// 0011 0000
#define C8254_0_MODE 0x30
// カウンタＡ＃０の初期値時間（１０ｍＳＥＣ）
#define C8254_C_INIT 7500
// カウンタ＃１、１０ｍＳＥＣ割り込み用
// クロック７５０Ｋ
// アドレス
// モード０一２０ｍ定周期生成
// 0111 0000
#define C8254_1_MODE 0x70
// カウンタ＃１初期値
#define C8254_1_INIT 15000
// カウンタ＃２フリーラン
// クロック７５０Ｋ
// モード０
// 1011 0000
#define C8254_2_MODE 0xb0
// カウンタ＃２初期値
#define C8254_2_INIT 15000
// ＦＩＦＯ
#define FIFO_WR 0x80
#define FIFO_RD 0x80
// エラー出力ポート
#define ERR_POUT 0xf0
#define NMI_EN_BIT 0x01
#define CHK_ERR_BIT 0x10
#define LIM_ERR_BIT 0x20
#define G1_ERR_BIT 0x40
#define G2_ERR_BIT 0x80
// ＦＩＦＯマスク
#define FIFO_FULL_MASK 0x01
#define FIFO_EMP_MASK 0x02
#define FIFO_HALF_MASK 0x04
// コマンドバッファ
#define COM_BUF_MAX 32
#define COM_STOPER 0xff
#define ERROR 0
#define OK 1
#define NO 0
#define YES 1
// エラー番号
#define ERROR_SEQ 0x01
#define ERROR_CHK 0x02
#define LONG_HIGH_MASK 0x00ff0000
#define LONG_MID_MASK 0x0000ff00
#define LONG_LOW_MASK 0x000000ff
#define INT_HIGH_MASK 0xff00
#define INT_LOW_MASK 0x00ff
#define ACC_FLAG 1
#define UACC_FLAG 2
#define NO_ACC_FLAG 0
#define LIM_MASK 0x3f // 00111111
#define LIM_MASK_X1 0x01 // 左
#define LIM_MASK_X2 0x02 // 右
#define LIM_MASK_X (LIM_MASK_X1 | LIM_MASK_X2)
#define LIM_MASK_Y1 0x04 // 前
#define LIM_MASK_Y2 0x08 // 奥
#define LIM_MASK_Y (LIM_MASK_Y1 | LIM_MASK_Y2)
#define LIM_MASK_Z1 0x10 // 下
#define LIM_MASK_Z2 0x20 // 上
#define LIM_MASK_Z (LIM_MASK_Z1 | LIM_MASK_Z2)
// ＮＭＩ割り込み間隔指定
#define NMI_MAXVAL 10000 // 最適値は実験で求める
#define NMI_MINVAL 0x100 // 最適値は実験で求める
#define STEP_VAL 250
#define CARYB_TIMER 0x100
// コマンド番号とコマンド長、応答長
struct COM_DEF {
UCHAR num;
UCHAR coml;
UCHAR resl;
} const com_def[] = {
COM_RESET_NUM,COM_RESET_COML,COM_RESET_RESL,
COM_DIAG_NUM,COM_DIAG_COML,COM_DIAG_RESL,
COM_VER_NUM,COM_VER_COML,COM_VER_RESL,
COM_TEST_NUM,COM_TEST_COML,COM_TEST_RESL,
COM_LOOPB_NUM,COM_LOOPB_COML,COM_LOOPB_RESL,
COM_SYNC_NUM,COM_SYNC_COML,COM_SYNC_RESL,
COM_RCURR_NUM,COM_RCURR_COML,COM_RCURR_RESL,
COM_WCURR_NUM,COM_WCURR_COML,COM_WCURR_RESL,
COM_RMAX_NUM, COM_RMAX_COML,COM_RMAX_RESL,
COM_WMAX_NUM, COM_WMAX_COML,COM_WMAX_RESL,
COM_RSTEP_NUM,COM_RSTEP_COML,COM_RSTEP_RESL,
COM_WSTEP_NUM,COM_WSTEP_COML,COM_WSTEP_RESL,
COM_TIMER_RES_NUM,COM_TIMER_RES_COML,COM_TIMER_RES_RESL,
COM_TIMER_NRES_NUM,COM_TIMER_NRES_COML,COM_TIMER_NRES_RESL,
COM_OPORT_NUM,COM_OPORT_COML,COM_OPORT_RESL,
COM_IPORT_NUM,COM_IPORT_COML,COM_IPORT_RESL,
COM_3RMOVE_AU_NUM,COM_3RMOVE_AU_COML,COM_3RMOVE_AU_RESL,
COM_XRMOVE_AU_NUM,COM_XRMOVE_AU_COML,COM_XRMOVE_AU_RESL,
COM_YRMOVE_AU_NUM,COM_YRMOVE_AU_COML,COM_YRMOVE_AU_RESL,
COM_ZRMOVE_AU_NUM,COM_ZRMOVE_AU_COML,COM_ZRMOVE_AU_RESL,
COM_3RMOVE_A_NUM,COM_3RMOVE_A_COML,COM_3RMOVE_A_RESL,
COM_3RMOVE_NUM,COM_3RMOVE_COML,COM_3RMOVE_RESL,
COM_XRMOVE_NUM,COM_XRMOVE_COML,COM_XRMOVE_RESL,
COM_YRMOVE_NUM,COM_YRMOVE_COML,COM_YRMOVE_RESL,
COM_ZRMOVE_NUM,COM_ZRMOVE_COML,COM_ZRMOVE_RESL,
COM_3RMOVE_U_NUM,COM_3RMOVE_U_COML,COM_3RMOVE_U_RESL,
COM_CARYB_NUM,COM_CARYB_COML,COM_CARYB_RESL,
COM_3AMOVE_AU_NUM,COM_3AMOVE_AU_COML,COM_3AMOVE_AU_RESL,
COM_XAMOVE_AU_NUM,COM_XAMOVE_AU_COML,COM_XAMOVE_AU_RESL,
COM_YAMOVE_AU_NUM,COM_YAMOVE_AU_COML,COM_YAMOVE_AU_RESL,
COM_ZAMOVE_AU_NUM,COM_ZAMOVE_AU_COML,COM_ZAMOVE_AU_RESL,
COM_3AMOVE_A_NUM,COM_3AMOVE_A_COML,COM_3AMOVE_A_RESL,
COM_3AMOVE_NUM,COM_3AMOVE_COML,COM_3AMOVE_RESL,
COM_XAMOVE_NUM,COM_XAMOVE_COML,COM_XAMOVE_RESL,
COM_YAMOVE_NUM,COM_YAMOVE_COML,COM_YAMOVE_RESL,
COM_ZAMOVE_NUM,COM_ZAMOVE_COML,COM_ZAMOVE_RESL,
COM_3AMOVE_U_NUM,COM_3AMOVE_U_COML,COM_3AMOVE_U_RESL,
COM_MOON_NUM,COM_MOON_COML,COM_MOON_RESL,
COM_MOOFF_NUM,COM_MOOFF_COML,COM_MOOFF_RESL,
COM_PTON_NUM,COM_PTON_COML,COM_PTON_RESL,
COM_PTOFF_NUM,COM_PTOFF_COML,COM_PTOFF_RESL,
COM_XON_NUM,COM_XON_COML,COM_XON_RESL,
COM_XOFF_NUM,COM_XOFF_COML,COM_XOFF_RESL,
COM_YON_NUM,COM_YON_COML,COM_YON_RESL,
COM_YOFF_NUM,COM_YOFF_COML,COM_YOFF_RESL,
COM_ZON_NUM,COM_ZON_COML,COM_ZON_RESL,
COM_ZOFF_NUM,COM_ZOFF_COML,COM_ZOFF_RESL,
COM_3ON_NUM,COM_3ON_COML,COM_3ON_RESL,
COM_3OFF_NUM,COM_3OFF_COML,COM_3OFF_RESL,
COM_GON_NUM,COM_GON_COML,COM_GON_RESL,
COM_GOFF_NUM,COM_GOFF_COML,COM_GOFF_RESL,
COM_GIN_NUM,COM_GIN_COML,COM_GIN_RESL,
COM_STOPER, COM_STOPER, COM_STOPER
};
//
// 広域変数
//
// 移動制御用広域変数
// 相対移動目的座標
long Xr_obj, Yr_obj, Zr_obj;
// 移動目的座標
long X_obj, Y_obj, Z_obj;
// 現在座標
long X_cur, Y_cur, Z_cur;
// ＮＭＩワンショットＬＯＷ間隔
UINT Nmi_time;
// NMI最小間隔
UINT Nmi_min;
// NMI最大間隔
UINT Nmi_max;
// NMI最大間隔
UINT Nmi_val;
// 加速、減速ステップ
UINT Step_val;
// 加速減速長
long Acc_len;
// 減速開始長
long Uacc_len;
// バージョン番号
char *Soft_ver = "V0.97";
// エラー出力ポートデータ
UCHAR Err_pdata;
// ＮＭＩ割り込みフラグ
UCHAR Nmi_flag;
// インターバルタイマー
UINT I_count;
// コマンドシーケンスｎｏ
UCHAR Seq_no;
// コマンド長
UCHAR Com_len;
// 応答長
UCHAR Res_len;
// ２面の内どちらのバッファを処理中か示す
// コマンドバッファセレクト
UCHAR Com_buf_sel;
// 主軸とパトライトのＯＮ／ＯＦＦ
UCHAR Motor_sw;
// コマンドバッファは２面用意する。
//
// ｄｄａ＿ｍｏｖｅ中で待ちの時、使用していない
// 面にｆｉｆｏから読み込む。
//
// ｆｉｆｏコマンドバッファ３２バイトｘ２
UCHAR Com_buf[2][COM_BUF_MAX];
// ｆｉｆｏ応答バッファ
UCHAR Res_buf[COM_BUF_MAX];
// ERR_POUTの保存用
UCHAR E_pout;
// デバック用広域
long Temp1;
// Ｉ／Ｏと広域変数の初期化を行う
//
// init:
//
void init(void)
{
UCHAR i;
// Ｉ／Ｏを初期化する
// Ｐ８２５５の初期化
outp(P8255_C, P8255_INIT);
// 主軸、パトライトを停止状態にする
outp(P8255_1, P8255_0_INIT);
// ステッパモータをイネーブルにする
outp(P8255_2, 0x00);
// Ｃ８２４５のモード初期化
outp(C8254_C, C8254_0_MODE); // モード０：ＮＭＩ用
outp(C8254_C, C8254_1_MODE); // モード０：２０ｍＳＥＣ
outp(C8254_C, C8254_2_MODE); // モード０：フリーラン
// ＃１カウンタ２０ｍＳＥＣ設定
outp(C8254_1, (UCHAR)(C8254_1_INIT&C_MASK_LOW));
outp(C8254_1,
(UCHAR)((C8254_1_INIT&C_MASK_HIGH)>>8));
// ERR_POUTの保存用
E_pout = 0;
// ＃０カウンタＮＭＩイネーブル
outp(ERR_POUT, NMI_EN_BIT);
E_pout |= NMI_EN_BIT;
// 広域変数の初期化
I_count = 0;
Com_buf_sel = 0;
Seq_no = 0;
Com_len = 0;
Res_len = 0;
Com_buf_sel = 0;
// 割り込みフラグ
Nmi_flag = 0;
// 最大速度
Nmi_min = NMI_MINVAL;
Nmi_val = NMI_MINVAL;
// 最低速度
Nmi_max = NMI_MAXVAL;
// 加速、減速ステップ数
Step_val = STEP_VAL;
// ３軸現在値初期化
X_cur = 0x0L;
Y_cur = 0x0L;
Z_cur = 0x0L;
// ３軸目的値初期化
X_obj = 0x0L;
Y_obj = 0x0L;
Z_obj = 0x0L;
// ３軸目的値初期化
Xr_obj = 0x0L;
Yr_obj = 0x0L;
Zr_obj = 0x0L;
// バッファの初期化
for (i=0; i<COM_BUF_MAX; i++) {
Com_buf[0][i] = 0;
Com_buf[1][i] = 0;
}
}
///////////////////////////////////////
// dda_amoveの＃ｄｅｆｉｌｅ
#define X_CODE
0x01
#define Y_CODE
0x04
#define Z_CODE
0x10
#define MAX_CODE 0x01
#define MID_CODE 0x04
#define MIN_CODE 0x10
#define CHAR_ALL_1 0xff
#define MINUS 1
#define PLUS 0
#define MAIN_MO 0x80
#define PATO_MO 0x40
#define
X_DIR 0x02
#define
Y_DIR 0x08
#define
Z_DIR 0x20
#define DIR_MASK 0x2a
#define
X_CLK 0x01
#define
Y_CLK 0x04
#define
Z_CLK 0x10
#define CLK_MASK 0x15
#define X_P_LIM 0x02
#define Y_P_LIM 0x08
#define Z_P_LIM 0x20
#define X_M_LIM 0x01
#define Y_M_LIM 0x04
#define Z_M_LIM 0x10
//
// 現在座標から絶対座標目的まで移動する
//
// dda_amove:
//
void dda_amove(UCHAR acc_flag)
{
long x_abs, y_abs, z_abs;
long x_dlt, y_dlt, z_dlt;
long max_val, mid_val, min_val;
long work1_val, work2_val, max_step;
char max_c, mid_c, min_c;
int x_sig, y_sig, z_sig;
UCHAR x_smask, y_smask, z_smask;
UCHAR work_code, out_code, port_dir;
UCHAR port_clk, pdata;
UCHAR limit;
UINT nmi_set;
x_abs = labs(X_obj - X_cur);
y_abs = labs(Y_obj - Y_cur);
z_abs = labs(Z_obj - Z_cur);
x_smask = y_smask = z_smask = CHAR_ALL_1;
x_dlt = X_obj - X_cur;
if (x_dlt < 0)
x_sig = MINUS;
else if (x_dlt > 0)
x_sig = PLUS;
else x_smask = 0;
y_dlt = Y_obj - Y_cur;
if (y_dlt < 0)
y_sig = MINUS;
else if (y_dlt > 0)
y_sig = PLUS;
else y_smask = 0;
z_dlt = Z_obj - Z_cur;
if (z_dlt < 0)
z_sig = MINUS;
else if (z_dlt > 0)
z_sig = PLUS;
else z_smask = 0;
if (x_abs != 0) {
if (y_abs != 0) {
if (z_abs != 0) {
if ((x_abs - y_abs) >= 0) {
/* x_abs > y_abs */
if ((x_abs - z_abs) >= 0) {
max_val = x_abs;
max_c = 'x';
if ((y_abs - z_abs) >= 0) {
mid_val = y_abs;
mid_c = 'y';
min_val = z_abs;
min_c = 'z';
}
else {
mid_val = z_abs;
mid_c = 'z';
min_val = y_abs;
min_c = 'y';
}
}
else {
max_val = z_abs;
max_c = 'z';
if ((y_abs - x_abs) >= 0) {
mid_val = y_abs;
mid_c = 'y';
min_val = x_abs;
min_c = 'x';
}
else {
mid_val = x_abs;
mid_c = 'x';
min_val = y_abs;
min_c = 'y';
}
}
}
else {
/* y_abs > x_abs */
if ((y_abs - z_abs) >= 0) {
/* y_abs > z_abs */
max_val = y_abs;
max_c = 'y';
if ((x_abs - z_abs) >= 0) {
mid_val = x_abs;
mid_c = 'x';
min_val = z_abs;
min_c = 'z';
}
else {
mid_val = z_abs;
mid_c = 'z';
min_val = x_abs;
min_c = 'x';
}
}
else {
/* z_abs > y_abs */
max_val = z_abs;
max_c = 'z';
if ((x_abs - y_abs) >= 0) {
mid_val = x_abs;
mid_c = 'x';
min_val = y_abs;
min_c = 'y';
}
else {
mid_val = y_abs;
mid_c = 'y';
min_val = x_abs;
min_c = 'x';
}
}
}
}
else {
/* z == 0 */
if ((x_abs - y_abs) >= 0) {
max_val = x_abs;
max_c = 'x';
mid_val = y_abs;
mid_c = 'y';
min_val = z_abs;
min_c = 'z';
}
else {
max_val = y_abs;
max_c = 'y';
mid_val = x_abs;
mid_c = 'x';
min_val = z_abs;
min_c = 'z';
}
}
}
/* y == 0 */
else {
if
(z_abs > 0) {
if ((x_abs - z_abs) >= 0) {
max_val = x_abs;
max_c = 'x';
mid_val = z_abs;
mid_c = 'z';
min_val = y_abs;
min_c = 'y';
}
else {
max_val = z_abs;
max_c = 'z';
mid_val = x_abs;
mid_c = 'x';
min_val = y_abs;
min_c = 'y';
}
}
else
{
/* y == 0 and z == 0 */
max_val = x_abs;
max_c = 'x';
mid_val = y_abs;
mid_c = 'y';
min_val = z_abs;
min_c = 'z';
}
}
}
else {
/* x == 0 */
if ((y_abs - z_abs) >= 0)
{
max_val = y_abs;
max_c = 'y';
mid_val = z_abs;
mid_c = 'z';
min_val = x_abs;
min_c = 'x';
}
else {
/* z_abs > y_abs */
max_val = z_abs;
max_c = 'z';
mid_val = y_abs;
mid_c = 'y';
min_val = x_abs;
min_c = 'x';
}
}
max_step = max_val;
work1_val = max_val / 2;
work2_val = work1_val;
// 移動速度は最大
Nmi_val = Nmi_min;
// 加速減速処理があるか
if (acc_flag & ACC_FLAG) {
// _ <-Nmi_min
// /
// /
// /
// _/ <-Nmi_max
// ^
^
// |
|
// acc_step
//
加速
等速度 減速
//
_____________________________
//
/
\
//
_/
\_
//
^
//
|
|
//
ucaa_start_step
// 加速を停止するステップ数をあらかじめ計算して
// そのステップから加速を終了する。
//
(最大NMI時間間隔-最小NMI時間間隔)/Step_val
Acc_len = max_step -
((Nmi_max - Nmi_min)/Step_val);
// 最低速度から
Nmi_val = Nmi_max;
}
if (acc_flag & UACC_FLAG) {
//
減速時:等速からStep_valを加算してNmi_maxまで減速して停止
// Nmi_val -= Step_val;
// 減速開始するステップ数をあらかじめ計算して
// そのステップから減速を始める。
Uacc_len = (Nmi_max -
Nmi_min)/Step_val;
}
// 処理間隔修正用
// カウンタ２に０ｘＦＦＦＦを書く
// outp(C8254_2, (UCHAR)0xff);
// outp(C8254_2, (UCHAR)0xff);
// 最大STEP数処理する
// max_step:
while (max_step) {
// 初期化
out_code = 0;
work_code = 0;
work1_val -= mid_val;
if (work1_val <= 0) {
work1_val += max_val;
work_code |= (MAX_CODE | MID_CODE);
}
else work_code |= MAX_CODE;
work2_val -= min_val;
if (work2_val <= 0) {
work2_val += max_val;
work_code |= MIN_CODE;
}
if (work_code &
MAX_CODE) {
switch (max_c) {
case 'x' : out_code |= ((X_CODE << x_sig) & x_smask);
break;
case 'y' : out_code |= ((Y_CODE << y_sig) & y_smask);
break;
case 'z' : out_code |= ((Z_CODE << z_sig) & z_smask);
}
}
if (work_code &
MID_CODE) {
switch (mid_c) {
case 'x' : out_code |= ((X_CODE << x_sig) & x_smask);
break;
case 'y' : out_code |= ((Y_CODE << y_sig) & y_smask);
break;
case 'z' : out_code |= ((Z_CODE << z_sig) & z_smask);
}
}
if (work_code &
MIN_CODE) {
switch (min_c) {
case 'x' : out_code |= ((X_CODE << x_sig) & x_smask);
break;
case 'y' : out_code |= ((Y_CODE << y_sig) & y_smask);
break;
case 'z' : out_code |= ((Z_CODE << z_sig) & z_smask);
}
}
// クリア
port_dir = 0;
port_clk = 0;
// リミットＳＷの値をえる
limit = inp(P8255_1) &
P8255_1_3L_MASK;
// リミットＳＷは３軸とも無いか？
if (limit == P8255_1_NO_LIM)
{
//
３軸とも無い
if
(out_code & X_CODE) {
//Ｘ軸＋＋
port_dir |= X_DIR;
port_clk |= X_CLK;
}
if
(out_code & (X_CODE << MINUS))
//Ｘ軸−−
port_clk |= X_CLK;
if
(out_code & Y_CODE) {
//Ｙ軸＋＋
port_dir |= Y_DIR;
port_clk |= Y_CLK;
}
if
(out_code & (Y_CODE << MINUS))
//Ｙ軸−−
port_clk |= Y_CLK;
if
(out_code & Z_CODE) {
//Ｚ軸＋＋
port_dir |= Z_DIR;
port_clk |= Z_CLK;
}
if
(out_code & (Z_CODE << MINUS))
//Ｚ軸−−
port_clk |= Z_CLK;
}
else
{
//
３軸のどこかのリミットＳＷがＯＮ
if
(out_code & X_CODE) {
if (limit & P8255_1_X_L1) {
//Ｘ軸＋＋
port_dir |= X_DIR;
port_clk |= X_CLK;
}
}
if
(out_code & (X_CODE << MINUS))
if (limit & P8255_1_X_L2)
//Ｘ軸−−
port_clk |= X_CLK;
if
(out_code & Y_CODE) {
if (limit & P8255_1_Y_L1) {
//Ｙ軸＋＋
port_dir |= Y_DIR;
port_clk |= Y_CLK;
}
}
if
(out_code & (Y_CODE << MINUS))
if (limit & P8255_1_Y_L2)
//Ｙ軸−−
port_clk |= Y_CLK;
if
(out_code & Z_CODE) {
if (limit & P8255_1_Z_L1) {
//Ｚ軸＋＋
port_dir |= Z_DIR;
port_clk |= Z_CLK;
}
}
if
(out_code & (Z_CODE << MINUS))
if (limit & P8255_1_Z_L2)
//Ｚ軸−−
port_clk |= Z_CLK;
}
// モータ制御ポートの値をえる
pdata = inp(P8255_0);
// 回転方向とクロックのビットを０へ
pdata &= ~(DIR_MASK |
CLK_MASK);
// 回転方向をＯＲする
pdata |= (port_dir &
DIR_MASK);
// 回転方向を設定する
outp(P8255_0, pdata);
// 加速／減速を計算して
// Ｎｍｉ＿ｔｉｍｅを計算する
// 処理間隔修正用カウンタ２をラッチ
// outp(C8254_C, 0x80);
// 処理時間を計測する
// delay =
inp(C8254_2);
// delay |= (inp(C8254_2)
<< 8);
// ＮＭＩ割り込み処理間隔を補正する
// nmi_set = Nmi_val -
(0xffff - delay);
nmi_set = Nmi_val;
// 修正したＮＭＩ割り込み間隔を設定する
outp(C8254_0,
(UCHAR)(nmi_set&C_MASK_LOW));
outp(C8254_0,
(UCHAR)((nmi_set&C_MASK_HIGH)>>8));
// 回転させる必要のある軸のクロックを立ち上げる
pdata |= (port_clk &
CLK_MASK);
// ＮＭＩフラグをクリアする
Nmi_flag = NO;
// ＮＭＩをまつ
while (!Nmi_flag);
// 処理間隔補正用カウンタ２に０ｘＦＦＦＦを書く
// outp(C8254_2,
(UCHAR)0xff);
// outp(C8254_2,
(UCHAR)0xff);
// クロックを立ち上げてテーブルを移動させる
outp(P8255_0, pdata);
// 割り込み間隔を計算
// 加速減速があるか
// _ <-Nmi_min
// /
// /
// /
// _/ <-Nmi_max
// ^
^
// |
|
// acc_step
//
加速
等速度 減速
//
_____________________________
//
/
\
//
_/
\_
//
^
//
|
|
//
ucaa_start_step
if (acc_flag & ACC_FLAG)
if
(max_step > Acc_len)
Nmi_val -= Step_val;
if (acc_flag & UACC_FLAG)
if
(max_step < Uacc_len)
Nmi_val += Step_val;
// カウンタを減算
max_step--;
}
// 移動が終了、現在値を目的値に代入
X_cur = X_obj;
Y_cur = Y_obj;
Z_cur = Z_obj;
}
//
// 現在座標から目的の座標まで相対移動する
//
// dda_rmove:
//
void dda_rmove(UCHAR acc_flag)
{
long x_abs, y_abs, z_abs;
//long x_dlt, y_dlt, z_dlt;
long max_val, mid_val, min_val;
long work1_val, work2_val, max_step;
char max_c, mid_c, min_c;
int x_sig, y_sig, z_sig;
UCHAR x_smask, y_smask, z_smask;
UCHAR work_code, out_code, port_dir;
UCHAR port_clk, pdata;
UCHAR limit;
UINT nmi_set;
x_abs = labs(Xr_obj);
y_abs = labs(Yr_obj);
z_abs = labs(Zr_obj);
x_smask = y_smask = z_smask = CHAR_ALL_1;
if (Xr_obj < 0)
x_sig = MINUS;
else if (Xr_obj > 0)
x_sig = PLUS;
else x_smask = 0;
if (Yr_obj < 0)
y_sig = MINUS;
else if (Yr_obj > 0)
y_sig = PLUS;
else y_smask = 0;
if (Zr_obj < 0)
z_sig = MINUS;
else if (Zr_obj > 0)
z_sig = PLUS;
else z_smask = 0;
if (x_abs != 0) {
if (y_abs != 0) {
if (z_abs != 0) {
if ((x_abs - y_abs) >= 0) {
/* x_abs > y_abs */
if ((x_abs - z_abs) >= 0) {
max_val = x_abs;
max_c = 'x';
if ((y_abs - z_abs) >= 0) {
mid_val = y_abs;
mid_c = 'y';
min_val = z_abs;
min_c = 'z';
}
else {
mid_val = z_abs;
mid_c = 'z';
min_val = y_abs;
min_c = 'y';
}
}
else {
max_val = z_abs;
max_c = 'z';
if ((y_abs - x_abs) >= 0) {
mid_val = y_abs;
mid_c = 'y';
min_val = x_abs;
min_c = 'x';
}
else {
mid_val = x_abs;
mid_c = 'x';
min_val = y_abs;
min_c = 'y';
}
}
}
else {
/* y_abs > x_abs */
if ((y_abs - z_abs) >= 0) {
/* y_abs > z_abs */
max_val = y_abs;
max_c = 'y';
if ((x_abs - z_abs) >= 0) {
mid_val = x_abs;
mid_c = 'x';
min_val = z_abs;
min_c = 'z';
}
else {
mid_val = z_abs;
mid_c = 'z';
min_val = x_abs;
min_c = 'x';
}
}
else {
/* z_abs > y_abs */
max_val = z_abs;
max_c = 'z';
if ((x_abs - y_abs) >= 0) {
mid_val = x_abs;
mid_c = 'x';
min_val = y_abs;
min_c = 'y';
}
else {
mid_val = y_abs;
mid_c = 'y';
min_val = x_abs;
min_c = 'x';
}
}
}
}
else {
/* z == 0 */
if ((x_abs - y_abs) >= 0) {
max_val = x_abs;
max_c = 'x';
mid_val = y_abs;
mid_c = 'y';
min_val = z_abs;
min_c = 'z';
}
else {
max_val = y_abs;
max_c = 'y';
mid_val = x_abs;
mid_c = 'x';
min_val = z_abs;
min_c = 'z';
}
}
}
/* y == 0 */
else {
if
(z_abs > 0) {
if ((x_abs - z_abs) >= 0) {
max_val = x_abs;
max_c = 'x';
mid_val = z_abs;
mid_c = 'z';
min_val = y_abs;
min_c = 'y';
}
else {
max_val = z_abs;
max_c = 'z';
mid_val = x_abs;
mid_c = 'x';
min_val = y_abs;
min_c = 'y';
}
}
else
{
/* y == 0 and z == 0 */
max_val = x_abs;
max_c = 'x';
mid_val = y_abs;
mid_c = 'y';
min_val = z_abs;
min_c = 'z';
}
}
}
else {
/* x == 0 */
if ((y_abs - z_abs) >= 0)
{
max_val = y_abs;
max_c = 'y';
mid_val = z_abs;
mid_c = 'z';
min_val = x_abs;
min_c = 'x';
}
else {
/* z_abs > y_abs */
max_val = z_abs;
max_c = 'z';
mid_val = y_abs;
mid_c = 'y';
min_val = x_abs;
min_c = 'x';
}
}
max_step = max_val;
work1_val = max_val / 2;
work2_val = work1_val;
// 移動速度は最大
Nmi_val = Nmi_min;
// 加速減速処理があるか
if (acc_flag & ACC_FLAG) {
// _ <-Nmi_min
// /
// /
// /
// _/ <-Nmi_max
// ^
^
// |
|
// acc_step
//
加速
等速度 減速
//
_____________________________
//
/
\
//
_/
\_
//
^
//
|
|
//
ucaa_start_step
// 加速を停止するステップ数をあらかじめ計算して
// そのステップから加速を終了する。
//
(最大NMI時間間隔-最小NMI時間間隔)/Step_val
Acc_len = max_step -
((Nmi_max - Nmi_min)/Step_val);
// 最低速度から
Nmi_val = Nmi_max;
}
if (acc_flag & UACC_FLAG) {
//
減速時:等速からStep_valを加算してNmi_maxまで減速して停止
// Nmi_val -= Step_val;
// 減速開始するステップ数をあらかじめ計算して
// そのステップから減速を始める。
Uacc_len = (Nmi_max -
Nmi_min)/Step_val;
}
// 処理間隔修正用
// カウンタ２に０ｘＦＦＦＦを書く
// outp(C8254_2, (UCHAR)0xff);
// outp(C8254_2, (UCHAR)0xff);
// 最大STEP数処理する
// max_step:
while (max_step) {
// 初期化
out_code = 0;
work_code = 0;
work1_val -= mid_val;
if (work1_val <= 0) {
work1_val += max_val;
work_code |= (MAX_CODE | MID_CODE);
}
else work_code |= MAX_CODE;
work2_val -= min_val;
if (work2_val <= 0) {
work2_val += max_val;
work_code |= MIN_CODE;
}
if (work_code &
MAX_CODE) {
switch (max_c) {
case 'x' : out_code |= ((X_CODE << x_sig) & x_smask);
break;
case 'y' : out_code |= ((Y_CODE << y_sig) & y_smask);
break;
case 'z' : out_code |= ((Z_CODE << z_sig) & z_smask);
}
}
if (work_code &
MID_CODE) {
switch (mid_c) {
case 'x' : out_code |= ((X_CODE << x_sig) & x_smask);
break;
case 'y' : out_code |= ((Y_CODE << y_sig) & y_smask);
break;
case 'z' : out_code |= ((Z_CODE << z_sig) & z_smask);
}
}
if (work_code &
MIN_CODE) {
switch (min_c) {
case 'x' : out_code |= ((X_CODE << x_sig) & x_smask);
break;
case 'y' : out_code |= ((Y_CODE << y_sig) & y_smask);
break;
case 'z' : out_code |= ((Z_CODE << z_sig) & z_smask);
}
}
// クリア
port_dir = 0;
port_clk = 0;
// リミットＳＷの値をえる
limit = inp(P8255_1) &
P8255_1_3L_MASK;
// リミットＳＷは３軸とも無いか？
if (limit == P8255_1_NO_LIM)
{
//
３軸とも無い
if
(out_code & X_CODE) {
//Ｘ軸＋＋
port_dir |= X_DIR;
port_clk |= X_CLK;
}
if
(out_code & (X_CODE << MINUS))
//Ｘ軸−−
port_clk |= X_CLK;
if
(out_code & Y_CODE) {
//Ｙ軸＋＋
port_dir |= Y_DIR;
port_clk |= Y_CLK;
}
if
(out_code & (Y_CODE << MINUS))
//Ｙ軸−−
port_clk |= Y_CLK;
if
(out_code & Z_CODE) {
//Ｚ軸＋＋
port_dir |= Z_DIR;
port_clk |= Z_CLK;
}
if
(out_code & (Z_CODE << MINUS))
//Ｚ軸−−
port_clk |= Z_CLK;
}
else
{
//
３軸のどこかのリミットＳＷがＯＮ
if
(out_code & X_CODE) {
if (limit & P8255_1_X_L1) {
//Ｘ軸＋＋
port_dir |= X_DIR;
port_clk |= X_CLK;
}
}
if
(out_code & (X_CODE << MINUS))
if (limit & P8255_1_X_L2)
//Ｘ軸−−
port_clk |= X_CLK;
if
(out_code & Y_CODE) {
if (limit & P8255_1_Y_L1) {
//Ｙ軸＋＋
port_dir |= Y_DIR;
port_clk |= Y_CLK;
}
}
if
(out_code & (Y_CODE << MINUS))
if (limit & P8255_1_Y_L2)
//Ｙ軸−−
port_clk |= Y_CLK;
if
(out_code & Z_CODE) {
if (limit & P8255_1_Z_L1) {
//Ｚ軸＋＋
port_dir |= Z_DIR;
port_clk |= Z_CLK;
}
}
if
(out_code & (Z_CODE << MINUS))
if (limit & P8255_1_Z_L2)
//Ｚ軸−−
port_clk |= Z_CLK;
}
// モータ制御ポートの値をえる
pdata = inp(P8255_0);
// 回転方向とクロックのビットを０へ
pdata &= ~(DIR_MASK |
CLK_MASK);
// 回転方向をＯＲする
pdata |= (port_dir &
DIR_MASK);
// 回転方向を設定する
outp(P8255_0, pdata);
// 加速／減速を計算して
// Ｎｍｉ＿ｔｉｍｅを計算する
// 処理間隔修正用カウンタ２をラッチ
// outp(C8254_C, 0x80);
// 処理時間を計測する
// delay =
inp(C8254_2);
// delay |= (inp(C8254_2)
<< 8);
// ＮＭＩ割り込み処理間隔を補正する
// nmi_set = Nmi_val -
(0xffff - delay);
nmi_set = Nmi_val;
// 修正したＮＭＩ割り込み間隔を設定する
outp(C8254_0,
(UCHAR)(nmi_set&C_MASK_LOW));
outp(C8254_0,
(UCHAR)((nmi_set&C_MASK_HIGH)>>8));
// 回転させる必要のある軸のクロックを立ち上げる
pdata |= (port_clk &
CLK_MASK);
// ＮＭＩフラグをクリアする
Nmi_flag = NO;
// ＮＭＩをまつ
while (!Nmi_flag);
// 処理間隔補正用カウンタ２に０ｘＦＦＦＦを書く
// outp(C8254_2,
(UCHAR)0xff);
// outp(C8254_2,
(UCHAR)0xff);
// クロックを立ち上げてテーブルを移動させる
outp(P8255_0, pdata);
// 割り込み間隔を計算
// 加速減速があるか
// _ <-Nmi_min
// /
// /
// /
// _/ <-Nmi_max
// ^
^
// |
|
// acc_step
//
加速
等速度 減速
//
_____________________________
//
/
\
//
_/
\_
//
^
//
|
|
//
ucaa_start_step
if (acc_flag & ACC_FLAG)
if
(max_step > Acc_len)
Nmi_val -= Step_val;
if (acc_flag & UACC_FLAG)
if
(max_step < Uacc_len)
Nmi_val += Step_val;
// カウンタを減算
max_step--;
}
// 移動が終了、現在値を相対移動から計算
X_cur += Xr_obj;
Y_cur += Yr_obj;
Z_cur += Zr_obj;
}
// ＦＩＦＯから１バイトリードする
// read_fifo:
// 戻値:リードデータ
UCHAR read_fifo(void)
{
// データがあるか？
// データがなければ永遠にまつ
while (!(inp(P8255_2) & FIFO_EMP_MASK));
// １バイトリードする
return (inp(FIFO_RD));
}
// ＦＩＦＯへ１バイトライトする
// write_fifo:
// 引数:ライトデータ
void write_fifo(UCHAR data)
{
// ＦＩＦＯはフルか？
// 書けなければ永遠にまつ
while (!(inp(P8255_2) & FIFO_FULL_MASK));
// １バイトライトする
outp(FIFO_WR, data);
}
// エラーを処理する
// trans_err:
// 引数：エラー番号
void trans_err(UCHAR err_no)
{
// エラーをホストに通知する。
// ＣＨＫ＿ＥＲＲ（コントローラエラー、ＨＩＧＨ）
if (err_no == ERROR_SEQ) {
E_pout |= CHK_ERR_BIT;
outp(ERR_POUT, E_pout);
}
// ＬＩＭ＿ＥＲＲ（リミットエラー、ＨＩＧＨ）
else if (err_no == ERROR_SEQ) {
E_pout |= LIM_ERR_BIT;
outp(ERR_POUT, E_pout);
}
}
// コマンド番号からｃｏｍ＿ｄｅｆのｉｎｄｅｘをえる
//
// 引数：コマンド番号
//
// 戻値：ｉｎｄｅｘ
//
UCHAR get_com_no(UCHAR no)
{
UCHAR i;
// ストッパが来るまで
for (i=0; com_def[i].num != COM_STOPER; i++) {
if (no == com_def[i].num)
return (i);
}
}
//
// ｆｉｆｏを読んでコマンドを得る。
// コマンドが完結するまでバッファに
// ｆｉｆｏのデータを書込む。
//
// 戻値：バッファの先頭ポインタ
//
// com_get:
//
UCHAR *com_get(void)
{
UCHAR *ptr, *r_ptr, byte, chk, com_no, len;
// コマンドバッファのバンク？
if (Com_buf_sel) {
// バンク１
ptr = &Com_buf[1][0];
r_ptr = ptr;
}
else {
// バンク０
ptr = &Com_buf[0][0];
r_ptr = ptr;
}
// チェックを初期化
chk = 0;
// 先頭、シーケンスｎｏを得る
byte = read_fifo();
// シーケンスｎｏと等しい？
if (byte != Seq_no)
trans_err(ERROR_SEQ);
// チェック加算
chk += byte;
// シーケンスｎｏをコマンドバッファへ
*ptr++ = byte;
// コマンド先頭を得る
byte = read_fifo();
// チェック加算
chk += byte;
// コマンド先頭をコマンドバッファへ
*ptr++ = byte;
// コマンド番号から、コマンドｄｅｆのＩＮＤＥＸをえる
com_no = get_com_no(byte);
// ＩＮＤＥＸからコマンド長と応答長をえる
Com_len = com_def[com_no].coml;
Res_len = com_def[com_no].resl;
// 残りのコマンド長分
for (len=0; len<Com_len-2; len++) {
// ＦＩＦＯから得る
byte = read_fifo();
// データをコマンドバッファへ
*ptr++ = byte;
// チェック
chk += byte;
}
// チェックは？
if (chk != 0)
trans_err(ERROR_CHK);
// ポインタを戻す
return (r_ptr);
}
//
// 応答データをＦＩＦＯに書く
//
// res_trans:
//
void res_trans(void)
{
UCHAR i, sum, *ptr;
// 応答データバッファの先頭
ptr = &Res_buf[0];
// 初期化
sum = 0;
// ＣＨＫの前まで
for (i=0; i<Res_len-1; i++) {
// データを書く
write_fifo(*ptr);
// ＣＨＫを計算する
sum += *ptr;
ptr++;
}
// ＣＨＫを書く
write_fifo(0x0 - sum);
}
//
// タイマ割り込みのカウンタをクリアする
// cl_timer_res:
//
void cl_timer(void)
{
// 割り込み禁止
di();
// カウンタ値クリア
I_count=0;
// 割り込み許可
ei();
}
//
// タイマー割り込みをｃｏｕｎｔ回数分待つ
// タイマーは２０ｍＳＥＣ
// count:待ち回数
// wait_tick:
//
void wait_tick(UINT count)
{
// カウンタ値クリア
cl_timer();
while (1) {
// 設定回数待つ
if (I_count >= count)
//
抜ける
break;
}
}
// ＜キャリブレート＞：８０
// 動作：Ｘ、Ｙ、Ｚ軸とも原点に移動する。
// 原点は最左、手前、下のリミットＳＷがＯＮの位置
//
// データ数／コマンド総数：０／３
//
// フォーマット：８０
//
// 例：ＳＱ、８０、ＣＨＫ
//
// 応答データ：ＳＱ、８０、ＣＨＫ
// 原点に移動後、応答データを送信
//
// 応答データ数：３
//
// 80:
// com_caryb:
//
void com_caryb(void)
{
UCHAR *res_ptr;
UCHAR limit;
UCHAR pdata;
UCHAR port_dir;
UCHAR port_clk;
// リミットＳＷの状態を調べる
limit = inp(P8255_1) & LIM_MASK;
// リミットＳＷは全てＯＦＦでない？
if (limit != LIM_MASK) {
//
ＯＮのＳＷがあるので逆転させてＯＦＦになるまで移動する
while (1) {
//
クリア
port_dir = 0;
port_clk = 0;
//
ポートの値をえる
limit = inp(P8255_1) & LIM_MASK;
//
Ｘ軸のリミットＳＷ１がＯＮなら
if
(!(limit & LIM_MASK_X1)) {
//Ｘ軸＋＋
port_dir |= X_DIR;
port_clk |= X_CLK;
}
//
Ｘ軸のリミットＳＷ２がＯＮなら
else
if (!(limit & LIM_MASK_X2))
//Ｘ軸−−
port_clk |= X_CLK;
//
Ｙ軸のリミットＳＷ１がＯＮなら
if
(!(limit & LIM_MASK_Y1)) {
//Ｙ軸＋＋
port_dir |= Y_DIR;
port_clk |= Y_CLK;
}
//
Ｙ軸のリミットＳＷ２がＯＮなら
else
if (!(limit & LIM_MASK_Y2))
//Ｙ軸−−
port_clk |= Y_CLK;
//
Ｚ軸のリミットＳＷ１がＯＮなら
if
(!(limit & LIM_MASK_Z1)) {
//Ｚ軸＋＋
port_dir |= Z_DIR;
port_clk |= Z_CLK;
}
//
Ｚ軸のリミットＳＷ２がＯＮなら
else
if (!(limit & LIM_MASK_Z2))
//Ｚ軸−−
port_clk |= Z_CLK;
//
ポートの値をえる
pdata = inp(P8255_0);
//
回転方向とクロックのビットを０へ
pdata &= ~(DIR_MASK | CLK_MASK);
//
回転方向をＯＲする
pdata |= (port_dir & DIR_MASK);
//
回転方向を与える
outp(P8255_0, pdata);
//
移動する必要があるか？
if
(!port_clk)
// 抜ける
break;
//
回転させる必要のある軸のクロックを立ち上げる
pdata |= (port_clk & CLK_MASK);
//
クロックを立ち上げてテーブルを移動させる
outp(P8255_0, pdata);
//
移動待ち
wait_tick(CARYB_TIMER);
}
}
// ３軸のリミットＳＷがＯＮになるまで移動する
// 移動方向は最左、最前、最下のリミットＳＷがＯＮになるまで
while (1) {
// クリア
port_dir = 0;
port_clk = 0;
// リミットＳＷを読む
limit = inp(P8255_1) &
LIM_MASK;
// Ｘ軸のリミットＳＷがＯＦＦなら
if (limit & LIM_MASK_X1)
{
//Ｘ軸−−
port_clk |= X_CLK;
}
// Ｙ軸のリミットＳＷがＯＦＦなら
if (limit & LIM_MASK_Y1)
{
//Ｙ軸−−
port_clk |= Y_CLK;
}
// Ｚ軸のリミットＳＷがＯＦＦなら
if (limit & LIM_MASK_Z1)
{
//Ｚ軸−−
port_clk |= Z_CLK;
}
// ポートの値をえる
pdata = inp(P8255_0);
// 回転方向とクロックのビットを０へ
pdata &= ~(DIR_MASK |
CLK_MASK);
// 回転方向をＯＲする
pdata |= (port_dir &
DIR_MASK);
// 回転方向を与える
outp(P8255_0, pdata);
// 移動する必要があるか？
if (!port_clk)
//
抜ける
break;
// 回転させる必要のある軸のクロックを立ち上げる
pdata |= (port_clk &
CLK_MASK);
// クロックを立ち上げてテーブルを移動させる
outp(P8255_0, pdata);
// 移動待ち
wait_tick(CARYB_TIMER);
}
// 応答データのポインタを得る
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr = COM_CARYB_NUM;
// 応答データを書く
res_trans();
}
// ＜絶対ステップ加速３軸移動コマンド＞：８１
// 動作：指定された座標に速度０から加速し最高速度で移動、減速停止する
//
// データ数／コマンド総数：９／１２
//
// フォーマット：８１、Ｘｈ、Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、８１、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 81:
// com_3move_au:
//
void com_3amove_au(void)
{
UCHAR *com_ptr;
long data;
// 広域パラメータ:
// Nmi_min:最小間隔
// Nmi_max:最大間隔
// Step_val:ステップ数
// _ <-Nmi_min
// /
// /
// /
// _/ <-Nmi_max
// ^ ^
// | |
// Acc_len
// 加速、減速に何ステップかかるか
// (最大NMI時間間隔-最小NMI時間間隔)/Step_val
// Acc_len = (Nmi_max - Mni_min)/Step_val;
//
加速
等速度 減速
// _____________________________
//
/
\
//
_/
\_
//
^
//
|
|
//
Ucaa_len
// 減速開始するステップ数をあらかじめ計算して
// そのステップから減速を始める。
// 最大移動ステップから減速ステップを減算すれは
// 減速するステップが計算できる
// Uacc_len = (max_step - Acc_len);
// 総移動ステップが加速減速数よりも小さかった場合の処理?
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
X_obj = data;
// Ｙ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
Y_obj = data;
// Ｚ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= *com_ptr;
Z_obj = data;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_amove(ACC_FLAG | UACC_FLAG);
}
// ＜絶対ステップ加速Ｘ軸移動コマンド＞：８２
// 動作：指定されたＸ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：８２、Ｘｈ、Ｘｍ、Ｘｌ
//
// 例：ＳＱ、８２、００、００、１０、ＣＨＫ
//
// 応答データ：無し
//
// 82:
// com_xamove_au:
//
void com_xamove_au()
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr);
X_obj = data;
Y_obj = Y_cur;
Z_obj = Z_cur;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_amove(ACC_FLAG | UACC_FLAG);
}
// ＜絶対ステップ加速Ｙ軸移動コマンド＞：８３
// 動作：指定されたＹ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：８３、Ｙｈ、Ｙｍ、Ｙｌ
//
// 例：
// ＳＱ、８３、００、００、２０、ＣＨＫ
//
// 応答データ：無し
//
// 83:
// com_yamove_au:
//
void com_yamove_au()
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｙ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr);
Y_obj = data;
X_obj = X_cur;
Z_obj = Z_cur;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_amove(ACC_FLAG | UACC_FLAG);
}
// ＜絶対ステップ加速Ｚ軸移動コマンド＞：８４
// 動作：指定されたＺ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：８４、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：
// ＳＱ、８１、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 84:
// com_zamove_au:
//
void com_zamove_au()
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｚ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr);
Z_obj = data;
X_obj = X_cur;
Y_obj = Y_cur;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_amove(ACC_FLAG | UACC_FLAG);
}
// ＜絶対ステップ加速３軸移動コマンド＞：８５
// 動作：現在の位置から指定された座標に速度０からステップで加速しながら最高速度で移動する
//
// データ数／コマンド総数：９／１２
//
// フォーマット：８５、Ｘｈ、Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、８５、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 85:
// com_3amove_a:
void com_3amove_a(void)
{
UCHAR *com_ptr;
long data;
// 広域パラメータ:
// Nmi_min:最小間隔
// Nmi_max:最大間隔
// Step_val:ステップ数
// _ <-Nmi_min
// /
// /
// /
// _/ <-Nmi_max
// ^ ^
// | |
// Acc_len
// 加速、減速に何ステップかかるか
// (最大NMI時間間隔-最小NMI時間間隔)/Step_val
// Acc_len = (Nmi_max - Mni_min)/Step_val;
//
加速
等速度 減速
// _____________________________
//
/
\
//
_/
\_
//
^
//
|
|
//
Ucaa_len
// 減速開始するステップ数をあらかじめ計算して
// そのステップから減速を始める。
// 最大移動ステップから減速ステップを減算すれは
// 減速するステップが計算できる
// Uacc_len = (max_step - Acc_len);
// 総移動ステップが加速減速数よりも小さかった場合の処理?
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
X_obj = data;
// Ｙ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
Y_obj = data;
// Ｚ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= *com_ptr;
Z_obj = data;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_amove(ACC_FLAG);
}
// ＜絶対定速度３軸移動コマンド＞：８６
// 動作：現在の位置から指定された座標に現在の速度で移動する
//
// データ数／コマンド総数：９／１２
//
// フォーマット：８６、Ｘｈ、Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、８６、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 86:
// com_3move:
//
void com_3amove(void)
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
X_obj = data;
// Ｙ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
Y_obj = data;
// Ｚ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= *com_ptr;
Z_obj = data;
// 指定された位置に移動する（定速）
dda_amove(NO_ACC_FLAG);
}
// ＜絶対定速度Ｘ軸移動コマンド＞：８７
//
// 動作：現在の位置から指定されたＸ座標に現在の速度で移動する
//
// データ数／コマンド総数：３／６
//
// フォーマット：Ｘｈ、Ｘｍ、Ｘｌ
//
// 例：ＳＱ、８７、００、００、１０、ＣＨＫ
//
// 応答データ：無し
//
// 87:
// com_xmove:
//
void com_xamove(void)
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= *com_ptr;
X_obj = data;
Y_obj = Y_cur;
Z_obj = Z_cur;
// 指定された位置に移動する（定速）
dda_amove(NO_ACC_FLAG);
}
// ＜絶対定速度Ｙ軸移動コマンド＞：８８
//
// 動作：現在の位置から指定されたＹ座標に現在の速度で移動する
//
// データ数／コマンド総数：３／６
//
// フォーマット：Ｙｈ、Ｙｍ、Ｙｌ
//
// 例：
// ＳＱ、８８、００、００、２０、ＣＨＫ
//
// 応答データ：無し
//
// 88:
// com_ymove:
//
void com_yamove(void)
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｙ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= *com_ptr;
Y_obj = data;
X_obj = X_cur;
Z_obj = Z_cur;
// 指定された位置に移動する（定速）
dda_amove(NO_ACC_FLAG);
}
// ＜絶対定速度Ｚ軸移動コマンド＞：８９
//
// 動作：現在の位置から指定されたＺ座標に現在の速度で移動する
//
// データ数／コマンド総数：３／６
//
// フォーマット：８９、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：
// ＳＱ、８９、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 89:
// com_zamove:
//
void com_zamove(void)
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｚ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= *com_ptr;
Z_obj = data;
X_obj = X_cur;
Y_obj = Y_cur;
// 指定された位置に移動する（定速）
dda_amove(NO_ACC_FLAG);
}
// ＜絶対ステップ減速３軸移動コマンド＞：８ａ
//
// 動作：指定された座標に現在の速度からステップで減速しながら移動し停止する
//
// データ数／コマンド総数：９／１２
//
// フォーマット：８ａ、Ｘｈ、Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、８ａ、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 8a:
// com_3amove_u:
void com_3amove_u(void)
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
X_obj = data;
// Ｙ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
Y_obj = data;
// Ｚ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= *com_ptr;
Z_obj = data;
// 指定された位置に移動する（減速）
dda_amove(UACC_FLAG);
}
//
// ＜相対ステップ加速３軸移動コマンド＞：７１
// 動作：指定された座標に速度０から加速し最高速度で移動、減速停止する
//
// データ数／コマンド総数：６／９
//
// フォーマット：７１、Ｘｈ、Ｘｌ、Ｙｈ、Ｙｌ、Ｚｈ、Ｚｌ
//
// 例：ＳＱ、７１、００、１０、００、２０、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 71:
// com_3rmove_au:
//
void com_3rmove_au(void)
{
UCHAR *com_ptr;
short int data;
// 広域パラメータ:
// Nmi_min:最小間隔
// Nmi_max:最大間隔
// Step_val:ステップ数
// _ <-Nmi_min
// /
// /
// /
// _/ <-Nmi_max
// ^ ^
// | |
// Acc_len
// 加速、減速に何ステップかかるか
// (最大NMI時間間隔-最小NMI時間間隔)/Step_val
// Acc_len = (Nmi_max - Mni_min)/Step_val;
//
加速
等速度 減速
// _____________________________
//
/
\
//
_/
\_
//
^
//
|
|
//
Ucaa_len
// 減速開始するステップ数をあらかじめ計算して
// そのステップから減速を始める。
// 最大移動ステップから減速ステップを減算すれは
// 減速するステップが計算できる
// Uacc_len = (max_step - Acc_len);
// 総移動ステップが加速減速数よりも小さかった場合の処理?
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr++);
Xr_obj = (long)data;
// Ｙ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr++);
Yr_obj = (long)data;
// Ｚ軸
data = ((*com_ptr++) << 8);
data |= *com_ptr;
Zr_obj = (long)data;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_rmove(ACC_FLAG | UACC_FLAG);
}
// ＜相対ステップ加速Ｘ軸移動コマンド＞：７２
// 動作：指定されたＸ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：７２、Ｘｈ，Ｘｍ、Ｘｌ
//
// 例：ＳＱ、７２、００、００、１０、ＣＨＫ
//
// 応答データ：無し
//
// 72:
// com_xrmove_au:
//
void com_xrmove_au()
{
UCHAR *com_ptr;
short int data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr);
Xr_obj = (long)data;
Yr_obj = 0L;
Zr_obj = 0L;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_rmove(ACC_FLAG | UACC_FLAG);
}
// ＜相対ステップ加速Ｙ軸移動コマンド＞：７３
// 動作：指定されたＹ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：７３、Ｙｈ，Ｙｍ、Ｙｌ
//
// 例：
// ＳＱ、７３、００、００、２０、ＣＨＫ
//
// 応答データ：無し
//
// 73:
// com_yrmove_au:
//
void com_yrmove_au()
{
UCHAR *com_ptr;
short int data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｙ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr);
Yr_obj = (long)data;
Xr_obj = 0L;
Zr_obj = 0L;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_rmove(ACC_FLAG | UACC_FLAG);
}
// ＜相対ステップ加速Ｚ軸移動コマンド＞：７４
// 動作：指定されたＺ座標に速度０からステップで加速しながら最高速度で移動、減速停止する
//
// データ数／コマンド総数：３／６
//
// フォーマット：７４、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：
// ＳＱ、７１、００、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 74:
// com_zrmove_au:
//
void com_zrmove_au()
{
UCHAR *com_ptr;
short int data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｚ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr);
Zr_obj = (long)data;
Xr_obj = 0L;
Yr_obj = 0L;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_rmove(ACC_FLAG | UACC_FLAG);
}
// ＜相対ステップ加速３軸移動コマンド＞：７５
// 動作：現在の位置から指定された座標に速度０からステップで加速しながら最高速度で移動する
//
// データ数／コマンド総数：６／９
//
// フォーマット：７５、Ｘｈ、Ｘｌ、Ｙｈ、Ｙｌ、Ｚｈ、Ｚｌ
//
// 例：ＳＱ、７５、００、１０、００、２０、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 75:
// com_3rmove_a:
void com_3rmove_a(void)
{
UCHAR *com_ptr;
short int data;
// 広域パラメータ:
// Nmi_min:最小間隔
// Nmi_max:最大間隔
// Step_val:ステップ数
// _ <-Nmi_min
// /
// /
// /
// _/ <-Nmi_max
// ^ ^
// | |
// Acc_len
// 加速、減速に何ステップかかるか
// (最大NMI時間間隔-最小NMI時間間隔)/Step_val
// Acc_len = (Nmi_max - Mni_min)/Step_val;
//
加速
等速度 減速
// _____________________________
//
/
\
//
_/
\_
//
^
//
|
|
//
Ucaa_len
// 減速開始するステップ数をあらかじめ計算して
// そのステップから減速を始める。
// 最大移動ステップから減速ステップを減算すれは
// 減速するステップが計算できる
// Uacc_len = (max_step - Acc_len);
// 総移動ステップが加速減速数よりも小さかった場合の処理?
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr++);
Xr_obj = (long)data;
// Ｙ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr++);
Yr_obj = (long)data;
// Ｚ軸
data = ((*com_ptr++) << 8);
data |= *com_ptr;
Zr_obj = (long)data;
// 最低速度から（実験用、実際には最適値を求める）
Nmi_max = NMI_MAXVAL;
// 指定された位置に移動する（加速、減速あり）
dda_rmove(ACC_FLAG);
}
// ＜相対定速度３軸移動コマンド＞：７６
// 動作：現在の位置から指定された座標に現在の速度で移動する
//
// データ数／コマンド総数：６／９
//
// フォーマット：７６、Ｘｈ、Ｘｌ、Ｙｈ、Ｙｌ、Ｚｈ、Ｚｌ
//
// 例：ＳＱ、７６、００、１０、００、２０、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 76:
// com_3rmove:
//
void com_3rmove(void)
{
UCHAR *com_ptr;
short int data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr++);
Xr_obj = (long)data;
// Ｙ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr++);
Yr_obj = (long)data;
// Ｚ軸
data = ((*com_ptr++) << 8);
data |= *com_ptr;
Zr_obj = (long)data;
// 指定された位置に移動する（定速）
dda_rmove(NO_ACC_FLAG);
}
// ＜相対定速度Ｘ軸移動コマンド＞：７７
//
// 動作：現在の位置から指定されたＸ座標に現在の速度で移動する
//
// データ数／コマンド総数：２／５
//
// フォーマット：Ｘｈ、Ｘｌ
//
// 例：ＳＱ、７７、００、１０、ＣＨＫ
//
// 応答データ：無し
//
// 77:
// com_xrmove:
//
void com_xrmove(void)
{
UCHAR *com_ptr;
short int data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 8);
data |= *com_ptr;
Xr_obj = (long)data;
Yr_obj = 0L;
Zr_obj = 0L;
// 指定された位置に移動する（定速）
dda_rmove(NO_ACC_FLAG);
}
// ＜相対定速度Ｙ軸移動コマンド＞：７７
//
// 動作：現在の位置から指定されたＹ座標に現在の速度で移動する
//
// データ数／コマンド総数：２／５
//
// フォーマット：Ｙｈ、Ｙｌ
//
// 例：
// ＳＱ、７８、００、２０、ＣＨＫ
//
// 応答データ：無し
//
// 78:
// com_yrmove:
//
void com_yrmove(void)
{
UCHAR *com_ptr;
short int data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｙ軸
data = ((*com_ptr++) << 8);
data |= *com_ptr;
Yr_obj = (long)data;
Xr_obj = 0L;
Zr_obj = 0L;
// 指定された位置に移動する（定速）
dda_rmove(NO_ACC_FLAG);
}
*// ＜相対定速度Ｚ軸移動コマンド＞：７９
//
// 動作：現在の位置から指定されたＺ座標に現在の速度で移動する
//
// データ数／コマンド総数：２／５
//
// フォーマット：７９、Ｚｈ、Ｚｌ
//
// 例：
// ＳＱ、７９、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 79:
// com_zrmove:
//
void com_zrmove(void)
{
UCHAR *com_ptr;
short int data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｚ軸
data = ((*com_ptr++) << 8);
data |= *com_ptr;
Zr_obj = (long)data;
Xr_obj = 0L;
Yr_obj = 0L;
// 指定された位置に移動する（定速）
dda_rmove(NO_ACC_FLAG);
}
// ＜相対ステップ減速３軸移動コマンド＞：７ａ
//
// 動作：指定された座標に現在の速度からステップで減速しながら移動し停止する
//
// データ数／コマンド総数：６／９
//
// フォーマット：７ａ、Ｘｈ、Ｘｌ、Ｙｈ、Ｙｌ、Ｚｈ、Ｚｌ
//
// 例：ＳＱ、７ａ、００、１０、００、２０、００、３０、ＣＨＫ
//
// 応答データ：無し
//
// 7a:
// com_3rmove_u:
void com_3rmove_u(void)
{
UCHAR *com_ptr;
short int data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr++);
Xr_obj = (long)data;
// Ｙ軸
data = ((*com_ptr++) << 8);
data |= (*com_ptr++);
Yr_obj = (long)data;
// Ｚ軸
data = ((*com_ptr++) << 8);
data |= *com_ptr;
Zr_obj = (long)data;
// 指定された位置に移動する（減速）
dda_rmove(UACC_FLAG);
}
// ＜ダイレクトポート入力＞：５１
// 動作：ポート（Ｐ０、Ｐ１、Ｐ２、ＥＲＲ＿ＰＯＵＴ）から直接データを入力する
//
// データ数／コマンド総数：０／３
//
// フォーマット：５１
//
// 例：ＳＱ、５１、ＣＨＫ
//
// 応答データ数：ＳＱ、５１、Ｐ０、Ｐ１、Ｐ２、ＥＲＲ＿ＰＯＵＴ、ＣＨＫ
//
// com_iport:
//
void com_iport(void)
{
UCHAR *res_ptr, data;
// 応答データのポインタを得る
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr++ = COM_IPORT_NUM;
// ポート０を読む
data = inp(P8255_0);
*res_ptr++ = data;
// ポート１を読む
data = inp(P8255_1);
*res_ptr++ = data;
// ポート２を読む
data = inp(P8255_2);
*res_ptr++ = data;
// ＥＲＲ＿ＰＯＵＴを読む
*res_ptr = E_pout;
// 応答データを書く
res_trans();
}
// ＜ダイレクトポート出力＞：５０
// 動作：出力ポート（Ｐ０、Ｐ２Ｈ）に直接データを出力する
// 出力ポート
// Ｐ０：出力ポート／ステッパモータクロック、回転方向、主軸、パトライト
// Ｐ２上位：イネーブルポート
// ＥＲＲ＿ＰＯＵＴ：ＣＨＫ、ＬＩＭエラー、ＮＭＩ＿ＥＮ
//
// データ数／コマンド総数：６／９
//
// フォーマット：５０、Ｐ０、ＭＡＳＫ、Ｐ２Ｈ、ＭＡＳＫ、ＥＲＲ＿ＰＯＵＴ、ＭＡＳＫ
//
// 例：ＳＱ、５０、５５、ｆ０、ａａ、０ｆ、ｆ０、ｆ０、ＣＨＫ
//
// 応答データ数：無し
//
// com_oport:
//
void com_oport(void)
{
UCHAR *com_ptr, idata, odata, mask;
// データのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｐ０のデータを得る
odata = *com_ptr++;
mask = *com_ptr++;
if (mask != 0xff) {
// Ｐ０を読む
idata = inp(P8255_0);
// １の部分を残す
idata &= mask;
// ＯＲする
idata |= odata;
outp(P8255_0, idata);
}
// Ｐ２のデータを得る
odata = *com_ptr++;
mask = *com_ptr++;
if (mask != 0xff) {
// Ｐ２を読む
idata = inp(P8255_2);
// １の部分を残す
idata &= mask;
// ＯＲする
idata |= odata;
outp(P8255_2, idata);
}
// ＥＲＲ＿ＰＯＵＴのデータを得る
odata = *com_ptr++;
mask = *com_ptr;
if (mask != 0xff) {
// ＥＲＲ＿ＰＯＵＴを読む
idata = E_pout;
// １の部分を残す
idata &= mask;
// ＯＲする
idata |= odata;
outp(ERR_POUT, idata);
}
}
// ＜タイマー＞：４８
// 動作：指定したタイマーを待つ（応答データ無し）
//
// データ数／コマンド総数：２／５
//
// フォーマット：４７、Ｔｈ，Ｔｌ
//
// 例：ＳＱ、４７、５５、ａａ、ＣＨＫ
//
// 応答データ数：無し
//
void com_timer_nres(void)
{
UCHAR *com_ptr;
UINT count;
// コマンドのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// タイマー値をえる
count = ((*com_ptr++) << 8);
count |= (*com_ptr++);
// 待つ
wait_tick(count);
}
// ＜タイマー＞：４７
// 動作：指定したタイマーを待って応答データを返す
//
// データ数／コマンド総数：２／５
//
// フォーマット：４７、Ｔｈ，Ｔｌ
//
// 例：ＳＱ、４７、５５、ａａ、ＣＨＫ
//
// 応答データ数：ＳＱ、４７、ＣＨＫ
//
void com_timer_res(void)
{
UCHAR *com_ptr, *res_ptr;
UINT count;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// タイマー値をえる
count = ((*com_ptr++) << 8);
count |= (*com_ptr++);
// 応答データのポインタを得る
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr = COM_TIMER_RES_NUM;
// 待つ
wait_tick(count);
// 応答データを書く
res_trans();
}
// ＜ステップ数登録＞：４６
// 動作：ステップ数を登録する
//
// データ数／コマンド総数：２／５
//
// フォーマット：４６、STEP_h，STEP_L
//
// 例：ＳＱ、４６、１２、３４、ＣＨＫ
//
// 応答データ数：無し
//
void com_wstep(void)
{
UCHAR *com_ptr;
UINT data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
data = ((*com_ptr++) << 8);
data |= (*com_ptr);
Step_val = data;
}
// ＜ステップ数通知＞：４５
// 動作：ステップ数を通知する
//
// データ数／コマンド総数：０／３
//
// フォーマット：４５
//
// 例：ＳＱ、４５、ＣＨＫ
//
// 応答データ数：ＳＱ、４５、STEP_H, STEP_L,ＣＨＫ
//
void com_rstep(void)
{
UCHAR *res_ptr;
// 応答データのポインタを得る
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr++ = COM_RSTEP_NUM;
*res_ptr++ = (UCHAR)((Step_val & LONG_MID_MASK)
>> 8);
*res_ptr = (UCHAR)(Step_val & LONG_LOW_MASK);
// 応答データを書く
res_trans();
}
// ＜最高速度登録＞：４４
// 動作：移動最高速度を登録する
//
// データ数／コマンド総数：２／５
//
// フォーマット：４４
//
// 例：ＳＱ、４４、ＣＨＫ
//
// 応答データ数：無し
//
// com_wmax:
//
void com_wmax(void)
{
UCHAR *com_ptr;
UINT data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
data = ((*com_ptr++) << 8);
data |= (*com_ptr);
Nmi_min = data;
Nmi_val = data;
}
// ＜最高速度通知＞：４３
// 動作：最高速度を通知する
//
// データ数／コマンド総数：０／３
//
// フォーマット：４３
//
// 例：ＳＱ、４３、ＣＨＫ
//
// 応答データ数：ＳＱ、４３、MAX_H, MAX_L,ＣＨＫ
//
// com_rmax:
//
void com_rmax(void)
{
UCHAR *res_ptr;
// 応答データのポインタを得る
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr++ = COM_RMAX_NUM;
*res_ptr++ = (UCHAR)((Nmi_min & INT_HIGH_MASK)
>> 8);
*res_ptr = (UCHAR)(Nmi_min & INT_LOW_MASK);
// 応答データを書く
res_trans();
}
// ＜現在位置を登録する＞：４２
// 動作：現在の位置を登録する
//
// データ数／コマンド総数：１０／１３
//
// フォーマット：４２、Ｘｈ，Ｘｍ、Ｘｌ、Ｙｈ、Ｙｍ、Ｙｌ、Ｚｈ、Ｚｍ、Ｚｌ
//
// 例：ＳＱ、４２、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ数：なし
//
// com_wcurr:
//
void com_wcurr(void)
{
UCHAR *com_ptr;
long data;
// カレント座標のデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// Ｘ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
X_cur = data;
// Ｙ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr++);
Y_cur = data;
// Ｚ軸
data = ((*com_ptr++) << 16);
data |= ((*com_ptr++) << 8);
data |= (*com_ptr);
Z_cur = data;
}
// ＜現在位置を通知する＞：４１
// 動作：現在の位置を通知する
//
// データ数／コマンド総数：０／３
//
// フォーマット：４１
//
// 例：ＳＱ、４１、ＣＨＫ
//
// 応答データ：ＳＱ、４１、００、００、１０、００、００、２０、００、００、３０、ＣＨＫ
//
// 応答データ数：１２
//
// com_rcurr:
//
void com_rcurr()
{
UCHAR *res_ptr;
// 応答データのポインタを得る
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr++ = COM_RCURR_NUM;
// Ｘ軸
*res_ptr++ = (UCHAR)((X_cur & LONG_HIGH_MASK)
>> 16);
*res_ptr++ = (UCHAR)((X_cur & LONG_MID_MASK)
>> 8);
*res_ptr++ = (UCHAR)(X_cur & LONG_LOW_MASK);
// Ｙ軸
*res_ptr++ = (UCHAR)((Y_cur & LONG_HIGH_MASK)
>> 16);
*res_ptr++ = (UCHAR)((Y_cur & LONG_MID_MASK)
>> 8);
*res_ptr++ = (UCHAR)(Y_cur & LONG_LOW_MASK);
// Ｚ軸
*res_ptr++ = (UCHAR)((Z_cur & LONG_HIGH_MASK)
>> 16);
*res_ptr++ = (UCHAR)((Z_cur & LONG_MID_MASK)
>> 8);
*res_ptr = (UCHAR)(Z_cur &
LONG_LOW_MASK);
// 応答データを書く
res_trans();
}
// ＜シンク＞：４０
// 動作：移動の同期を取るために、このシーケンスの１つ前のコマンドの終了時にホストに応答する。
//
// データ数／コマンド総数：０／３
//
// フォーマット：４０
//
// 例：ＳＱ、４０、ＣＨＫ
//
// 応答データ：ＳＱ、４０、ＣＨＫ
//
// 応答データ数：３
//
// com_sync:
//
void com_sync(void)
{
UCHAR *res_ptr;
// １つ前のコマンドが終了したら応答データを返す
// 前のコマンドが終了するとこのＳＹＮＣコマンドが
// 実行されるので特別な処理は必要ない。
// 応答バッファ先頭
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr = COM_SYNC_NUM;
// 応答データを書く
res_trans();
}
// ＜ループバック＞：２０
// 動作：入力されたデータを４バイトそのまま返す。
//
// データ数／コマンド総数：４／７
//
// フォーマット：２０、Ｄ０、Ｄ１、Ｄ２、Ｄ３
//
// 例：ＳＱ、２０、００、５５、ａａ、ｆｆ、ＣＨＫ
//
// 応答データ：ＳＱ、２０、００、５５、ａａ、ｆｆ、ＣＨＫ
//
// 応答データ数：７
//
// com_loopb:
//
void com_loopb(void)
{
UCHAR *res_ptr, *com_ptr, i;
// 応答バッファ先頭
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr++ = COM_LOOPB_NUM;
// ループバックデータのポインタを得る
com_ptr = &Com_buf[Com_buf_sel][2];
// ループバックデータコピーする
for (i=0; i<=3; i++)
*res_ptr++ = *com_ptr++;
// 応答データを書く
res_trans();
}
// ＜テスト＞：１２
//
// 動作：テストする
//
// データ数／コマンド総数：０／３
//
// 例：ＳＱ、１２、ＣＨＫ
//
// 応答データ：ＳＱ、１２、ＣＨＫ
//
// 応答データ数：３
//
// com_test:
//
// 12:
//
void com_test(void)
{
UCHAR *res_ptr;
UCHAR i;
X_obj = 0L;
Y_obj = 0L;
X_cur = 0L;
Y_cur = 0L;
Z_cur = 0L;
for (i=0; i<10; i++) {
X_obj = 500L;
Y_obj = 500L;
Z_obj = 500L;
// 指定された位置に移動する（加速、減速あり）
dda_amove(ACC_FLAG |
UACC_FLAG);
X_obj = 0L;
Y_obj = 0L;
Z_obj = 0L;
// 指定された位置に移動する（加速、減速あり）
dda_amove(ACC_FLAG |
UACC_FLAG);
}
// 応答バッファ先頭
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr = COM_TEST_NUM;
// 応答データを書く
res_trans();
}
// ＜ソフトウェアバージョン通知＞：１１
//
// 動作：ソフトウェアのバージョンを返す
//
// データ数／コマンド総数：０／３
//
// 例：ＳＱ、１１、ＣＨＫ
//
// 応答データ：ＳＱ、１１、”Ｖ１．００”、ＣＨＫ
//
// 応答データ数：８
//
// com_ver:
//
void com_ver(void)
{
UCHAR *res_ptr, *soft_ptr;
// 応答バッファ先頭
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr++ = COM_VER_NUM;
// ポインタを得る
soft_ptr = Soft_ver;
// バージョン番号をコピーする
while (*soft_ptr != '\0')
*res_ptr++ = *soft_ptr++;
// 応答データを書く
res_trans();
}
// ＜自己診断＞：１０
// 動作：自己診断を行ない結果を返す
//
// データ数／コマンド総数：０／３
//
// 例：ＳＱ、１０、ＣＨＫ
//
// 応答データ：ＳＱ、１０、Ｒｅｓ、ＣＨＫ
// （Ｒｅｓ：正常＝１、異常＝０）
//
// 応答データ数：４
//
void com_diag(void)
{
UCHAR i, err, *res_ptr;
err = NO;
// コマンドバッファをテストする
for (i=0; i<COM_BUF_MAX; i++) {
Com_buf[0][i] = i;
Com_buf[1][i] = 0xff - i;
Res_buf[i] = i;
}
for (i=0; i<COM_BUF_MAX; i++) {
if (Com_buf[0][i] != i) {
err
= YES;
break;
}
if (Com_buf[1][i] != (0xff -
i)) {
err
= YES;
break;
}
if (Res_buf[i] != i) {
err
= YES;
break;
}
}
// 応答バッファ先頭
res_ptr = &Res_buf[0];
*res_ptr++ = Seq_no;
*res_ptr++ = COM_DIAG_NUM;
if (err == YES)
// エラー
*res_ptr = ERROR;
else
// 正常
*res_ptr = OK;
// 応答データを書く
res_trans();
}
//
// ＜リセット＞：００
// 動作：コントローラをリセットする
//
// データ数／コマンド総数：０／３
//
// 例：ＳＱ、００、ＣＨＫ
//
// 応答データ：無し
//
// com_reset:
//
void com_reset(void)
{
// リセット番地へ飛ぶ
START();
}
// ＜主軸モータ開始コマンド＞：９０
//
// 動作：主軸モータを回転させる。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９０
//
// 例：ＳＱ、９０、ＣＨＫ
//
// 応答データ：無し
//
// com_moon:
//
void com_moon(void)
{
UCHAR data;
// ポートを読む
data = inp(P8255_0);
// ONに
data |= P8255_MOON_BIT;
outp(P8255_0, data);
}
// ＜主軸モータ停止コマンド＞：９１
//
// 動作：主軸モータを回転させる。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９１
//
// 例：ＳＱ、９１、ＣＨＫ
//
// 応答データ：無し
//
// com_mooff:
//
void com_mooff(void)
{
UCHAR data;
// ポートを読む
data = inp(P8255_0);
// OFF
data &= ~(P8255_MOON_BIT);
outp(P8255_0, data);
}
// ＜警告回転灯開始コマンド＞：９２
//
// 動作：警告回転灯をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９２
//
// 例：ＳＱ、９２、ＣＨＫ
//
// 応答データ：無し
//
// com_pton:
//
void com_pton(void)
{
UCHAR data;
// ポートを読む
data = inp(P8255_0);
// ONに
data |= P8255_PTON_BIT;
outp(P8255_0, data);
}
// ＜警告回転灯停止コマンド＞：９３
//
// 動作：警告回転灯をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９３
//
// 例：ＳＱ、９３、ＣＨＫ
//
// 応答データ：無し
//
// com_ptoff:
//
void com_ptoff(void)
{
UCHAR data;
// ポートを読む
data = inp(P8255_0);
// OFFに
data &= ~(P8255_PTON_BIT);
outp(P8255_0, data);
}
// ＜Ｘ軸イネーブルコマンド＞：９４
//
// 動作：Ｘ軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９４
//
// 例：ＳＱ、９４、ＣＨＫ
//
// 応答データ：無し
//
// com_xon:
//
void com_xon(void)
{
outp(P8255_C, P8255_C_XON);
}
// ＜Ｘ軸ディゼーブルコマンド＞：９５
//
// 動作：Ｘ軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９５
//
// 例：ＳＱ、９５、ＣＨＫ
//
// 応答データ：無し
//
// com_xoff:
//
void com_xoff(void)
{
outp(P8255_C, P8255_C_XOFF);
}
// ＜Ｙ軸イネーブルコマンド＞：９６
//
// 動作：Ｙ軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９６
//
// 例：ＳＱ、９６、ＣＨＫ
//
// 応答データ：無し
//
//
// com_yon:
//
void com_yon(void)
{
outp(P8255_C, P8255_C_YON);
}
// ＜Ｙ軸ディゼーブルコマンド＞：９７
//
// 動作：Ｙ軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９７
//
// 例：ＳＱ、９７、ＣＨＫ
//
// 応答データ：無し
//
// com_yoff:
//
void com_yoff(void)
{
outp(P8255_C, P8255_C_YOFF);
}
// ＜Ｚ軸イネーブルコマンド＞：９８
//
// 動作：Ｚ軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９８
//
// 例：ＳＱ、９８、ＣＨＫ
//
// 応答データ：無し
//
//
// com_zon:
//
void com_zon(void)
{
outp(P8255_C, P8255_C_ZON);
}
// ＜Ｚ軸ディゼーブルコマンド＞：９９
//
// 動作：Ｚ軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９９
//
// 例：ＳＱ、９９、ＣＨＫ
//
// 応答データ：無し
//
// com_zoff:
//
void com_zoff(void)
{
outp(P8255_C, P8255_C_ZOFF);
}
// ＜３軸イネーブルコマンド＞：９ａ
//
// 動作：３軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ａ
//
// 例：ＳＱ、９ａ、ＣＨＫ
//
// 応答データ：無し
//
// com_3on:
//
void com_3on(void)
{
UCHAR data;
// ポートを読む
data = inp(P8255_2);
// ＯＮ（０）に
data &= ~(P8255_3ON_BIT);
outp(P8255_2, data);
}
// ＜３軸ディゼーブルコマンド＞：９ｂ
//
// 動作：３軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ｂ
//
// 例：ＳＱ、９ｂ、ＣＨＫ
//
// 応答データ：無し
//
// com_3off:
//
void com_3off(void)
{
UCHAR data;
// ポートを読む
data = inp(P8255_2);
// ＯＦＦ（１）に
data |= P8255_3ON_BIT;
outp(P8255_2, data);
}
//
//
//
//
// ＜汎用出力ポートネーブルコマンド＞：９ｃ
//
// 動作：３軸をＯＮする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ｃ
//
// 例：ＳＱ、９ｃ、ＣＨＫ
//
// 応答データ：無し
//
void com_gon(void)
{
outp(P8255_C, P8255_C_GON);
}
//
//
//
//
// ＜汎用出力ポートディゼーブルコマンド＞：９ｄ
//
// 動作：３軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ｄ
//
// 例：ＳＱ、９ｄ、ＣＨＫ
//
// 応答データ：無し
//
void com_goff(void)
{
outp(P8255_C, P8255_C_GOFF);
}
//
//
//
// ＜汎用入力ポート入力コマンド＞：９ｅ
//
// 動作：３軸をＯＦＦする。
//
// データ数／コマンド総数：０／３
//
// フォーマット：９ｅ
//
// 例：ＳＱ、９ｄ、ＣＨＫ
//
// 応答データ：無し
//
void com_gin(void)
{
}
// ３軸モータコントローラメイン関数
// 無限ループをする。
//
// main:
//
void main(void)
{
UCHAR *com_ptr;
// 初期化する
init();
// 割り込みを許可する
ei();
// 無限に処理する
while (1) {
// ｆｉｆｏを読んでコマンドを得る
com_ptr = com_get();
// コマンド番号へ
com_ptr++;
// コマンドを処理する
switch(*com_ptr) {
//
//
テーブル移動
//
/////////////////////// 相対移動
case
COM_3RMOVE_NUM:
// ＜相対定速３軸移動コマンド＞：７６
// 動作：指定された座標に定速移動する
//
com_3rmove();
break;
case
COM_3RMOVE_AU_NUM:
// ＜相対ステップ加速３軸移動コマンド＞：７１
// 動作：指定された座標に速度０からしながら最高速度で移動、減速停止する
//
com_3rmove_au();
break;
case
COM_3RMOVE_A_NUM:
// ＜相対ステップ加速３軸移動コマンド＞：７５
// 動作：指定された座標に速度０からステップで加速し最高速度移動する
//
com_3rmove_a();
break;
case
COM_3RMOVE_U_NUM:
// ＜相対ステップ減速３軸移動コマンド＞：７ａ
// 動作：指定された座標に現在の速度からステップで減速し停止する
com_3rmove_u();
break;
case
COM_XRMOVE_AU_NUM:
// ＜相対ステップ加速Ｘ軸移動コマンド＞：７２
// 動作：指定されたＸ座標に速度０からステップで加速し最高速度で移動、減速停止する
//
com_xrmove_au();
break;
case
COM_YRMOVE_AU_NUM:
// ＜相対ステップ加速Ｙ軸移動コマンド＞：７３
// 動作：指定されたＹ座標に速度０からステップで加速し最高速度で移動、減速停止する
//
com_yrmove_au();
break;
case
COM_ZRMOVE_AU_NUM:
// ＜相対ステップ加速Ｚ軸移動コマンド＞：７４
// 動作：指定されたＺ座標に速度０からステップで加速し最高速度で移動、減速 停止する
//
com_zrmove_au();
break;
case
COM_XRMOVE_NUM:
// ＜相対定速度Ｘ軸移動コマンド＞：７７
// 動作：指定されたＸ座標に現在の速度で移動する
//
com_xrmove();
break;
case
COM_YRMOVE_NUM:
// ＜相対定速度Ｙ軸移動コマンド＞：７８
// 動作：指定されたＹ座標に現在の速度で移動する
//
com_yrmove();
break;
case
COM_ZRMOVE_NUM:
// ＜相対定速度Ｚ軸移動コマンド＞：７９
// 動作：現在の位置から指定されたＺ座標に現在の速度で移動する
//
com_zrmove();
break;
/////////////////////// 絶対移動
case
COM_3AMOVE_NUM:
// ＜絶対定速度３軸移動コマンド＞：８６
// 動作：指定された座標に現在の速度で移動する
//
com_3amove();
break;
case
COM_3AMOVE_AU_NUM:
// ＜絶対ステップ加速３軸移動コマンド＞：８１
// 動作：指定された座標に速度０からしながら最高速度で移動、減速停止する
//
com_3amove_au();
break;
case
COM_3AMOVE_A_NUM:
// ＜絶対ステップ加速３軸移動コマンド＞：８５
// 動作：指定された座標に速度０からステップで加速し最高速度移動する
//
com_3amove_a();
break;
case
COM_3AMOVE_U_NUM:
// ＜絶対ステップ減速３軸移動コマンド＞：８ａ
// 動作：指定された座標に現在の速度からステップで減速し停止する
com_3amove_u();
break;
case
COM_XAMOVE_AU_NUM:
// ＜絶対ステップ加速Ｘ軸移動コマンド＞：８２
// 動作：指定されたＸ座標に速度０からステップで加速し最高速度で移動、減速停止する
//
com_xamove_au();
break;
case
COM_YAMOVE_AU_NUM:
// ＜絶対ステップ加速Ｙ軸移動コマンド＞：８３
// 動作：指定されたＹ座標に速度０からステップで加速し最高速度で移動、減速停止する
//
com_yamove_au();
break;
case
COM_ZAMOVE_AU_NUM:
// ＜絶対ステップ加速Ｚ軸移動コマンド＞：８４
// 動作：指定されたＺ座標に速度０からステップで加速し最高速度で移動、減速 停止する
//
com_zamove_au();
break;
case
COM_XAMOVE_NUM:
// ＜絶対定速度Ｘ軸移動コマンド＞：８７
// 動作：指定されたＸ座標に現在の速度で移動する
//
com_xamove();
break;
case
COM_YAMOVE_NUM:
// ＜絶対定速度Ｙ軸移動コマンド＞：８８
// 動作：指定されたＹ座標に現在の速度で移動する
//
com_yamove();
break;
case
COM_ZAMOVE_NUM:
// ＜絶対定速度Ｚ軸移動コマンド＞：８９
// 動作：現在の位置から指定されたＺ座標に現在の速度で移動する
//
com_zamove();
break;
case
COM_CARYB_NUM:
// ＜キャリブレート＞：８０
// 動作：Ｘ、Ｙ、Ｚ軸とも原点に移動する。
// 原点は最左、手前、下のリミットＳＷがＯＮの位置
//
com_caryb();
break;
//
//
リセット、ダイアグ、バージョン通知、ループバック
//
case
COM_RESET_NUM:
// ＜リセット＞：００
// 動作：コントローラをリセットする
//
com_reset();
break;
case
COM_DIAG_NUM:
// ＜自己診断＞：１０
// 動作：自己診断を行ない結果を返す
//
com_diag();
break;
case
COM_VER_NUM:
// ＜ソフトウェアバージョン通知＞：１１
// 動作：ソフトウェアのバージョンを返す
//
com_ver();
break;
case
COM_TEST_NUM:
// ＜テスト＞：１２
// 動作：テスト
//
com_test();
break;
case
COM_LOOPB_NUM:
// ＜ループバック＞：２０
// 動作：入力されたデータを４バイトそのまま返す。
com_loopb();
break;
//
//
コマンドシンク、現在位置登録、タイマ制御、ダイレクトポート制御
//
case
COM_SYNC_NUM:
// ＜シンク＞：４０
// 動作：移動の同期を取るために、このシーケンスの１つ前
// のコマンドの終了時にホストに応答する。
com_sync();
break;
case
COM_RCURR_NUM:
// ＜現在位置を通知する＞：４１
// 動作：現在の位置を通知する
//
com_rcurr();
break;
case
COM_WCURR_NUM:
// ＜現在位置を登録する＞：４２
// 動作：現在の位置を登録する
//
com_wcurr();
break;
case
COM_RMAX_NUM:
// ＜最高速度通知＞：４３
// 動作：移動最高速度を通知する
com_rmax();
break;
case
COM_WMAX_NUM:
// ＜最高速度登録＞：４４
// 動作：移動最高速度を登録する
//
com_wmax();
break;
case
COM_RSTEP_NUM:
// ＜ステップ数通知＞：４５
// 動作：ステップ数を通知する
//
com_rstep();
break;
case
COM_WSTEP_NUM:
// ＜ステップ数登録＞：４６
// 動作：ステップ数を登録する
com_wstep();
break;
case
COM_TIMER_RES_NUM:
// ＜タイマー＞：４７
// 動作：指定したタイマーを待って応答データを返す
//
com_timer_res();
break;
case
COM_TIMER_NRES_NUM:
// ＜タイマー＞：４８
// 動作：指定したタイマーを待つ（応答データ無し）
//
com_timer_nres();
break;
case
COM_OPORT_NUM:
// ＜ダイレクトポート出力＞：４８
// 動作：出力ポート（Ｐ０、Ｐ２Ｌ）に直接データを出力する
//
com_oport();
break;
case
COM_IPORT_NUM:
// ＜ダイレクトポート入力＞：４９
// 動作：入力ポート（Ｐ１、Ｐ２Ｈ）から直接データを入力する
//
com_iport();
break;
//
//
主軸モータ制御
//
case
COM_MOON_NUM:
// ＜主軸モータ開始コマンド＞：９０
// 動作：主軸モータを回転させる。
//
com_moon();
break;
case
COM_MOOFF_NUM:
// ＜主軸モータ停止コマンド＞：９１
// 動作：主軸モータを停止させる。
//
com_mooff();
break;
case COM_PTON_NUM:
// ＜警告回転灯開始コマンド＞：９２
//
// 動作：警告回転灯をＯＮする。
//
com_pton();
break;
case
COM_PTOFF_NUM:
// ＜警告回転灯停止コマンド＞：９３
//
// 動作：警告回転灯をＯＦＦする。
//
com_ptoff();
break;
case
COM_XON_NUM:
// ＜Ｘ軸イネーブルコマンド＞：９４
//
// 動作：Ｘ軸をＯＮする。
//
com_xon();
break;
case
COM_XOFF_NUM:
// ＜Ｘ軸ディゼーブルコマンド＞：９５
//
// 動作：Ｘ軸をＯＦＦする。
//
com_xoff();
break;
case
COM_YON_NUM:
// ＜Ｙ軸イネーブルコマンド＞：９６
//
// 動作：Ｙ軸をＯＮする。
//
com_yon();
break;
case
COM_YOFF_NUM:
// ＜Ｙ軸ディゼーブルコマンド＞：９７
//
// 動作：Ｙ軸をＯＦＦする。
//
com_yoff();
break;
case
COM_ZON_NUM:
// ＜Ｚ軸イネーブルコマンド＞：９８
//
// 動作：Ｚ軸をＯＮする。
//
com_zon();
break;
case
COM_ZOFF_NUM:
// ＜Ｚ軸ディゼーブルコマンド＞：９９
//
// 動作：Ｚ軸をＯＦＦする。
//
com_zoff();
break;
case
COM_3ON_NUM:
// ＜３軸イネーブルコマンド＞：９ａ
//
// 動作：３軸をＯＮする。
//
com_3on();
break;
case
COM_3OFF_NUM:
// ＜３軸ディゼーブルコマンド＞：９ｂ
//
// 動作：３軸をＯＦＦする。
//
com_3off();
break;
}
// シーケンスｎｏを＋＋
Seq_no++;
}
}
//
// ２０ｍＳｅｃタイマー割り込み
//
// int_handler:
//
void int_handler(void)
{
// グローバルをインクリメント
I_count++;
// ＃１カウンタ再度２０ｍＳＥＣ設定
outp(C8254_1, (UCHAR)(C8254_1_INIT&C_MASK_LOW));
outp(C8254_1,
(UCHAR)((C8254_1_INIT&C_MASK_HIGH)>>8));
}