Diff: krs.cpp

Revision:

3:e356b3e7ecfd

Child:

4:b4c8d6671241

diff -r 33681dfc2aa5 -r e356b3e7ecfd krs.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/krs.cpp	Tue Dec 24 14:56:31 2019 +0000
@@ -0,0 +1,85 @@
+#include "mbed.h"
+#include "krs.h"
+
+krs::krs(PinName tx, PinName rx, PinName io, int baud) : master(tx,rx),en(io)
+{
+    master.baud(baud);
+    if(baud==112500){
+        waittime=0.0002;
+        }else if(baud==1250000){
+            waittime=0.00002;
+            }
+    master.format(8, Serial::Even, 1);
+    en.output();
+    }
+
+bool krs::setpos(int deg,int id){
+    txlen=3;
+    rxlen=3;
+    int txdata[txlen];
+    int rxdata[rxlen];
+    en=1;
+    
+    if(deg>135 || deg<-135){
+        printf("failed\r\n");
+        return 0;
+        }
+    int pos=deg*29.633;
+    pos=pos+7500;
+    
+    txdata[0]=0x80+id;
+    txdata[1]=((pos >> 7) & 0x007F);
+    txdata[2]=(pos & 0x007F); 
+    
+    for(int i=0;i<txlen;i++){
+        master.putc(txdata[i]);
+        }
+        wait(waittime);
+        //wait(0.00002);//1250000bps用
+        //wait(0.0002);
+        
+       en=0;
+        for(int i=0;i<rxlen;i++){
+            rxdata[i]=master.getc();
+            }
+    
+    int degnow;  
+    degnow=((rxdata[1] << 7) & 0x3F80) + (rxdata[2] & 0x007F);
+    degnow=(degnow-7500)/29.6;
+   
+    /*pc.printf("%d\r\n",rxdata[0]);
+    pc.printf("%d\r\n",rxdata[1]);
+    pc.printf("%d\r\n",rxdata[2]);*/
+    printf("角度%d\r\n",degnow);
+    }
+
+void krs::getpos(int id){
+    txlen=2;
+    rxlen=4;
+    
+    en=1;
+    unsigned char txdata[txlen];
+    unsigned char rxdata[rxlen];
+    int degnow;
+    
+    txdata[0]=0xA0+id;
+    txdata[1]=0x05;
+    
+    
+    for(int i=0;i<txlen-1;i++){
+        master.putc(txdata[i]);
+        }
+    wait(waittime);
+    en=0;
+    
+    for(int i=0;i<rxlen+1;i++){
+        rxdata[i]=master.getc();
+        }
+
+    printf("%d\r\n",rxdata[0]);
+    degnow=((rxdata[2] << 7) & 0x3F80) + (rxdata[3] & 0x007F);
+    degnow=(degnow-7500)/29.6;
+    printf("%d\r\n",degnow);
+    
+    } 
+ 
\ No newline at end of file