KRS.cpp

00001 #include "KRS.h"
00002 #include "mbed.h"
00003 
00004 KRS::KRS(PinName tx,PinName rx) : _serial(tx,rx){
00005     _serial.baud(115200);
00006     _serial.format(8,Serial::Even,1);
00007 }
00008 
00009 void KRS::WriteAddress(uint8_t address){
00010     uint8_t ins = 0b11100000;
00011     if(address <= 0x1F){
00012         ins |= address;
00013     }else{
00014         ins |= 0x1F;
00015     }
00016     
00017     _serial.putc(ins);
00018     _serial.putc(0x01);
00019     _serial.putc(0x01);
00020     _serial.putc(0x01);
00021     
00022     wait_ms(10);
00023 }
00024 
00025 void KRS::SetStretch(uint8_t address, uint8_t value){
00026     uint8_t ins = 0b11000000;
00027     if(address <= 0x1F){
00028         ins |= address;
00029     }else{
00030         ins |= 0x1F;
00031     }
00032     
00033     if(value > 0x80){
00034         value = 0x80;
00035     }
00036     
00037     _serial.putc(ins);
00038     _serial.putc(0x01);
00039     _serial.putc(value);
00040     
00041     wait_ms(10);
00042 }
00043 
00044 void KRS::SetSpeed(uint8_t address, uint8_t value){
00045     uint8_t ins = 0b11000000;
00046     if(address <= 0x1F){
00047         ins |= address;
00048     }else{
00049         ins |= 0x1F;
00050     }
00051     
00052     if(value > 0x80){
00053         value = 0x80;
00054     }
00055     
00056     _serial.putc(ins);
00057     _serial.putc(0x02);
00058     _serial.putc(value);
00059     
00060     wait_ms(10);
00061 }
00062 
00063 void KRS::SetCurrent(uint8_t address, ROTATE rotate, uint8_t value){
00064     uint8_t ins = 0b11000000;
00065     if(address <= 0x1F){
00066         ins |= address;
00067     }else{
00068         ins |= 0x1F;
00069     }
00070     
00071     if(value > 0x40){
00072         value = 0x40;
00073     }
00074     
00075     switch(rotate){
00076         case F:
00077             _serial.putc(ins);
00078             _serial.putc(0x03);
00079             _serial.putc(value);
00080             break;
00081         case R:
00082             _serial.putc(ins);
00083             _serial.putc(0x03);
00084             _serial.putc(value+64);
00085             break;
00086         case COM:
00087             _serial.putc(ins);
00088             _serial.putc(0x03);
00089             _serial.putc(value);
00090             wait_ms(10);
00091             _serial.putc(ins);
00092             _serial.putc(0x03);
00093             _serial.putc(value+64);
00094             break;
00095     }
00096     
00097     wait_ms(10);
00098 }
00099 
00100 void KRS::SetPosition(uint8_t address, uint16_t value){
00101     uint8_t ins = 0b10000000;
00102     if(address <= 0x1F){
00103         ins |= address;
00104     }else{
00105         ins |= 0x1F;
00106     }
00107     
00108     if(value == 0){
00109         value = 0;
00110     }else if(value < 3500){
00111         value = 3500;
00112     }else if(value > 11500){
00113         value = 11500;
00114     }
00115     uint8_t byteH = value >> 7;
00116     uint8_t byteL = value;
00117     byteL &= (0 << 8);
00118     
00119     _serial.putc(ins);
00120     _serial.putc(byteH);
00121     _serial.putc(byteL);
00122     
00123     wait_ms(10);
00124 }