File content as of revision 3:e356b3e7ecfd:

#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);
    
    }