koki konishi / KRS

KRS ICSサーボモータ用ライブラリ

初期化 KRS name(tx,rx);

位置指定 void SetPosition(id, position); [position : 0 3500 ~ 11500]

速度設定 void SetSpeed(id, speed); [speed : 0 ~ 127]

ストレッチ設定 void SetStretch(id, stretch); [stretch : 0 ~ 127]

電流制限設定 void SetCurrent(id, rotate, value : 0 ~ 127); [rotate : KRS::F KRS::R KRS::COM, current : 0 ~ 64]

ID書き込み void WriteAddress(id);

id : 0x00 ~ 0x1F

Diff: KRS.cpp

Revision:
1:50d0eb774b0d
Parent:
0:7bf595a76895
Child:
2:431a802d01e2
--- a/KRS.cpp	Sat Jul 02 03:12:15 2022 +0000
+++ b/KRS.cpp	Sun Jul 03 04:35:46 2022 +0000
@@ -6,8 +6,83 @@
     _serial.format(8,Serial::Even,1);
 }
 
+void KRS::SetStretch(uint8_t address, uint8_t value){
+    uint8_t ins = 0b11000000;
+    if(address <= 0x1F){
+        ins |= address;
+    }else{
+        ins |= 0x1F;
+    }
+    
+    if(value > 0x80){
+        value = 0x80;
+    }
+    
+    _serial.putc(ins);
+    _serial.putc(0x01);
+    _serial.putc(value);
+    
+    wait_ms(10);
+}
+
+void KRS::SetSpeed(uint8_t address, uint8_t value){
+    uint8_t ins = 0b11000000;
+    if(address <= 0x1F){
+        ins |= address;
+    }else{
+        ins |= 0x1F;
+    }
+    
+    if(value > 0x80){
+        value = 0x80;
+    }
+    
+    _serial.putc(ins);
+    _serial.putc(0x02);
+    _serial.putc(value);
+    
+    wait_ms(10);
+}
+
+void KRS::SetCurrent(uint8_t address, ROTATE rotate, uint8_t value){
+    uint8_t ins = 0b11000000;
+    if(address <= 0x1F){
+        ins |= address;
+    }else{
+        ins |= 0x1F;
+    }
+    
+    if(value > 0x40){
+        value = 0x40;
+    }
+    
+    switch(rotate){
+        case F:
+            _serial.putc(ins);
+            _serial.putc(0x03);
+            _serial.putc(value);
+            break;
+        case R:
+            _serial.putc(ins);
+            _serial.putc(0x03);
+            _serial.putc(value+64);
+            break;
+        case COM:
+            _serial.putc(ins);
+            _serial.putc(0x03);
+            _serial.putc(value);
+            wait_ms(10);
+            _serial.putc(ins);
+            _serial.putc(0x03);
+            _serial.putc(value+64);
+            break;
+    }
+    
+    wait_ms(10);
+}
+
 void KRS::SetPosition(uint8_t address, uint16_t value){
-    ins = 0b10000000;
+    uint8_t ins = 0b10000000;
     if(address <= 0x1F){
         ins |= address;
     }else{
@@ -21,11 +96,13 @@
     }else if(value > 11500){
         value = 11500;
     }
-    byteH = value >> 7;
-    byteL = value;
+    uint8_t byteH = value >> 7;
+    uint8_t byteL = value;
     byteL &= (0 << 8);
     
     _serial.putc(ins);
     _serial.putc(byteH);
     _serial.putc(byteL);
+    
+    wait_ms(10);
 }
\ No newline at end of file