逢甲自動控制工程學系專題製作專題論文 ·...

逢甲大學自動控制工程學系專題製作

專題論文

PIC16F877直流馬達伺服控制

PIC16F877 DC Motor Server Control

指導教授陳孝武學生張岑瑋

張恩焌彭子昂

考試日期中華民國九十三年一月

逢甲大學自動控制工程學系

學年度第學期畢業專題評分表

專題題目

學生姓名

1 論點

內容(30)2 創見

3 深度

分

1 結構

文字(20)2 修辭

3 圖表

分

1 理路分析

口試(50)2 表達能力

3 綜合識見

分

總分分

改進意見

評審教授

i

感謝

本專題的完成首先要感謝指導教授陳孝武老師這一年多來細心的指導

在我們對此專題還不熟悉找不到方向時老師給予我們很大協助不論是在設計

還是觀念架構上給予我們的啟發與分享使我們授益良多再來要感謝大學生

涯中的教導過的老師們讓我們打下了良好的基礎使我們在完成這項專題的同

時能夠把過去學過的知識運用在上面還有實驗室的學長同學提供了不少寶

貴的經驗給我們在許多小細節提醒我們使我們能夠順利完成專題

ii

中文摘要

現今科技發達在工業界中自動化已經是相當普遍也相當重要的一環而馬

達控制則是自動化中不可或缺的一項本專題為 PIC16F877直流馬達伺服控制

由 PIC單晶片以及相關的電路實現目的為得到一個控制命令用來減去位置回

授產生 error 再送出至下級在這專題中我們採用的控制命令的輸入方式為

AD 轉換這部份也可以使用其他種類的輸入譬如鍵盤輸入回授的部份由馬

達的 Encoder得到利用單晶片之硬體功能將 AB做為觸發外部中斷將馬

達的位置存入 PIC內部之暫存器如需要做顯示角位移可直接將此暫存器用任

意方式表現譬如七節顯示器經過 AD 的轉換之後由馬達的驅動電路實現

最後得到我們想要之位置控制

iii

Abstract

In high-tect age it is almost popular and also important part to use automation

in industrial field and motor-control is a necessary part of the automations This

subject is talking about PIC16F877 DC motor server controlBy using PIC uni-chip

and related circuit accomplishing the goal is to get a control order and to release

the position producing error-reback and send the control order to the next lower

levelIn this subject we use AD transformation to show the input of control order

and this part of input could represent by other methods like keyboard-input The

part of reback could get from encoder of motorBy using the function of hardware

of uni-chip we could use A and B to trigger external interrupt and save the

position of motor in the registers of PIC internal part If we need to show the angle

displacement we could directly use any kind of representation of registers like 7

segment LED Therefore after using AD transformation we could get the result

from propel circuit of motor Finally we get what we want the position control

iv

目錄

感謝middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddoti

中文摘要middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotii

Abstractmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotiii

表目錄middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotvii

第一章前言middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot1

第二章 PIC與馬達的基本介紹 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot2

21 微電腦與微控制器middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot2

211 微電腦系統的基本架構middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot2

212 微控制器的基本架構middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot3

22 PIC16F87X 單晶片微控制器架構middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot4

221 PIC16F87X單晶片微控制器的系統架構方塊圖 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot4

222 PIC16F87X輸出輸入功能介紹 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot5

223 MPLAB 介紹middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot6

23 PIC晶片內部架構 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot7

231 PIC16F877之接腳圖 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot7

232 PIC16F877的內部結構 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot8

24 馬達的基本介紹middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot9

第三章基本架構及驅動電路middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot11

31 基本架構middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot11

32 電子元件介紹middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot12

33 電路內部說明middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot17

331 驅動電路內部說明middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot17

332 PIC16F877電路 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot18

第四章程式middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot20

v

41 暫存器介紹middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot20

42 主程式流程圖middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot25

421 外部中斷middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot27

422 AD轉換 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot31

423 24位元＋24位元加法副程式middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot33

424 16位元times8位元乘法副程式middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot35

425 16位元－16位元減法副程式middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot38

43 PID控制器介紹 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot40

431 Kp_Controlmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot41

432 Kd_Controlmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot45

433 Ki_Controlmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot47

44 PWM的介紹 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot50

45 PIC主程式 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot57

第五章結論middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot81

附錄 A 中斷 middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot82

附錄 B AD轉換middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot83

附錄 C AD轉換時間middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot84

附錄 D MPLAB操作說明middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot85

參考文獻middotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddotmiddot89

vi

圖目錄

圖 21 微電腦基本方塊圖 2

圖 22 微控制器單晶片架構方塊圖 3

圖 23 PIC16F87X單晶片微控制器的系統架構方塊圖5

圖 25 PIC16F877的內部電路圖8

圖 26 直流馬達外觀 10

圖 31 整體基本架構圖11

圖 32 硬體電路架構12

圖 33 三角波及直流電壓比較產生 PWM訊號 12

圖 34 UC3843接腳圖13

圖 35 UC3843內部電路圖13

圖 36 光耦合接腳圖 14

圖 38 IR2111接腳圖15

圖 310 DCW03B-15元件圖以及接腳圖16

圖 311 current sensor電路圖16

圖 312 LM348電路圖17

圖 313 完整驅動電路圖 18

圖 314 PIC16F877電路圖19

圖 41 AB相訊號關係圖30

vii

表目錄

表 21 PIC16F87X家族成員6

1

第一章前言

現在工業界由於市場競爭激烈自動化的技術成為工業界相當重要的一

環而自動化技術最主要的基礎就是有良好的馬達控制系統而如果欠缺了一套

優良的驅動方式去控制馬達一切都是空談

本專題是利用單晶片微處理器作為馬達控制器之用單晶片微控制器（Micro

controller unitMCU）憑藉其速度快體積耗電量小和彈性大的優點逐漸

取代傳統電子電路或邏輯電路的自動化領域包括辦公室自動化工廠自動化和

家庭自動化顯而易見的單晶片微電腦必是未來自動化最受歡迎的主控元件

我們這次專題所使用到的是美國 Microchip公司所開發的 8位元單晶片微控

制器 PIC16F877在硬體方面配合 PWM的工作方式配合 H型的馬達驅動電路來

驅動馬達經由編碼器回授位置構成精密的馬達位置控制軟體方面是將傳統

之 PID控制寫入單晶片中由於 PIC的指令較少易上手如果再加上已有之動

作或程式流程圖這將會是一種極容易擴充其功能的系統

預期能夠利用單晶片使得馬達控制之重心由電路轉移至單晶片方面達到

自動化之控制

2

第二章 PIC與馬達的基本介紹

21 微電腦與微控制器

211 微電腦系統的基本架構

電腦泛指具有中央處理器加上記憶體以及輸出輸入設備之系統圖 2-1為一

般電腦系統的基本方塊圖微處理器是指將 CPU的功能製造於一個半導體晶片

上微電腦系統則是指使用微處理器及外加一些記憶體以及一些輸入輸出設備或

週邊元件所構成的一個電腦系統微電腦系統中 CPU程式與資料的記憶體與輸

出輸入間的資料必須藉由匯流排來讀取及寫入〔參考 PIC16F87X微控制器原理

實習與專題製作施慶隆〕

匯流排可分為資料匯流排（Data Bus）位址匯流排（Address Bus）與控

制匯流排（Control Bus）資料匯流排負責傳遞資料所有往來於 CPU記憶

體與周邊 IO 的資料訊息與控制命令都要經過資料匯流排傳送位址匯流排負

責傳送記憶體及周邊 IO 的位址線控制匯流排上控制訊號包括有指示傳輸資料

方向交談的時序訊號以及中斷訊號等

圖 21 微電腦基本方塊圖

3

212 微控制器的基本架構

微控制器又稱單晶片一般微電腦系統大多採用 Von Neumann架構而微控

制器與數位信號處理器則大多採用 Harvard架構因為微控制器將應用所需的所

有硬體（計時器數位輸出輸入類比輸入）皆包括在單晶片中因此系統只需

接上電源震盪器與啟動按鈕即可工作

單晶片微控制器的內部記憶體通常只有數千個位元大小故用小型系統控

制一般微控制器的單晶片的程式通常放置於內部的 ROM程式記憶體而資料變

數則放置於內部 RAM資料記憶體

圖 22 微控制器單晶片架構方塊圖

4

22 PIC16F87X 單晶片微控制器架構

221 PIC16F87X單晶片微控制器的系統架構方塊圖

PIC微控制器是由美國 Microchip公司所開發生產的 8位元單晶片微電腦

PIC是採用精簡指令架構PIC16F87X系列單晶片微控制器屬於精簡指令集 RISC

（Reduced Instruction Set Computing）作為設計架構它只有 35個組合語言

指令且每一個指令皆固定 14位元的字元

PIC16F87X 單晶片微控制器採用 Harvard 電腦結構系列中主要有 4 個不

同型號的單晶片 PICF873874876877基本上這四種單晶片接腳數和記憶體容

量交互搭配所得到四種選擇

工作頻率範圍為 DC至 20MHz內建有 Power-on Reset和 Brown-out Reset

兩種重置功能電源啟動延遲計時器和震盪器啟動延遲計時器除了一個看門狗

計時器外另外還有 3 個計時器和兩個 CCP 模組串聯通訊模組方面共支援

USARTSPI及 I2C

由於 PIC 的程式記憶體與資料記憶體分開所有指令只須從程式記憶體讀

取一次加上它的 CPU具有 Pipelining式的平行處理功能因此可以執行一個

指令同時讀取下一個指令故除了屬於跳躍指令其他一般指令只是單一個指令

週期（Instruction Cycle）的執行時間PIC 系統輸入 Clock 周期的 4 倍因

此 PIC的一小段組合語言程式的執行時間很容易計算出來PIC微控制器的最大

工作頻率為 20MHz

5

圖 23 PIC16F87X單晶片微控制器的系統架構方塊圖

222 PIC16F87X輸出輸入功能介紹

1 3個計時器（Timer）除了最基本的定時功能之外具有捕捉比較產生

PWM信號等功能

2 12個外部中斷源共有一個中斷向量（位於程式記憶體之 0x04）

3 33個輸出輸入接腳

4 串聯 RS232通訊

5 8個 10-bit類比數位轉換器（AD）

6 同步串列通訊包括 SPI主控模式及主控與從屬模式

7 8位元的平行從屬模式（Parallel Slave PortPSP）

6

表 21 PIC16F87X家族成員

223 MPLAB 介紹

MPLAB是 Microchip 公司對 PIC系列單晶片所發展的一套整合發展環境

（Integrated Development Environment IDE） MPLAB包含下列工具

1 MPLAB Editor―程式編輯器

2 MPASM Assembler―程式組譯器

3 MPLAB-SIM Software Simulator―軟體模擬器

4 MPLAB-ICEMULATOR―硬體模擬器

5 MPLAB-ICD―ICD元件的偵錯模擬環境

7

23 PIC晶片內部架構

231 PIC16F877之接腳圖

PIC系列之單晶片微電腦是一個 40隻接腳接腳的排列如圖 24所示

圖 24 PIC16F87X接腳圖

8

232 PIC16F877的內部結構

PIC16F877系列單晶片微電腦的內部電路圖 25

圖 25 PIC16F877的內部電路圖

9

24 馬達的基本介紹

電動機即為工業界俗稱的馬達種類依照使用電源可分成直流馬達(DC

motor)與交流馬達(AC motor)兩大類若再以控制方式啟動方式與繞組方式分

類則可分成步進馬達(stepping motor)伺服馬達(servo motor)無刷馬達（霍爾

馬達）單相交流馬達三相感應馬達串激式直流馬達分激式直流馬達與

複激式直流馬達等

馬達之基本構造

1 電樞(armature)為馬達旋轉的部份材質為永久磁鐵線圈(外接電源)

導線(無外接電源)或特殊形狀之導磁材料

2 場繞組（field）材質為永久磁鐵或是線圈(外接電源)

3 滑環（slip ring）連接轉子繞線至外部換向器用於改變電樞繞線之電流方

向使用永久磁鐵為轉子材質的馬達則無需滑環或換向器

4 軸承(bearing)可使用滾珠滾針滾柱主要提供轉子穩固的支撐

5 馬達控制器（motor controller）

包含控制馬達的輸出扭矩速度或轉角以及大型馬達起動停止之順序控

制

10

專題所使用的直流馬達規格如下

1 直流電壓範圍3~75V

2 最大轉速1800rpm

3 功率30W

4 每轉一圈會產生 360個脈衝訊號

5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相

圖 26 直流馬達外觀

11

第三章基本架構及驅動電路

31 基本架構

PID控制器 DA 馬達驅動電路

Encoder

Input θ+

-int

ω

圖 31 整體基本架構圖

圖 31中虛線左側的部份由 PIC單晶片以及相關的電路實現目的為得到

一個控制命令用來減去位置回授的值產生誤差值再送出至後級在這專題中

我們採用的控制命令的輸入方式為 AD 轉換回授的部份由馬達的編碼器得到

利用單晶片之外部中斷將 AB 相訊號之上升邊緣訊號作為觸發將馬達的位

置存入 PIC內部之暫存器如需要做顯示角位移可直接將此暫存器用任意方式

表現譬如七節顯示器

經過 AD 的轉換之後進入到虛線右邊由馬達的驅動電路實現最後得到

我們想要之位置控制

12

圖 32 硬體電路架構

32 電子元件介紹

UC3843

1工作頻率由 TC TR 來決定

T=f1

TT CRf 81

=

2直流電壓輸入 25V時責任週期為 50

3輸出振幅為 0V~15V

4內部為直流電壓與三角波做比較產生 PWM訊號調整直流電壓即調整

PWM的 duty cycle (責任週期)如圖 33

圖 33 三角波及直流電壓比較產生 PWM訊號

13

5UC3843接腳

由圖 34第 2根接腳由 LM348輸入一直流電壓第 8根接腳輸出 REFV 供給

TC TR BJT運作第 7根接腳接上 15V為了驅動 IC的電壓以及 PWM輸出的

參考電壓當電壓 25V由第 3根接腳輸入 UC3843內時內部的比較器跟直流電

壓做比較後產生 PWM訊號

圖 34 UC3843接腳圖

圖 35 UC3843內部電路圖

14

TLP250

1目的為了把兩個不同的電源分開來

2輸出的參考電壓為 0V和 17V

3UC3843所輸入的 PWM訊號經由兩顆 TLP250後產生出二組反向的 PWM訊號

圖 36 光耦合接腳圖

IR2111

1 主要的目的是將一組 PWM訊號分成二組反向之 PWM訊號

2 二組 PWM訊號分別接到二個 MOS之 GSV 使 MOS進入三極區或是截止區作

為開關使用

3 IR21112的 HO及 LO分別接到 MOS的 Gate接腳如圖 37

圖 37 IR2111內部電路

15

圖 38 IR2111接腳圖

電源電路

1由一般的 110V的交流電壓經過變壓器轉為 12V之交流電壓

經過全波整流器電容產生 17V 的直流電壓 DCW03B-15 產生三個直流參考

電壓 15V 0V -15V提供 IC動作

2經過變壓器轉為 48V之交流電壓經過全波整流器電容後產生 70V的直流

電壓作為馬達之驅動電壓

3由齊納二極體之特性逆偏時崩潰而近似一直流電壓2V

圖 39 齊納二極體電路

16

圖 310 DCW03B-15元件圖以及接腳圖

current sensor(HY05P)與 OP(LM384)

current sensor主要目的為檢測馬達電流轉成電壓大小做為回授

接腳 1+15V

接腳 2-15V

接腳 3量測電壓輸出

接腳 40V

接腳 5電流輸入

接腳 6電流輸出

圖 311 current sensor電路圖

17

LM384

內部由 4個 op所組成作為 current sensor回授前級的輸出以及提升準

位

圖 312 LM348電路圖

33 電路內部說明

331 驅動電路內部說明

由 PIC產生的 PWM訊號經過電容充放電轉成中心準位在 2V之直流電壓

送入 LM348後送出中心準位於 25V的直流電壓送入 UC3843UC3843將輸入

訊號經由內部的放大器降低電壓後與三角波比較後產生準位在 50的 PWM訊

號此時的 PWM訊號在 50的情況下馬達將會停止而超過 50時馬達會做

正轉的動作在 50以下時馬達會做反轉的動作

由 UC3843所送出的 PWM訊號送入 2個 TLP250內如圖 313的編號 12

TLP250送出一組反向輸出的 PWM訊號TLP250主要用途是將兩個不同的電源分

開編號 1的光耦合 PWM訊號送入如圖 313編號 3的 IR2111編號 2的光耦合

送入編號 4的 IR2111IR2111功用是將一 PWM分兩個反向之 PWM訊號一個有

浮接功能編號 3的 IR2111輸出的兩個 PWM訊號中一個由 HO端接至編號 5的

MOS之 G端Vs接至 S端LO則送至編號 7的 MOS之 G端編號 4之 IR2111 同

理最後使得 4個 MOS之 G端訊號編號 5以及編號 8同相編號 6以及編號 7

18

同相

圖 313 完整驅動電路圖

332 PIC16F877電路

PIC

第 1隻接腳用來做開機 RESET用途

第 2隻接腳RA0作為 AD轉換通道

第 11123132隻接腳PIC的參考電壓以及接地

第 1314隻接腳震盪器

第 17隻腳RC2PIC的 PWM輸出

第 3334接腳接到圖 313 右邊的 Encoder的 46隻接腳Encoder的第 5

19

隻接腳接至 5V

LM7805 5V之穩壓 IC

圖 314 PIC16F877電路圖

由 RA0當做系統之輸入AD轉換之後將輸入值以 2進位形式存於暫存器中

3334 接腳為回授以外部中斷之方式將數值存於暫存器中稍後於程式說

明中會介紹由第 17隻接腳輸出 PWM訊號當訊號為 5V時PIC通過電阻對電

容充電訊號為 0V 時PIC 通過電阻對電容放電由於充放電時間短近似於

一個直流電壓值送至馬達驅動電路作為輸入電壓

20

第四章程式

41 暫存器介紹

基本功能暫存器

STATUS 狀態

PORTX 輸入輸出接腳與暫存器同名（PORT有 5組X=ABCDE）

TRISX 設定各個 PORTX接腳的輸入輸出形式（X=ABCDE）

TXCON 計時計數器以及 PWM功能設定（X=0122為特殊功能如 PWM）

中斷相關暫存器

INTCON 基本中斷的致能以及旗標

PIE1 特殊中斷的致能

PIR1 特殊中斷的旗標

OPTION_REG觸發信號設定

AD轉換相關暫存器

ADCON0設定振盪器種類轉換通道

ADCON1設定轉換類型

ADRESH轉換結束資料的高位元組

ADRESL轉換結束資料的低位元組

PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値

CCPR1L 決定 Duty Cycle之 L位元組（可設定 10位元）

21

CCPR1H 決定 Duty Cycle之 H位元組

TMR2 Timer2計數値

自定義暫存器

W_Temp EQU 0x20 W暫存器暫時存放處

STATUS_Temp EQU 0x21 STATUS狀態暫存處

SUB1_H EQU 0x23 減法副程式相關指令

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b 加法副程式相關指令

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e 乘法副程式相關指令

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32 加法副程式相關指令

TMP_M EQU 0x33

TMP_H EQU 0x34

ERR_SGN EQU 0x35 PID副程式儲存正負號

Count EQU 0x36 乘法副程式計數

EncoderL EQU 0x37 馬達位置計數器

22

EncoderH EQU 0x38

Cmd_H EQU 0x39 控制命令暫存器

Cmd_L EQU 0x3a

Error_L EQU 0x3b 誤差

Error_H EQU 0x3c

Error1_L EQU 0x3d 前一刻之誤差

Error1_H EQU 0x3e

Kp EQU 0x4f 比例常數

Kp_Term_L EQU 0x40 比例常數乘上誤差值

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43 微分常數

Kd_Term_L EQU 0x44 微分常數乘上誤差變化量

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47 積分常數

Ki_Term_L EQU 0x48 積分常數乘上累積誤差

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b Kp_Term+ Kd_Term+ Ki_Term

MSum_M EQU 0x4c

MSum_H EQU 0x4d

Itemp_L EQU 0x5e 累積誤差（可能會更改到數字）

Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23

Error_Sum_L EQU 0x51 累積誤差（只累加）

Error_Sum_M EQU 0x52

Error_Sum_H EQU 0x53

ErrorOver EQU 0x54 誤差的高位元組

相關指令介紹

movf 將 f暫存器的値移至 W暫存器

movwf 將 W暫存器的値存入 f暫存器中

movlw 將常數存入 W暫存器中

clrw 清除 W暫存器

clrf 清除 f暫存器

swapf 一位元組中高四位元與低四位元互換

rlf 暫存器資料左移一位元

rrf 暫存器資料右移一位元

addwf 將 f暫存器加上 W暫存器

addlw 將常數 k加入 W暫存器

subwf 將 f暫存器減去 W暫存器

sublw 將常數 k減去 W暫存器

comf 將 f暫存器取補數

incf 將 f暫存器加 1

decf 將 f暫存器減 1

bcf 將 f暫存器指定之位元設為 0

bsf 將 f暫存器指定之位元設為 1

goto 跳躍指令

call 必須與 return或 retlw配合的副程式呼叫指令

24

return 回至上層程式

retfie 離開中斷程式

btfss 判斷指定位元狀態0則執行下個指令1則執行第二個指令

btfsc 判斷指定位元狀態1則執行下個指令0則執行第二個指令

nop 不執行動作

25

42 主程式流程圖

MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output

開機後程式先進入初始值設定在 Initial 中設定 AD 轉換PWM 輸出

暫存器初始值外部中斷接著進入 AD轉換在 AD轉換中取得輸入命令存入

Cmd暫存器中再與 Encoder暫存器相減得到 Error將 Error放入 PID副程式

中做運算運算完的值存入 MSum中經由 PWM輸出

以下會逐步介紹程式內部副程式的程式流程圖副程式包含了硬體功能的

PWMAD轉換外部中斷等軟體部分包含了比例微分積分等運算且由於

PWM指令限制無法直接做出乘法以及 PID運算中所需要多位元組的計算所

以各別寫出 16位元－16位元減法副程式24位元＋24位元加法副程式16位

元times8位元乘法副程式等

26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷

在 8051中每一個中斷都有著中斷向量在 PIC中所有的中斷向量皆在

0x04程式判斷為中斷後跳至 0x04再用軟體判斷它是屬於何種中斷再跳

到中斷程式執行可在以下程式第 4到 6行加入判斷中斷旗標之程式附錄中會

有所有中斷之簡介

28

IntService 中斷副程式

movwf W_Temp 儲存狀態

swapf STATUSw

movwf STATUS_Temp

banksel INTCON 檢查 INTCON暫存器

btfsc INTCONINTF 判斷此為何種中斷

call Encoder_ISR 是外部中斷則進入 Encoder

計數之副程式

swapf STATUS_Tempw 取回狀態

movwf STATUS

swapf W_Tempf

swapf W_Tempw


Encoder_ISR Encoder計數副程式

banksel PORTB B相信號 L則跳至 Forward

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse B相信號 L則跳至 Forward

banksel INTCON

bcf INTCONINTF

return

29

Forward Encoder計數値＋1

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return

Reverse Encoder計數値－1

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相

圖 41 AB相訊號關係圖

這邊先介紹硬體部分將馬達 Encoder之 A相信號接至 RB0B向信號接至

RB1（PIC程式中用 RB0表示 PORTB0其餘接腳亦同）程式部分進入中斷

後首先將目前的狀態儲存儲存 W 暫存器以及 STATUS馬達位置計數方式參

考上表中斷程式設計為當 A相之上升邊緣信號產生時後程式跳至中斷副程

式此時讀取 B相信號的輸入值當 A相上升邊緣觸發時中斷產生時如果 B相

信號 L判斷為正轉Encoder暫存器值＋1如果 B相信號＝H則 Encoder暫

存器值－1接著取回狀態回到主程式PIC 系列單晶片中return 指令代表

回到上一層程式retfie代表中斷結束回到主程式

31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0

Cmd_H=ADRESHCmd_L=ADRESL

return

No

Yes

AD_Convert

banksel ADCON0

bsf ADCON0GO 開始 AD轉換

AD_Wait

btfsc ADCON0GO 等待轉換時間

goto AD_Wait

movf ADRESHw 轉換結束儲存結果

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return

AD轉換是 PIC系列單晶片的一大特色不用再外接 ADC0804稍微比較一下

二者的差別ADC0804需外接轉換位元為 8位元大約需要 100uspic16f877

內建的 AD 轉換轉換位元為 10 位元轉換時間約為 40us詳細的算法於附錄

中介紹且可依照 AD 模式的選定以數根接腳做為輸入雖然不能同時轉換

但也大大的節省了外部電路的麻煩以及時間

在初始值設定過之後AD轉換就只需要把 ADCON0中的第 2位元 GoDone設

為 1接著等待轉換時間等 GoDone位元變為 0即為轉換完畢將資料存入 Cmd

（控制命令）中回到主程式

33

423 24位元＋24位元加法副程式

34

加法副程式的目的在運算多位元組的加法將被加數放入 Sum_HSum_M

Sum_L暫存器中加數放入 TMP_HTMP_MTMP_L答案存入 Sum_HSum_MSum_L

中

先將低位元組相加如有進位則中位元組加 1如中位元組加 1後進位

則高位元組再加 1再將中位元組相加如有進位則高位元組加 1回主程式

ADD24

banksel TMP_L 低位元組相加

movf TMP_Lw

addwf Sum_Lf

movlw 0x01 處理低位元組相加後之溢位

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw 中位元組相加

addwf Sum_Mf

movlw 0x01 處理中位元組相加後之溢位

btfsc STATUSC

addwf Sum_Hf 高位元組相加

movf TMP_Hw

addwf Sum_Hf

return

35

424 16位元times8位元乘法副程式

MULT16

Count=8

MM20=0

Call ADD24

MM2右移一位TMP左移一位

Count=Count-1

Count=0

Yes

No

return

Yes

No

乘法副程式主要用在有關 KpKdKi的運算所以用 16位元乘 8位元即可

這邊的設計呼叫副程式之前將被乘數存入 MM1_HMM1_MMM1_L中乘數放

入 MM2副程式中將 MM1_HMM1_MMM1_L分別存入 TMP_HTMP_MTMP_L中

36

運算後之結果放入 Sum_HSum_MSum_L中

進入副程式後將 Count暫存器存入 8代表著做 8次迴圈判斷 MM20是

否為 0如果不是則呼叫加法副程式將 MM1 加入 Sum如果是 0則直接進

入下一步將 MM2右移一位可以讓下一迴圈時繼續判斷 MM20將 TMP左移

一位代表著乘以 2如同加法一般做疊加的動作在此將 MM1的值存入 TMP

是因為使用到加法副程式所以必須存進 TMP重複執行 8次後回主程式

在此需要注意乘法副程式限用 2位元組之內的在這裡不用擔心超過 3個

位元組因為 Error與 Error1（Error後面會提到）都很小ErrorSum（累

積誤差後面會提到）有加入條件限制都會在 2位元組之內乘上 1位元組

可以用 3位元組儲存但是如果被乘數以補數形式出現高位元的 1將會使得答

案錯誤所以如果要加強乘法副程式的功能使被乘數以補數型態仍可運算

可在原副程式之上加入判斷 MM1為正或負負的話取補數記錄正負號原副

程式執行完之後依照已記錄之正負號再取補數

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08 連做 8次加法存入計數値

movwf Count

movf MM1_Hw 數字存入加法相關暫存器

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop

btfsc MM20 MM20＝0時不執行加法

call ADD24 MM20＝1時執行加法

bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38

425 16位元－16位元減法副程式

減法程式用在控制命令（Cmd）減回授值（Encoder）與 Error 減上一筆

Error時所以使用 16位元相減

將被減數存入 SUB2_HSUB2_L中減數存入 SUB1_HSUB1_L中先做低位

元組之相減判斷是否借位如果是則高位元組減 1否則直接進行下一步

再來做高位元組之相減答案存入 temp_Htemp_L中回主程式

SUB16

banksel SUB1_L

movf SUB1_Lw 低位元相減

subwf SUB2_Lw

39

movwf temp_L

btfss STATUSC 處理借位現象

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40

43 PID控制器介紹

PID控制器就是根據系統的誤差利用比例積分微分計算出控制量比例控

制是一種最簡單的控制方式其控制器的輸出與輸入誤差訊號成比例關係當僅

有比例控制時系統輸出存在穩態誤差（Steady-state error）在積分控制中

控制器的輸出與輸入誤差訊號的積分成正比關係

對一個自動控制系統如果在進入穩態後存在穩態誤差則稱這個控制系統

是有穩態誤差的或簡稱有差系統（System with Steady-state Error）為了消

除穩態誤差在控制器中必須引入ldquo積分項rdquo積分項對誤差取關於時間的積

分隨著時間的增加積分項會增大這樣即便誤差很小積分項也會隨著時

間的增加而加大它推動控制器的輸出增大使穩態誤差進一步減小直到等於零

因此比例+積分(PI)控制器可以使系統在進入穩態後無穩態誤差在微分控

制中控制器的輸出與輸入誤差訊號的微分（即誤差的變化率）成正比關係

自動控制系統在克服誤差的調節過程中可能會出現振盪甚至失穩解決的辦

法是使克服誤差的作用的變化要有些ldquo超前rdquo即在誤差接近零時克服誤差的

作用就應該是零這就是說在控制器中僅引入ldquo比例rdquo項往往是不夠的比例

項的作用僅是放大誤差的幅值而目前需要增加的是ldquo微分項rdquo它能預測誤差

變化的趨勢這樣具有比例+微分的控制器就能夠提前使克服誤差的控制作

用等於零甚至為負值所以對有較大慣性和（或）滯後的被控對象比例+微

分(PD)的控制器能改善系統在調節過程中的動態特性

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control

進入副程式後將控制命令（Cmd）－回授值（Encoder）得到 Error（存於

temp）接下來做乘法的動作前面提到過乘法副程式被乘數只能是正數所以

先將暫存器 temp的正負號儲存於 ERR_SGN0中如已是補數型態則再補數再

進入下一步正數的話直接進入下一步將 Kptimes|Error|存入 Kp_term中再依

照 ERR_SGN0所記錄原本是取補數的再取補數一次原本是正數的則不動

回主程式

42

Kp_Control

banksel Cmd_L 控制命令減去位置值得到 error

movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0 儲存 error正負號

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive error為正至 Positive做乘法

comf temp_Lf error為負取補數後再至

comf temp_Hf Positive做乘法

movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0 error原來為正回 PID主程式

return

44

comf Kp_Term_Lf 取過補數之 error再取補數得

comf Kp_Term_Mf 原來數再回 PID程式

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control

temp=Error-Error1Error1=Error

ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0

Kd_Term=(-1)timesKd_Term

return

Yes

Yes

No

No

進入微分控制副程式後將 Error－Error1Error1為前一刻的 Error（存

於 temp）第一次執行時設為 0將暫存器 temp的正負號儲存於 ERR_SGN1中

如是補數型態則再取補數再進入下一步正數的話直接進入下一步將 Kdtimes

|temp|存入 Kd_term中再依照 ERR_SGN1所記錄原本是取補數的再取補數

一次原本是正數的則不動回主程式

46

Kd_Control

movf Error_LW Error1減去 Error得到error

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H 將此刻之 Error存入 Error1

movf Error_Hw 供下一輪使用

movwf Error1_H

movf Error_Lw

movwf Error1_L

下面步驟與 Kp_Control相似

47

433 Ki_Control

Ki_Control

Error_Sum=Error+ErrorSumItemp=ErrorSum

ERR_SGN2=0

Itemp=(-1)timesItemp

Ki_Term=KitimesItemp

ERROR_SGN2=0

Ki_Term=(-1)timesKi_Term

return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes

進入積分副程式後首先將 ErrorSum（累積誤差初值為 0）加上 Error（這

48

次的誤差）存入 ErrorSum以及 Itemp由於乘法副程式的限制被乘數不能超

過 2位元組所以在這邊檢查 ErrorSum是否超過 2位元組超過則回主程式

沒超過則繼續進行積分運算將 Itemp的正負號儲存於 ERR_SGN2中如已是補

數型態則再取補數再進入下一步正數的話直接進入下一步將 Kitimes|Itemp|

存入 Ki_term中再依照 ERR_SGN2所記錄原本是取補數的再取補數一次

原本是正數的則不動回主程式

Ki_Control

banksel ErrorOver 判斷 Error是否過大

movf ErrorOverf 過大則回主程式

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw 累加 Error

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H

call ADD24 以下步驟與前面類似

50

44 PWM的介紹

馬達的驅動電路有很多種其中間斷式驅動電路大多採用脈寬調變(PWM)來

作工作原理是利用一顆 OP 放大器將不同電壓值和三角波來作比較所產生出

不同脈波寬度的方波

脈波寬度調變(Pulse Width Modulation PWM)此種驅動方式以固定頻率

開關直流供應電壓它的驅動原理是將一可調式的電壓和一個固定頻率的三角

波信號比較經由比較器比較後可得工作週期(Duty Cycle)和頻率不變的方

波這一方波的平均電壓值正比於輸入之可調電壓若輸入電壓改變輸出的方

波寬度亦不同所以脈波寬度來表示其電壓型態稱為脈波寬度調變

Summation以及 PWM輸出

將 Kp_Term加 Kd_Term加 Ki_Term存入 MSum再判斷其正負號正則將 RC3

設為 1負則取補數RC3設為 0回主程式

Summation

51

movf Kp_Term_Lw Kp_Term存入 TMP

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw Kd_Term存入 Sum

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H

call ADD24 Sum＝Kp_Term＋Kd_Term

movf Ki_Term_Lw Ki_Term存入 Sum

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24 Sum＝Kp_Term＋Kd_Term＋Ki_Term

52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7 判斷總和的正負號以決定增加或

bcf PORTC3 減少 duty cycle

btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出

在這將之前所得之|MSum|以及正負號反應到 PWM的 duty cycle上這

邊會受到一些限制在這之前的運算Kp_Temp等數值都是用 2位元組來表示

54

但是 pic決定 duty cycle是由 1個位元組來表示所以先判斷|MSum_H|是否有

值如果有則再依照 RC3為 0或 1將 duty cycle設為 100或是 0若 MSum_H

為 0則繼續往下執行若 MSum_M≧0x04則如上再依照 RC3為 0或 1將 duty

cycle設為 100或是 0若 MSum_M＜0x04則繼續往下執行

由於暫存器 CCPR1L的值決定 duty cycle又無誤差時必須在 50所以

先存入 Brsquo10000000rsquo代表 50如此之後可存入之值只能為 7位元將 MSum_M

MSum_L右移 3位這樣一來MSum_L即為所需要之 7位元而這裡忽略掉最低

3位元降低靈敏度這裡可依照使用需要而改變MSum右移 3位後依照 RC3

的值使 CCPR1L加上 MSum_L或是減掉 MSum_L由 PWM形式的輸出達到 PID

的目的

PWM_Output

movf MSum_Hf 判斷 MSum值的大小

btfss STATUSZ

goto PWM_Limit 過大則判斷全速轉動

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf MSum過大參數調整不易

rrf MSum_Lf 在這將其値做適當之減少

bcf STATUSC 方便觀察結果

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send

PWM_Limit 全速轉動之副程式

banksel CCPR1L 在此判斷應反轉或是正轉

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC

include ltp16f877incgt

W_Temp EQU 0x20

STATUS_Temp EQU 0x21

SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50

Error_Sum_L EQU 0x51



ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG

bsf OPTION_REGINTEDG

bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論

以前讀書的時候都是由理論方面去看從來不知道實做的時候會有什麼問

題真正著手的時候才發現一切都和以前不同從一開始的硬體部分電路測試

就碰到了許多問題比方說 IC不會使用電阻值如何選定到了做 PCB板的時

候又碰到許多焊接的問題接下來進入程式部份接觸一個完全沒學過的單晶

片可能會碰到的問題自然不在話下每一個階段都碰到了許多問題後來都慢

慢的解決與其說學會了如何控制馬達不如說學會了正視問題以及解決問題

在這專題中還有一些地方是我們已經發現的但是無法確實處理的問題

說到位置控制應該都會想到說要如何顯示出角位移這一點在上面提到過

了加裝個顯示就可以做到重點在於這樣的結果是否真的能達到想要的位移

以我們的程式發現只有 error=0的時候馬達才會停止轉動這點倒是可以確定

了馬達有確實執行程式但是其實還是會有誤差其原因可能發生在於馬達的轉

角是否確實的讓 PIC讀取到了或是編碼器之問題或是單晶片程式問題這

點倒是可以改善的地方

82

附錄 A 中斷

PIC中有三種最常用的中斷分別是 TIMER0（TOIE致能TOIF旗標）

RB0外部中斷（致能 INTE旗標 INTF）RB4RB7中斷（致能 RBIE旗標 RBIF）

這三種中斷的致能以及旗標都在中斷設定中最常使用到的暫存器 INTCON

中可以設定其餘的就要到暫存器 PIR（旗標）PIE（致能）中找尋要注

意的是RB4RB7中斷都用同一個中斷旗標使用時需要加入一些程式判斷

可以看到的是RB0是一個很常用的中斷而且好判斷所以把它直接用來當作

輸入或是最主要的中斷是個很常用的作法

與 8051不同的是PIC的中斷向量都在 0x04如果同時使用到多種中斷

其優先權是由使用者在程式中定義這也算是個 PIC的優點

83

附錄 B AD轉換

PIC的 AD 轉換最大的強處是它有數個 AD 轉換通道經由 ADCON1暫存器

之＜30＞位元（PCFG3PCFG0）設定可以決定出要使用到幾組類比輸入通道

以及參考電壓方式使用時在初始值中設定好之後在 ADCON0 暫存器之＜53

＞位元做類比輸入通道的選擇然後將 ADCON0暫存器＜2＞位元設為 1就能開

始轉換了

84

附錄 C AD轉換時間

總轉換時間 ADADACQ TTTT 212 ++= 其中 ACQT 為內部取樣保持以及溫度參數

的詳細計算PIC的 data sheet中已有計算約為 1972us ADT 為轉換一個 bit

所需時間而轉換 10個 bit時間至少需要 12 ADT 後面的 2 ADT 代表如果

接著要轉換其他 AD轉換通道則需要等待 2 ADT 的時間才能繼續進行考慮 ADT

的值如果使用 20MHz的震盪器則 OSCAD TT 32= 其目的在使 ADT 至少為 16us

用以上公式算出來如果使用 20MHz之震盪器以及多個 AD轉換通道每

一個轉換通道之轉換時間約為

T=1972us＋14times16us=4212us

85

附錄 D MPLAB操作說明

參考使用到的編譯軟體為 MPLAB-IDE這是一個免費軟體仿間賣的書後之

目錄都可以找到或是由官方網（httpwwwmicrochipcom1010indexhtm）

下載本專題使用到的版本為 57版官方網站至今為止最新到 64版由於使

用介面與參考書籍不同所以我仍然使用 57版方便使用

安裝過程沒什麼需要注意的以下從安裝完之後的介面開始介紹

第一次使用要在 Project-New Project建立專案準備一個資料夾專案

產生的檔案要與以後編譯的程式放在一起專案的目的是儲存個人喜好的使用環

境以後開啟 MPLAB-IDE時會自動跳出提示畫面選擇自己要開啟的專案

（pjt）再執行後續動作

Project-New Project之後會出現以下畫面

86

這畫面以後要開啟的話可以在 Project-Edit Project找到會很常需要使

用這指令按下 OK就可以從 File開始編寫程式

程式編寫完成後儲存Project-Edit Project在「Add Node」中將程式

（asm）加入在「Change」可選擇軟體模擬硬體模擬等功能要注意的是

要加入的程式要先組譯過（下面會說明）才行

按下 Change可以看到以下畫面

87

以下就會使用到的選項做說明

None「Editor Only」編輯

MPLAB SIM Simulator在 PC上模擬

MALAB ICD Debugger硬體（MALAB-ICD）模擬

選擇完模式之後右上方選擇使用到的單晶片型號按下 OK回到主畫面

程式編輯好之後接著是組譯在 Project下面可以看到三個指令

Make Project這指令只能組譯包含一個程式的專案

Build All組譯專案內所有的程式

Build Node只組譯選擇的檔案比方說有多個檔案的程式只組譯所選的

在此專題較常使用的是 Build All

組譯完成後所出現的訊息與操作方式和程式語言的編輯環境一樣不贅述

如在 Edit Project所選擇的是軟體模擬組譯完之後可由 Debug下的子

指令來操作若要換到硬體模擬要先到 Edit Project「Change」為硬體模

擬再繼續操作

要注意一點選擇硬體模擬時會出現以下畫面

這畫面在選擇硬體模擬時是不能關閉的他代表著硬體的連結訊息以及環境

設定

左側的 COMX選擇自己所連接至電腦的通訊埠

右側代表所使用的震盪器其他不用變更

「Reconnect」代表重新與硬體做連線硬體連線是否順利可以從MALAB-ICD

上的指示燈得知燈亮代表連結成功閃爍代表連結失敗連結不順利就可

以從這邊重新連線直到成功為止

88

「Program」是將已組譯好之程式燒錄進單晶片中

「Option」是其餘更仔細的設定如下圖所示

「Oscillator」需仔細設定代表所使用的震盪器種類

LP低功率振盪晶體（Low Power Crystal）

XT振盪器諧振器（Crystal Resonator）

HS高速振盪器（High Speed Crystal Resonator）

RC電阻電容器（Resistor Capacitor）

有關振盪器的詳細的介紹以及設定可從書籍上深入得知

以下的選擇本專題沒有去研究專題目的只在燒錄可以燒錄就行了以下

功能的選擇都選擇關閉不變更右下勾選的部份

89

參考文獻

1 Microchip PIC16F87X Data Sheet 1999

2 卓恩慶郭建志直流電動機之脈波寬度調變驅動電路研究逢甲大學自動

控制系學士論文民 86

3 施慶隆PIC16F87X微控制器原理實習與專題製作全華科技圖書民 90

4 蔡木村小型電動機全華科技圖書民 83

5 趙春棠PIC單晶片學習秘笈全威圖書有限公司民 91

逢甲大學自動控制工程學系

學年度第學期畢業專題評分表

專題題目

學生姓名

1 論點

內容(30)2 創見

3 深度

分

1 結構

文字(20)2 修辭

3 圖表

分

1 理路分析

口試(50)2 表達能力

3 綜合識見

分

總分分

改進意見

評審教授

i

感謝







ii

中文摘要










iii

Abstract














iv

目錄

























v




















vi

圖目錄









圖 34 UC3843接腳圖13



圖 38 IR2111接腳圖15



圖 312 LM348電路圖17




vii

表目錄


1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







i

感謝







ii

中文摘要










iii

Abstract














iv

目錄

























v




















vi

圖目錄









圖 34 UC3843接腳圖13



圖 38 IR2111接腳圖15



圖 312 LM348電路圖17




vii

表目錄


1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







ii

中文摘要










iii

Abstract














iv

目錄

























v




















vi

圖目錄









圖 34 UC3843接腳圖13



圖 38 IR2111接腳圖15



圖 312 LM348電路圖17




vii

表目錄


1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







iii

Abstract














iv

目錄

























v




















vi

圖目錄









圖 34 UC3843接腳圖13



圖 38 IR2111接腳圖15



圖 312 LM348電路圖17




vii

表目錄


1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







iv

目錄

























v




















vi

圖目錄









圖 34 UC3843接腳圖13



圖 38 IR2111接腳圖15



圖 312 LM348電路圖17




vii

表目錄


1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







v




















vi

圖目錄









圖 34 UC3843接腳圖13



圖 38 IR2111接腳圖15



圖 312 LM348電路圖17




vii

表目錄


1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







vi

圖目錄









圖 34 UC3843接腳圖13



圖 38 IR2111接腳圖15



圖 312 LM348電路圖17




vii

表目錄


1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







vii

表目錄


1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







1

第一章前言














自動化之控制

2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







2
















3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







3










4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







4













USARTSPI及 I2C







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







5




PWM信號等功能







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







6


223 MPLAB 介紹








7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







7





8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







8




9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







9
















制

10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







10




3 功率30W


5 編碼器接線-----紫---+5V

棕---GND

橙---A相

白---B相


11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







11


31 基本架構


Encoder

Input θ+

-int

ω










12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







12



UC3843


T=f1

TT CRf 81

=






13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







13

5UC3843接腳







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







14

TLP250





IR2111



為開關使用



15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







15


電源電路








16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







16




接腳 1+15V

接腳 2-15V


接腳 40V




17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







17

LM384


位
















18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







18

同相


332 PIC16F877電路

PIC







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







19

隻接腳接至 5V








20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







20

第四章程式

41 暫存器介紹


STATUS 狀態














PWM相關暫存器

CCP1CONPWM設定

PR2 設定週期値


21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







21



自定義暫存器




SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a


Sum_M EQU 0x2c

Sum_H EQU 0x2d


MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31


TMP_M EQU 0x33

TMP_H EQU 0x34




22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







22

EncoderH EQU 0x38


Cmd_L EQU 0x3a


Error_H EQU 0x3c


Error1_H EQU 0x3e



Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42



Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46



Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a


MSum_M EQU 0x4c

MSum_H EQU 0x4d


Itemp_M EQU 0x5f

Itemp_H EQU 0x50

23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







23





相關指令介紹


















goto 跳躍指令


24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







24





nop 不執行動作

25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







25


MainLine

Initial設定

AD_Converter

PID_Control

PWM_Output










26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







26

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







27

421 外部中斷





28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







28



swapf STATUSw

movwf STATUS_Temp




計數之副程式


movwf STATUS

swapf W_Tempf

swapf W_Tempw




btfss PORTB1

call Forward

btfsc PORTB1


banksel INTCON

bcf INTCONINTF

return

29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







29


banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

incf EncoderHf

clrf EncoderL

return


banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







30

A相

B相










31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







31

422 AD轉換

AD轉換

ADCON02=1

ADCON02=0


return

No

Yes

AD_Convert

banksel ADCON0


AD_Wait


goto AD_Wait


clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







32

banksel Cmd_L

movwf Cmd_L

return









33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







33


34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







34



中



ADD24


movf TMP_Lw

addwf Sum_Lf


btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf


addwf Sum_Mf


btfsc STATUSC


movf TMP_Hw

addwf Sum_Hf

return

35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







35


MULT16

Count=8

MM20=0

Call ADD24


Count=Count-1

Count=0

Yes

No

return

Yes

No




36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







36














MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L


movwf Count


movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







37

MUL_Loop



bcf STATUSC

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







38







SUB16

banksel SUB1_L


subwf SUB2_Lw

39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







39

movwf temp_L


decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







40




















PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







41

431 Kp_Control






回主程式

42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







42

Kp_Control


movf Cmd_Lw

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H


btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0




movlw 0x01

43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







43

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L


return

44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







44



comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







45

432 Kd_Control

Kd_Control


ERR_SHN1=0

temp=(-1)timestemp

Kd_Term=Kdtimestemp

ERR_SGN1=0


return

Yes

Yes

No

No






46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







46

Kd_Control


movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16



movwf Error1_H

movf Error_Lw

movwf Error1_L


47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







47

433 Ki_Control

Ki_Control


ERR_SGN2=0



ERROR_SGN2=0


return

Yes

Yes

No

No

|ErrorSum_H|=0

No

Yes


48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







48







Ki_Control



btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1


movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







49

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

movf Error_Sum_Hw

movwf TMP_H


50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







50

44 PWM的介紹












Summation

51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







51


movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H


movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

movf Kd_Term_HW

movwf Sum_H



movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H


52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







52

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H



btfss MSum_H7

bsf PORTC3

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







53

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM輸出



54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







54










的目的

PWM_Output


btfss STATUSZ


movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







55

btfsc temp_H7

goto PWM_Limit

bcf STATUSC




rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

goto PWM_Send



movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







56

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







57

45 PIC主程式

PIC主程式

list p=16f877R=DEC


W_Temp EQU 0x20


SUB1_H EQU 0x23

SUB1_L EQU 0x24

SUB2_H EQU 0x25

SUB2_L EQU 0x26

temp_L EQU 0x29

temp_H EQU 0x2a

Sum_L EQU 0x2b

Sum_M EQU 0x2c

Sum_H EQU 0x2d

MM1_L EQU 0x2e

MM1_M EQU 0x2f

MM1_H EQU 0x30

MM2 EQU 0x31

TMP_L EQU 0x32

TMP_M EQU 0x33

TMP_H EQU 0x34

58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







58

ERR_SGN EQU 0x35

Count EQU 0x36

EncoderL EQU 0x37

EncoderH EQU 0x38

Cmd_H EQU 0x39

Cmd_L EQU 0x3a

Error_L EQU 0x3b

Error_H EQU 0x3c

Error1_L EQU 0x3d

Error1_H EQU 0x3e

Kp EQU 0x4f

Kp_Term_L EQU 0x40

Kp_Term_M EQU 0x41

Kp_Term_H EQU 0x42

Kd EQU 0x43

Kd_Term_L EQU 0x44

Kd_Term_M EQU 0x45

Kd_Term_H EQU 0x46

Ki EQU 0x47

Ki_Term_L EQU 0x48

Ki_Term_M EQU 0x49

Ki_Term_H EQU 0x4a

MSum_L EQU 0x4b

MSum_M EQU 0x4c

MSum_H EQU 0x4d

59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







59

Itemp_L EQU 0x5e

Itemp_M EQU 0x5f

Itemp_H EQU 0x50




ErrorOver EQU 0x54

org 0x00

nop

goto MainLine

org 0x04

goto IntService

IntService

movwf W_Temp

swapf STATUSw

movwf STATUS_Temp

banksel INTCON

btfsc INTCONINTF

call Encoder_ISR

swapf STATUS_Tempw

60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







60

movwf STATUS

swapf W_Tempf

swapf W_Tempw

retfie

Encoder_ISR

banksel PORTB

btfss PORTB1

call Forward

btfsc PORTB1

call Reverse

banksel INTCON

bcf INTCONINTF

return

Forward

banksel EncoderL

movlw 0xff

subwf EncoderLw

btfsc STATUSZ

goto Forward1

incf EncoderLf

return

Forward1

61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







61

incf EncoderHf

clrf EncoderL

return

Reverse

banksel EncoderL

movf EncoderLf

btfsc STATUSZ

goto Reverse1

decf EncoderLf

return

Reverse1

decf EncoderHf

decf EncoderLf

return

MainLine

call Initial

MainLoop

call AD_Convert

call PID_Control

call PWM_Output

goto MainLoop

62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







62

Initial

banksel ERR_SGN

clrf ERR_SGN

clrf Error1_L

clrf Error1_H

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

clrf Kp_Term_L

clrf Kp_Term_M

clrf Kp_Term_H

clrf Kd_Term_L

clrf Kd_Term_M

clrf Kd_Term_H

clrf Error_Sum_L

clrf Error_Sum_M

clrf Error_Sum_H

clrf MSum_L

clrf MSum_M

clrf MSum_H

clrf Cmd_L

clrf Cmd_H

movlw 0x6e

movwf Kp

63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







63

movlw 0x80

movwf Kd

movlw 0x01

movwf Ki

banksel TRISC

movlw B00000000

movwf TRISC

banksel PORTC

movlw B00000000

movwf PORTC

movlw 0xff

movwf PR2

banksel TMR2

clrf TMR2

movlw 0x80

movwf CCPR1L

movlw 0x0C

movwf CCP1CON

movlw 0x00

movwf T2CON

bsf T2CONTMR2ON

movlw 0x00

movwf EncoderL

64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







64

movwf EncoderH

banksel OPTION_REG


bsf TRISB0

bsf TRISB1

movlw B10010000

movwf INTCON

banksel ADCON1

movlw B10001110

movwf ADCON1

bsf TRISA0

bcf STATUSRP0

movlw B10000001

movwf ADCON0

return

AD_Convert

banksel ADCON0

bsf ADCON0GO

AD_Wait

btfsc ADCON0GO

goto AD_Wait

movf ADRESHw

65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







65

clrf Cmd_H

movwf Cmd_H

banksel ADRESL

movf ADRESLw

banksel Cmd_L

movwf Cmd_L

return

MULT16

clrf Sum_H

clrf Sum_M

clrf Sum_L

movlw 0x08

movwf Count

movf MM1_Hw

movwf TMP_H

movf MM1_Mw

movwf TMP_M

movf MM1_Lw

movwf TMP_L

MUL_Loop

btfsc MM20

call ADD24

bcf STATUSC

66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







66

rrf MM2f

bcf STATUSC

rlf TMP_Lf

rlf TMP_Mf

rlf TMP_Hf

decfsz Countf

goto MUL_Loop

return

ADD24

banksel TMP_L

movf TMP_Lw

addwf Sum_Lf

movlw 0x01

btfsc STATUSC

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Mw

addwf Sum_Mf

movlw 0x01

btfsc STATUSC

addwf Sum_Hf

movf TMP_Hw

67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







67

addwf Sum_Hf

return

SUB16

banksel SUB1_L

movf SUB1_Lw

subwf SUB2_Lw

movwf temp_L

btfss STATUSC

decf SUB2_Hf

movf SUB1_Hw

subwf SUB2_Hw

movwf temp_H

return

PID_Control

call Kp_Control

call Kd_Control

call Ki_Control

call Summation

return

Kp_Control

banksel Cmd_L

movf Cmd_Lw

68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







68

movwf SUB2_L

movf Cmd_Hw

movwf SUB2_H

movf EncoderLw

movwf SUB1_L

movf EncoderHw

movwf SUB1_H

call SUB16

movf temp_Lw

movwf Error_L

movf temp_Hw

movwf Error_H

bcf ERR_SGN0

btfsc temp_H7

bsf ERR_SGN0

btfss ERR_SGN0

goto Positive

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







69

Positive

movf temp_Hw

movwf ErrorOver

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kpw

movwf MM2

call MULT16

movf Sum_Hw

movwf Kp_Term_H

movf Sum_Mw

movwf Kp_Term_M

movf Sum_Lw

movwf Kp_Term_L

btfss ERR_SGN0

return

comf Kp_Term_Lf

comf Kp_Term_Mf

70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







70

comf Kp_Term_Hf

movlw 0x01

addwf Kp_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Kp_Term_Hf

return

Kd_Control

movf Error_LW

movwf SUB2_L

movf Error_Hw

movwf SUB2_H

movf Error1_Lw

movwf SUB1_L

movf Error1_Hw

movwf SUB1_H

call SUB16

banksel Error1_H

71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







71

movf Error_Hw

movwf Error1_H

movf Error_Lw

movwf Error1_L

bcf ERR_SGN1

btfsc temp_H7

bsf ERR_SGN1

btfss ERR_SGN1

goto Kd_Control1

comf temp_Lf

comf temp_Hf

movlw 0x01

addwf temp_Lf

btfsc STATUSC

incf temp_Hf

Kd_Control1

clrf MM1_H

movf temp_Hw

movwf MM1_M

movf temp_Lw

movwf MM1_L

movf Kdw

movwf MM2

72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







72

call MULT16

movf Sum_Hw

movwf Kd_Term_H

movf Sum_Mw

movwf Kd_Term_M

movf Sum_Lw

movwf Kd_Term_L

btfss ERR_SGN1

return

comf Kd_Term_Hf

comf Kd_Term_Mf

comf Kd_Term_Lf

movlw 0x01

addwf Kd_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Kd_Term_Mf

btfss STATUSC

return

movlw 0x01

73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







73

addwf Kd_Term_Hf

return

Ki_Control

banksel ErrorOver

movf ErrorOverf

btfsc STATUSZ

goto Ki_Control1

clrf Ki_Term_L

clrf Ki_Term_M

clrf Ki_Term_H

return

Ki_Control1

movf Error_Lw

movwf Sum_L

movf Error_Hw

movwf Sum_M

movlw 0xff

movwf Sum_H

btfss Error_H7

clrf Sum_H

movf Error_Sum_Lw

movwf TMP_L

movf Error_Sum_Mw

movwf TMP_M

74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







74

movf Error_Sum_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf Error_Sum_L

movwf Itemp_L

movf Sum_Mw

movwf Error_Sum_M

movwf Itemp_M

movf Sum_Hw

movwf Error_Sum_H

movwf Itemp_H

bcf ERR_SGN2

btfsc Itemp_H7

bsf ERR_SGN2

btfss ERR_SGN2

goto Ki_Control2

comf Itemp_Lf

comf Itemp_Mf

comf Itemp_Hf

movlw 0x01

addwf Itemp_Lf

btfss STATUSC

goto Ki_Control2

movlw 0x01

75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







75

addwf Itemp_Mf

btfss STATUSC

goto Ki_Control2

movlw 0x01

addwf Itemp_Hf

Ki_Control2

movf Itemp_Lw

movwf MM1_L

movf Itemp_Mw

movwf MM1_M

movf Itemp_Hw

movwf MM1_H

movf Kiw

movwf MM2

call MULT16

movf Sum_Lw

movwf Ki_Term_L

movf Sum_Mw

movwf Ki_Term_M

movf Sum_Hw

movwf Ki_Term_H

btfss ERR_SGN2

return

comf Ki_Term_Lf

76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







76

comf Ki_Term_Mf

comf Ki_Term_Hf

movlw 0x01

addwf Ki_Term_Lf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Mf

btfss STATUSC

return

movlw 0x01

addwf Ki_Term_Hf

return

Summation

movf Kp_Term_Lw

movwf TMP_L

movf Kp_Term_Mw

movwf TMP_M

movf Kp_Term_Hw

movwf TMP_H

movf Kd_Term_Lw

movwf Sum_L

movf Kd_Term_Mw

movwf Sum_M

77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







77

movf Kd_Term_HW

movwf Sum_H

call ADD24

movf Ki_Term_Lw

movwf TMP_L

movf Ki_Term_Mw

movwf TMP_M

movf Ki_Term_Hw

movwf TMP_H

call ADD24

movf Sum_Lw

movwf MSum_L

movf Sum_Mw

movwf MSum_M

movf Sum_Hw

movwf MSum_H

btfsc MSum_H7

bcf PORTC3

btfss MSum_H7

bsf PORTC3

78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







78

btfss MSum_H7

return

comf MSum_Lf

comf MSum_Mf

comf MSum_Hf

movlw 0x01

addwf MSum_Lf

btfss STATUSC

return

movlw 0x01

addwf MSum_Mf

btfss STATUSC

return

movlw 0x01

addwf MSum_Hf

return

PWM_Output

movf MSum_Hf

btfss STATUSZ

goto PWM_Limit

79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







79

movlw 0x00

movwf SUB2_L

movlw 0x04

movwf SUB2_H

movf MSum_Lw

movwf SUB1_L

movf MSum_Mw

movwf SUB1_H

call SUB16

btfsc temp_H7

goto PWM_Limit

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

bcf STATUSC

rrf MSum_MF

rrf MSum_LF

bcf STATUSC

rrf MSum_Mf

rrf MSum_Lf

goto PWM_Send

80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







80

PWM_Limit

banksel CCPR1L

movlw 0xff

btfsc PORTC3

movwf CCPR1L

btfss PORTC3

clrf CCPR1L

return

PWM_Send

banksel CCPR1L

movlw 0x80

movwf CCPR1L

movf MSum_Lw

btfsc PORTC3

addwf CCPR1Lf

btfss PORTC3

subwf CCPR1Lf

return

END

81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







81

第五章結論














82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







82

附錄 A 中斷










83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







83

附錄 B AD轉換





始轉換了

84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







84










85












86







87














擬再繼續操作



設定






88











89

參考文獻







85












86







87














擬再繼續操作



設定






88











89

參考文獻







86







87














擬再繼續操作



設定






88











89

參考文獻







87














擬再繼續操作



設定






88











89

參考文獻







88











89

參考文獻







89

參考文獻







逢甲自動控制工程學系專題製作專題論文 ·...

Documents

Transcript of 逢甲自動控制工程學系專題製作專題論文 ·...

逢 甲 自動控制工程學系專題製作 專 題 論 文 ·...

Documents

Transcript of 逢 甲 自動控制工程學系專題製作 專 題 論 文 ·...

逢甲自動控制工程學系專題製作專題論文 ·...

Transcript of 逢甲自動控制工程學系專題製作專題論文 ·...