❤ 2019.11.13
之前寫過各個模塊的測試程序，但是並沒有將各個模塊整合起來，所以總體來說工作量還是很大的。
❤ 2019.11.13
〇 總體流程
這個版本的目標是能夠實現電機隨上位指令執行相應動作。
其實說起來挺簡單，我接收上位指令，然後控制驅動器，再接收反饋，再發送反饋信息。
〇 總體思路
要實現總體目標，首先要把目標拆分成一個一個的小目標（此小目標非彼小目標。。。）。
我的計劃是：（可能會有改動）
首先實現接收並解析上位機指令（之前已經實現了部分），然後實現同時控制多個驅動器，然後實現讀取驅動器狀態，然後實現讀取5600模擬量，最後實現向上位發送反饋信息。
❤ 2019.11.14
〇 接收上位指令程序（先實現接收並解析舵機角度指令）
♣ 第一步，先確定CAN指令的ID。
雖然有通訊協議，但是鑑於過於難以理解（或者說懶得看。。。），我決定用can分析儀直接測試。。。
因爲上位通訊佔用了一個，所以還得用一個Pcan來接收。
這個是四個舵機的位置信息對應的can指令：
從這裏可以看出的是，舵機的靜態控制指令分爲兩部分，第一條ID爲02A82000，包含指令次數信息，第二條ID爲02AC2001，包含四個舵機的角度信息，關於角度和其雙字節數據的對應關係並不能直觀的看出來（其實之前我已經將其“破解”了）。
在這裏其實我不太瞭解這個指令次數的含義。
首先把我破解的上位機角度數據的格式記錄一下。一條Can指令共8個字節，包含四個舵機的角度指，每個角度值爲16位有符號整型數值，傳輸方式爲低位優先，最小分辨率爲0.002°。
♣ 第二步，配置cana和canb
因爲我在我設計的PCB上CanA是用來和上位通訊，CanB用來和驅動器通訊，但是研旭的開發板只引出了CanB，所以我要先把can總線驅動匹配到我的pcb上，因爲兩個can口負責兩種不同的工作，所以我決定把can驅動分成兩個文件，分別配置CanA和CanB。
這裏要特別說明一下，更改can總線的輸出引腳的時候一定要重新配置下GPIO的設置，雖然是常識，但是我之前調試的時候就在這裏卡住了好久，怎麼都不能在研旭的開發板上把ti的例程調試成功，切記！
都要改！
修改過之後經過一些調試，已經可以實現CabB的正常通訊，我用的是ti的官方例程進行的測試，不要問爲什麼，問就是懶！
但是在我設計的pcb中，和上位機通訊的是CanA，所以這裏先調試CanA口。
首先把can的驅動程序分成CanA和CanB。
→→→→→
但是這時候，我突然想到，這樣的話我需要改很多東西，於是我又有了偷懶的想法，我把之前的bsp_can改成bsp_canb，然後新建一個bsp_can，把新的bsp_cana和和bsp_canb都include進去。
不過還是需要改好多東西。。。
讓後就變成了這樣。。。
編譯一下，出錯了
還沒有改bsp_cana裏的內容，下面把裏面的canb都換成cana。
在把文件裏的canb修改成cana之後，記得修改庫文件裏cana的初始化參數，比如波特率什麼的。
這個是把波特率配置成500k，其實要求的是1M，不過先用較低速率調試嘛。
除了cana的初始化函數之外，還需要修改cana的中斷，canb的中斷我選擇了中斷線0.所以cana我選用了中斷線1,（關於中斷線0和1有什麼區別和作用我還不是很瞭解，能不能同時用0或者1我也沒試過，，，（等會兒可以試試。。。））。
先要修改bsp_cana裏面的初始化函數的中斷配置部分。
------------------------------------------------------------------------------------------------------------------------------------------------------
這裏有個小地方注意一下，如果把中斷改到中斷線1，那麼設置是0xFFFFFFFF，而不是1.。。。（感覺自己好傻。。）
另外我看別人說，中斷線0比中斷線1的優先級要高，所以一般把系統級的中斷，比如溢出、錯誤、丟失等，配置到中斷線0，把郵箱中斷配置到中斷線0.
------------------------------------------------------------------------------------------------------------------------------------------------------
然後修改外部中斷配置文件bsp_pie中的部分。
另外還有GPIO的配置，我選用的是GPIO18和19作爲CanA的端口。
然後修改下主函數，編譯，測試，OK~非常完美！
♣ 第三步，編寫舵機角度指令解析程序。
首先說一個我發現的雖然沒什麼用但是讓我很困惑的事情，can總線模塊的郵箱的MBOX這個結構體有個奇怪的地方，就是就是ReceiveData的字節和字不是對應的，這可能和低位優先和高位優先有關係。
其實獲取角度數據還是很容易的，首先定義一個全局變量，然後把can收到的數據存進去就可以。
全局變量。
關於volatile這個關鍵字今天學到了很多內容，等我稍後整理一下。
這個是接收中斷
上位指令除了舵機角度數據外，其實還有比如說自檢指令、區分單次舵機控制和多次舵機控制的指令等等，不過爲了能夠儘快進行整體調試，我打算先能實現整個流程，然後再來完善細節，所以接下來先調試DSP與驅動器的通訊以及控制多個舵機的功能。
〇 與驅動器的通訊及多驅動器控制程序（僅包括角度控制）
首先介紹一下老師選擇的厲害的不要不要的驅動器。
而且這個驅動器還有自己的軟件可以編程序，感覺完全不需要再加個dsp嘛，（而且他本身就是個dsp。。。），這個驅動器用在這裏真是大材小用，但是沒辦法，誰讓這是老師的想法呢，我就是個幹活的。。。。
♣ 第一步，確定驅動器的can總線ID及指令格式
確定can總線的ID及指令格式，需要用他的軟件EasyMotionStudio。
把驅動器連接好後，啓動軟件。
設置好電機參數巴拉巴拉。
我選擇“梯形”模式，配置界面如下。
關於電機的控制方式，昨天我和朱工探討過，這個驅動器是支持絕對位置動作的，也就是用內置的霍爾開關編碼器。但是由於內部編碼器是安裝在電機輸出軸上的，最終輸出的精度受到減速器精度的影響，所以外部編碼器還是有必要的，也就是說，不可以直接把上位指令的角度信息直接發送給驅動器，而需要將上位指令用外部編碼器的反饋信息進行修正之後再發送給驅動器，至於其處理方法在後面再說。
但是，這裏主要是測試通訊，ad模塊還沒有加進來，所以這裏先用絕對位置做測試。
上面的參數設置對應的語句如下
語句對應的can指令如下
可以看出，這一段指令用了7個ID，如果再加上用來反饋驅動器狀態的語句至少還要加上2個ID，但是F28335每個can線路只有32個mailbox，四個驅動器平均每個只能分到8個ID，也就是說，不夠。不過在我試驗之下發現有的語句是可以省略的。
首先加速的和速度的設置，爲了發揮出電機的最大性能，這裏一般應該設置爲最大值（具體是多少我也不知道）
然後這幾句，我也不知道是幹嘛的，試了一下沒有也可以動。
UDP是必須的，相當於execute。
這兩句是等待運行完成，應該也可以省略，但是不知道會不會有其他影響。
所以精簡一下對應的can指令就是這要的，這是can信號分析儀的指令格式。
0480209E 00 0F 00 00 ext data
00002108 ext data
讓後把其他驅動器做相同的配置，並且記錄相應ID。
♣ 第二步，配置dsp的canb初始化程序編寫發送函數
首先配置郵箱ID和發送字符數
♣ 第二步，編寫發送函數
發送函數包含接收到的角度數據轉換爲驅動器能識別的角度數據格式，運行的角度數據寫入郵箱發送緩存，執行發送幾部分。
上位發送來的角度數據是16位有符號整型，-20°~+20°（但是老師說的是運動範圍只有20°。。。待確認），分辨率0.002°，對應數值範圍是-10000~+10000，驅動器識別的角度格式爲32位有符號整型，但是因爲有減速比的設置，指令的角度數據是實際運行角度乘以48，分辨率0.1°（分辨率有點低。。。）。
所以換算一下就是：運行角度數據 = 指令角度數據 / 500 * 48
下面建立全局變量
再編寫發送函數
運行測試，預料之中的並沒有成功，查看can指令發現了問題
順序反了。。。也就是說發送郵箱的發送順序是從大數字到小數字的。
下面我把順序調整過來。
運行測試，bingo！
❤ 2019.11.28
〇 讀取5600反饋
本來計劃先讀取驅動器狀態，不過之前已經實現驅動器向dsp發送狀態信息，所以這裏就先不做這個，先讀取5600模擬量。
在正是編程序之前，先把控制器裝到盒子裏去，因爲早晚要裝的啊哈哈哈。。。。
。。。。
在經過幾天的調試與糾結之後， 終於塞進去了。。。。
雖然出了一些小問題，但是總算能跑之前的程序了，但是電機不轉！
一陣排查之後發現最可能的問題是，我把驅動器的Reset和Enable引腳和DSP連接了，根據說明書，Reset引腳是高電平有效，Enable也是高電平有效，然後DSP的IO口默認是高電平，所以驅動器一直處於Reset狀態。
所以先要寫一段IO口的控制程序。
很簡單嘛~（爲後面被打臉埋下了伏筆。。。）
首先把相應IO口配置爲普通IO口，然後把方向配置爲輸出。
然後通過宏定義把IO口的輸入輸出寄存器定義爲變量。
然後在初始化語句中加入IO口的初始值。
運行，然後問題出現了。。。IO的輸出並不是和我所設置的一樣的。。。。
然後去掉宏定義，直接用寄存器設置。。。並不行
但是我只用一條語句是可以的，多條語句放在一起就不行了。。。
啊啊啊啊啊啊啊啊啊啊。。。
後來我放棄用這個寄存器了，雖然在手冊裏說是可以的
然後我還是用了GPxSET和GPxCLEAR寄存器。。。
雖然沒解決問題，但是程序暫時可以用了，只是如果需要對IO口進行操作會比較不一樣。
主要部件都可以裝在盒子裏了，下面就開始正式寫adc功能的代碼。
先把adc功能添加到程序裏，再編寫函數，實現角度的讀取和轉換，最後編寫通過5600反饋的閉環控制函數
♣ 第一步添加adc功能
首先把之前調試好的bsp_ADC文件添加到工程裏，能在工程裏讀出各驅動器5600的反饋值。
話說，從模塊化可移植的角度來說，相關模塊的變量應該放在相應的bsp文件裏定義與聲明吧。。。。
初始化程序
void ADC_Init(void)
{
EALLOW;
SysCtrlRegs.HISPCP.all = ADC_MODCLK; // HSPCLK = SYSCLKOUT/ADC_MODCLK，這個是設置全局的高速外設的工作頻率，按說不應該在這裏設置的,但是其他地方沒辦法設置6分頻，所以只能在這裏設置了
// HSPCLK=25MHz， 將高速外設的工作頻率設置爲6分頻（25MHz），ADC工作的最高工作頻率可以配置爲25MHz。
EDIS;
InitAdc(); //官方ADC初始化函數
AdcRegs.ADCTRL1.bit.ACQ_PS = ADC_SHCLK; // 設置採集窗口的大小（具體有什麼影響還不清楚），Sequential mode: Sample rate = 1/[(2+ACQ_PS)*ADC clock in ns]
// = 1/(3*40ns) =8.3MHz (for 150 MHz SYSCLKOUT)
// = 1/(3*80ns) =4.17MHz (for 100 MHz SYSCLKOUT)
// If Simultaneous mode enabled: Sample rate = 1/[(3+ACQ_PS)*ADC clock in ns]
AdcRegs.ADCTRL3.bit.ADCCLKPS = ADC_CKPS; //ADC工作在25MHz下，不分頻（這裏沒有辦法設置成3分頻或者6分頻，所以在前面設置了總的高速外設頻率25MHz）
AdcRegs.ADCTRL1.bit.SEQ_CASC = 1; // 級聯模式，SEQ1和SEQ2級聯起來，作爲一個16狀態序列發生器操作，1 Cascaded mode，（我只需要4個採集端口，所以其實不需要級聯，不過有什麼影響還不清楚）
AdcRegs.ADCTRL1.bit.CONT_RUN = 1; // 連續採樣模式，Setup continuous run
AdcRegs.ADCTRL1.bit.SEQ_OVRD = 1; // 使能排序覆蓋，Enable Sequencer override feature，（具體有什麼區別我還沒太清楚）
AdcRegs.ADCMAXCONV.bit.MAX_CONV1 = 0x3; // 設置ad轉換通道數量，4個，（n+1），因爲是級聯模式，所以只用CONV1
// 設置採樣通道
AdcRegs.ADCCHSELSEQ1.bit.CONV00 = 0x2; //驅動器1反饋
AdcRegs.ADCCHSELSEQ1.bit.CONV01 = 0x4; //驅動器2反饋
AdcRegs.ADCCHSELSEQ1.bit.CONV02 = 0x6; //驅動器3反饋
AdcRegs.ADCCHSELSEQ1.bit.CONV03 = 0x0; //驅動器4反饋
//AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1 = 1; //使能SEQ1中斷
//AdcRegs.ADCTRL2.bit.INT_MOD_SEQ1 = 0; //設置SEQ1中斷方式，每個SEQ1序列轉換結束時，置位SEQ1的中斷標誌位
AdcRegs.ADCTRL2.bit.SOC_SEQ1 = 1; //啓動SEQ1轉換序列
}
這個初始化程序基本就是ti的官方程序稍作修改，有個地方需要注意下就是adc模塊的最大速率是25MHz，如果系統時鐘是150MHz的話需要6分頻，adc掛載的高速外設總線可以直接設置6分頻，但是這會影響其他的外設的速率設置。adc自帶的時鐘可以設置分頻係數，但是無法設置3分頻或6分頻，也就是說想要得到最高的25NHz只能在高速外設總線那裏設置6分頻。這個地方以後如果有其他外設的話需要注意，不過這個工程裏沒啥影響，因爲只有adc。
然後在主函數裏添加測試語句段
測試結果符合預期。
♣ 第二步，編寫角度讀取與轉換程序
因爲5600磁編碼器存在輸出不穩定的情況，所以接收到的模擬量需要濾波，這裏用算術平均值法，讀取若干次，然後取平均值，至於這個方法造成的延遲問題後面再說吧。。。
首先把驅動器設置好，把絕對位置控制修改爲相對位置。爲什麼要先修改驅動器程序呢？因爲dsp的ad模塊最大輸入電壓是3V，雖然我查到資料說超過3V的部分會一直顯示爲最大值，但是書上說會燒壞adc模塊。。。。好吧，寧可信其有，先通過驅動器設置調整電機的位置，使5600的輸出值位於1.5V附近，5600用3.3V驅動時最大輸出是3.3V，舵機有效行程是+-20°，所以這樣就不會超過量程了。
在用EasyMotionStudio連接驅動器的時候出現了意想不到的事情。如果直接用USB轉232串口連接驅動器一切正常，但是把驅動器裝到安裝板上，再用USB轉232連航空插頭上的引出的數據線時就會出現無法建立連接的情況。然而，當DSP處於運行狀態，然後讓電機動一動，不管動的是不是引出232串口線的那個驅動器，然後再連接就正常了。。。我去，這是什麼鬼問題。。。。
我去！我天！我把5600接的是5V！我的媽呀！雖然說舵機的工作範圍比較小，但是增大了超過額定電壓的概率和幅度，萬一不小心。。。墨菲定律。。。
我計算了一下，
運行角度按照+-20°來算，就是40°，40/360=1/9, 5*1/9約=0.55V，這個變化幅度挺小的。
爲了不讓adc超出量程，我把舵機位置調整到編碼器輸出電壓的30%，5*0.3=1.5V，這樣（應該）就不會有問題了。
但是我有個新問題，電壓變化幅度這麼小，精度夠麼？
我計算了一下。
在使用5V供電的情況下，0.55/3約=0.18，0.18*4095（F28335AD模塊3V電壓對應的輸出值）=737（個值），
但是呢，我做了個實驗，在舵機位置不動的情況下，連續讀取多次adc模塊採集的結果，每次採集10個結果，記錄每次的最大值與最小值，連續採集10次。
然後取所有值裏面的最大值和最小值，相減，除以最大運動範圍對應的結果數，得到驚人的10.6%！
也就是說誤差居然達到了10%！！！
不知道現在這個方案還有沒有做下去的必要了。。。。
我想了幾個修改方案，
1、使用放大電路，把輸出信號的幅值放大到0-2.5V左右，缺點是放大係數有一定的誤差，導致最終結果可能誤差較大
2、使用驅動器的編碼器，不使用外置編碼器，然後精確測量傳動比，提高半閉環控制精度，缺點長時間運行之後可能會有累積誤差
3、使用5048的數字信號反饋，避免adc模塊帶來的二次誤差，缺點是沒有根本改變運動幅值太小的問題。後來我有用電位器測試了一下，adc的誤差是很低的，基本在5以內，也就是說主要的誤差來自於編碼器。
4、朱工說給ad端口加個500歐並聯電阻，降低ad的阻抗，雖然沒明白能起到什麼作用，但是好厲害的樣子。
5、想到再說
我想了一下，還想確實應該在adc的輸出端口加個接地的電阻，這樣在不接編碼器的時候adc讀到的數據就是0，現在的情況是如果不接編碼器，adc的讀書就是一個不確定值，可能會對後面的程序有影響
不過呢，還是要把這個部分的工作完成的。
○ 首先要有個讀取舵機初始位置並將其設置爲零點的程序。
首先讀取一定次數的當前編碼器的輸出值，然後求算數平均數，並將其保存到相應變量中。
獲取原點函數
void ServoOrigin_Init(void)
{
Uint16 index = 0;
Uint16 Reading = 1; //正在讀取編碼器反饋電壓標記
//讀取一定次數的編碼器反饋值，每次讀取操作中間間隔10us（需要麼？我也不知道）
while(Reading)
{
while(AdcRegs.ADCST.bit.INT_SEQ1 == 0);
AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1;
InitSampleTableServo1[index] = ( (AdcRegs.ADCRESULT0)>>4);
InitSampleTableServo2[index] = ( (AdcRegs.ADCRESULT1)>>4);
InitSampleTableServo3[index] = ( (AdcRegs.ADCRESULT2)>>4);
InitSampleTableServo4[index] = ( (AdcRegs.ADCRESULT3)>>4);
SumServo1 += InitSampleTableServo1[index];
SumServo2 += InitSampleTableServo2[index];
SumServo3 += InitSampleTableServo3[index];
SumServo4 += InitSampleTableServo4[index];
index++;
if(index==INIT_BUF_SIZE)
{
index = 0;
Reading = 0;
}
DELAY_US(10);
}
//計算讀到的值的算數平均數
OriginServo1 = SumServo1 / INIT_BUF_SIZE;
OriginServo2 = SumServo2 / INIT_BUF_SIZE;
OriginServo3 = SumServo3 / INIT_BUF_SIZE;
OriginServo4 = SumServo4 / INIT_BUF_SIZE;
}
不過在這個過程中我遇到了一個問題，就是把所有數據求和的時候，得到的數據很小，一開始以爲是方法的問題，就換成了for循環，但還是不行，想了想，一定是溢出了。我之前定義的變量是16位無符號整型，換成32位無符號整型，OK~
○ 然後就是讀取當前舵機角度程序
我的計劃是ad轉換一直進行，每完整的採集一次完整的數據之後就轉換爲角度數據存儲在變量裏。
首先關於5600的返回值，在之前的測試中我發現adc讀取到的數值非常不穩定，最大值和最小值之差非常大，（誤差達到了驚人的10%），但是在用萬能表測試發現輸出的波動並不大，朱工用示波器檢測後，發現5600的輸出值整體來說還是比較穩定的，但是每隔15us左右就會產生一次非常大的毛刺，其波動值甚至達到了伏級！F28335的adc模塊最高工作頻率是25MHz，採樣帶寬12.5MHz，其實最開始我對這個參數是沒有什麼概念的，但是經朱工一提醒，我算了一下，12.5MHz意味着adc最快採集速度是每0.08us一次，雖然我用多次採集求算術平均值的方法以減少干擾和誤差，但是由於ad採集速度太快，以至於很有可能採集的數據全在5600輸出值發生強烈波動的區間上，（至於波動的原因我不是很清楚，我覺得最好用硬件的方法濾掉，但是朱工說很難），這樣因爲採集的數據中包含的誤差信號過多，所以即使用算術平均值濾波也無法得出5600的實際輸出值。
最後朱工建議的方法就是像初始化的時候一樣，每隔10us採集一次，一共採集100次，再求算術平均值，這要採集一次用約1ms的時間，上位指令的發送週期是5ms，這要從理論上既保證了採集到的數據的準確度，又滿足了響應速度的要求。不過我覺得這沒有最大發揮F28335的性能，應該用實時採集，然後過濾掉突變數據，只保留有效數據的方法，這個以後再說吧。
目前的讀取舵機當前角度數據的函數是這樣的。
void ReadCurrentAngle(void)
{
ReadingEncoder = 1;
array_index = 0;
while(ReadingEncoder)
{
while(AdcRegs.ADCST.bit.INT_SEQ1 == 0);
AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1;
SampleTableServo1[array_index] = ( (AdcRegs.ADCRESULT0)>>4);
SampleTableServo2[array_index] = ( (AdcRegs.ADCRESULT1)>>4);
SampleTableServo3[array_index] = ( (AdcRegs.ADCRESULT2)>>4);
SampleTableServo4[array_index] = ( (AdcRegs.ADCRESULT3)>>4);
SumServo1 += SampleTableServo1[array_index];
SumServo2 += SampleTableServo2[array_index];
SumServo3 += SampleTableServo3[array_index];
SumServo4 += SampleTableServo4[array_index];
array_index++;
if(array_index==BUF_SIZE)
{
ReadingEncoder = 0;
array_index = 0;
ServoAngle1 = (int16)(SumServo1 / BUF_SIZE - OriginServo1) * 26.3736264;
ServoAngle2 = (int16)(SumServo2 / BUF_SIZE - OriginServo2) * 26.3736264;
ServoAngle3 = (int16)(SumServo3 / BUF_SIZE - OriginServo3) * 26.3736264;
ServoAngle4 = (int16)(SumServo4 / BUF_SIZE - OriginServo4) * 26.3736264;
//這個係數是，當前相對零點的讀數值設爲n，n / 4095 * 3 /(5 / 360) / 0.002,
SumServo1 = 0;
SumServo2 = 0;
SumServo3 = 0;
SumServo4 = 0;
}
}
DELAY_US(READ_DELAY);
}
在測試過程中遇到了個小問題，最後得到的角度數據總是溢出，而且是一會兒正常，一會兒溢出。根據經驗我知道是一個計算式中包含不同種數據格式產生的問題。
最開始我是這要寫的，SumServo1爲無符號整型，BUF_SIZE是個整數，OriginServo1是個無符號整型，ServoAngle1是個整型
經過各種分析與試驗，最後發現是在減法運算時，如果結果大於0，那就是正常的，如果結果小於0，因爲參與運算的都是無符號類型數據，於是會把有符號的數值存爲無符號數據再繼續參與運算，於是就出錯了。
然後改了一下，好了
♣ 第三步，修改舵機角度發送程序，使系統根據當前位置與上位指令，控制舵機以相對位置方式運行。
只要把之前的canb發送程序裏面的運行角度值由接收到的角度值直接運算改爲先減去當前角度值在進行運算就可以了
void CANB_SendDegreeRelative(void)
{
if(AngleDataAvailable == 1) //檢測上位角度數據指令標記，1爲有效，0爲無效
{
MotionAngle1 = (InstructionAngle1 - ServoAngle1) / 500 * 48; //將上位角度指令轉化爲驅動器能識別的格式
MotionAngle2 = (InstructionAngle2 - ServoAngle2) / 500 * 48;
MotionAngle3 = (InstructionAngle3 - ServoAngle3) / 500 * 48;
MotionAngle4 = (InstructionAngle4 - ServoAngle4) / 500 * 48;
ECanbMboxes.MBOX1.MDL.all = MotionAngle1; //將角度值存入發送緩存
ECanbMboxes.MBOX5.MDL.all = MotionAngle2;
ECanbMboxes.MBOX9.MDL.all = MotionAngle3;
ECanbMboxes.MBOX13.MDL.all = MotionAngle4;
ECanbRegs.CANTRS.all = 0x00003333; //啓動發送
AngleDataAvailable = 0; //上位角度數據有效位清零
}
}
同時驅動器程序也要從絕對位置改爲相對位置。
運行，然後就出問題了，當我不斷髮送同一位置的指令時，理論上應該只動一次，但是舵機卻一直會動。
通過調試，我發現當我發送+10°的指令時，dsp讀到的位置卻是負的，也就是說編碼器的方向和電機的運行方向是相反的。
我測試了一下，電機正向運行時，面對輸出軸時，輸出軸順時針運動，（面對電機的霍爾開關編碼器時，也是順時針運動，此時面對編碼器時，在驅動器設置界面，編碼器運動的顯示方向與實際方向是一致的）。當電機正向運行時，5600輸出的電壓是逐漸減小的，也就是說，面向輸出軸時，編碼器的方向是逆時針方向爲正。按照右手定則，應該是逆時針爲正，所以得出的結論，電機的運行方向是反的。
我最開始打算直接把電機的正負極對調，但是轉念一想，這樣電機的運行方向和電機編碼器的正方向就相反了，於是只好在程序裏做調整。
因爲默認5600方向與指令方向相同，所以需要修改的就是電機的運行方向。
在運行角度賦值這裏加個負號，搞定~
但是，但是，又出問題了！理論上來講，上位指令精度達到了0.002°，5600反饋的精度有40°/737（前面算過）約=0.054°，驅動器運行角度這裏稍微有點問題，就是隻能是整數，但是朱工通過設置減速比使角度能夠達到0.1°，但是在測試過程中，上位指令變動幅度大於1°時，dsp纔會發出動作信號，這不科學呀！
經過分析，我敏銳的感覺，這個又是因爲運算過程中數據類型的限制引發的錯誤！
經過排查，發現問題在這裏
先做除法相當於使精度降低了兩個數量級，改一下
但是，但是！問題又又又出現了！現在舵機會出現一種情況，就是在連續發送指令的時候沒有問題，但是我直接輸入某一個角度的時候，舵機不響應或者只轉動很小的角度。這又是什麼神仙問題！
經過排查，我覺得問題依然出自這幾個角度計算式中，我把每一步分開，用temp1到3來分別緩存每一步的結果
我以爲會在某一步裏出現問題，但是結果卻是沒有問題！我去，爲啥連起來算就不行，分開算就行！？
這時，我略一沉思，發現事情非常簡單，還是數據類型的問題。指令角度和當前角度都是16位整型，當變化角度很小的時候，他們相減的值乘以48沒有問題，但是當變化角度比較大時，他們相減的值乘以48就會溢出。
所以要在相減之後把數據類型強制轉換爲32位整型，如下
暫時可以用了。。。希望別再有問題了。。。
精度現在沒啥問題了，可是又有了新的問題，抖動。。。。角度變化速率慢的時候一直抖，抖得很嚴重。。。。很明顯是5600信號不穩定的問題。。。。看來只好改濾波算法了。