File content as of revision 7:c8bf191a325d:

#include "mbed.h"
#include "krs.h"
#include "param.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(float 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);
     #if 1 
       en=0;
        for(int i=0;i<rxlen;i++){
            rxdata[i]=master.getc();
            }
    #if 1
    int degnow;  
    degnow=((rxdata[1] << 7) & 0x3F80) + (rxdata[2] & 0x007F);
    degnow=(degnow-7500)/29.6;
   
   
    printf("%d\r\n",rxdata[0]);
    printf("%d\r\n",rxdata[1]);
    printf("%d\r\n",rxdata[2]);
    printf("角度%d\r\n",degnow);
    #endif
    #endif
    }
    
int krs::getpos(int id){
    en=1;
    txlen=2;
    rxlen=4;
    unsigned char txdata[txlen];
    unsigned char rxdata[rxlen];
    int degnow;
    
    txdata[0]=0xA0+id;
    txdata[1]=0x05;
    
    
    for(int i=0;i<txlen;i++){
        master.putc(txdata[i]);
        }
    wait(waittime);
    //wait(0.00002);
    en=0;
    
    for(int i=0;i<rxlen;i++){
        rxdata[i]=master.getc();
        }

    printf("motor ID: %d",rxdata[0]&0x1F);
    degnow=((rxdata[2] << 7) & 0x3F80) + (rxdata[3] & 0x007F);
    degnow=(degnow-7500)/29.6;
    printf(" angle:%d\r\n",degnow);
    return degnow;
    } 

void krs::getid(){
    
    txlen=4;
    rxlen=1;
    unsigned char txdata[txlen];
    unsigned char rxdata[rxlen];
    
    en=1;
    txdata[0]=0xFF;
    txdata[1]=0x00;
    txdata[2]=0x00;
    txdata[3]=0x00;
    
    for(int i=0;i<txlen;i++){
        master.putc(txdata[i]);
        }
    wait(waittime);
    en=0;
    
    for(int i=0;i<rxlen;i++){
        rxdata[i]=master.getc()&0x1F;
        }
    printf("motor id is %d\r\n",rxdata[0]);
    }

void krs::setid(int id){
    txlen=4;
    rxlen=1;
    unsigned char txdata[txlen];
    unsigned char rxdata[rxlen];
    
    en=1;
    txdata[0]=0xE0 + id;
    txdata[1]=0x01;
    txdata[2]=0x01;
    txdata[3]=0x01;
    
    for(int i=0;i<txlen;i++){
        master.putc(txdata[i]);
        }
    wait(waittime);
    en=0;
    
    for(int i=0;i<rxlen;i++){
        rxdata[i]=master.getc()&0x1F;
        }
    printf("motor id is %d\r\n",rxdata[0]);
    
    }

void krs::setspeed(int id){
    txlen=3;
    rxlen=3;
    unsigned char txdata[txlen];
    unsigned char rxdata[rxlen];
    
    en=1;
    txdata[0]=speed_cmd+id;
    txdata[1]=speed_sc;
    txdata[2]=max_speed;
    
    for(int i=0;i<txlen;i++){
        rxdata[i]=master.putc(txdata[i]);
        }
    wait(waittime);
    en=0;
    
    for(int i=0;i<rxlen;i++){
        rxdata[i]=master.getc();
        }
    printf("motor %d speed is %d now\r\n",id,rxdata[2]);
    }

 void krs::setstrech(int id,int strech){
    txlen=3;
    rxlen=3;
    unsigned char txdata[txlen];
    unsigned char rxdata[rxlen];
    
    en=1;
    txdata[0]=strech_cmd+id;
    txdata[1]=strech_sc;
    if(strech==1){
    txdata[2]=weak_strech;
    }else if(strech==2){
        txdata[2]=mid_weak_strech;
        } else if(strech==3){
                txdata[2]=mid_strech;
                }else{
                    txdata[2]=max_strech;
                    }
                
    master.putc(txdata[0]);
    master.putc(txdata[1]);
    master.putc(txdata[2]);

    wait(waittime);
    en=0;
    
    for(int i=0;i<rxlen;i++){
        rxdata[i]=master.getc();
        }
    printf("motor %d strech is %d now\r\n",id,rxdata[2]);
    }
    
void krs::set_slavemode_off(int id){
    txlen=4;
    rxlen=2;
    
    unsigned char txdata[txlen];
    unsigned char rxdata[rxlen];
    
    en=1;
    txdata[0]=efform_cmd+id;
    txdata[1]=efform_sc;
    txdata[2]=slave_off_up;
    txdata[3]=slave_off_down;
    
    for(int i=0;i<txlen;i++){
        master.putc(txdata[i]);
        }
    wait(waittime);
    en=0;
    
    for(int i=0;i<rxlen;i++){
        rxdata[i]=master.getc();
        }
    if(rxdata[0]==id && rxdata[1]==efform_sc){
    printf("motor is not slave mode now\r\n");
    }else {
        printf("%d,%d\r\n",rxdata[0],rxdata[1]);
        }
    }
    
void krs::set_slavemode(int id){
    txlen=4;
    rxlen=2;
    
    unsigned char txdata[txlen];
    unsigned char rxdata[rxlen];
    
    en=1;
    txdata[0]=efform_cmd+id;
    txdata[1]=efform_sc;
    txdata[2]=slave_on_up;
    txdata[3]=slave_on_down;
    
    for(int i=0;i<txlen;i++){
        master.putc(txdata[i]);
        }
    
    wait(waittime);
    en=0;
    
    for(int i=0;i<rxlen;i++){
        rxdata[i]=master.getc();
        }
    if(rxdata[0]==id && rxdata[1]==efform_sc){
    printf("motor is slave mode now\r\n");
    }else {
        printf("%d,%d\r\n",rxdata[0],rxdata[1]);
        }
    }