Diff: main.cpp

Revision:

0:19618cb94138

Child:

1:859610213d81

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 25 05:41:38 2017 +0000
@@ -0,0 +1,143 @@
+// Masahiro Furukawa - m.furukawa@ist.osaka-u.ac.jp
+// Apr 25, 2017
+//
+#define TITLE  "KONDO Kagaku ICS Servo Controller rev0.1"
+//
+//
+// reference :
+// Serial Control
+// http://d.hatena.ne.jp/rinie/20110408/1302265191
+// https://developer.mbed.org/users/okini3939/notebook/Serial_jp/
+//
+// half dublex communication
+// http://kondo-robot.com/faq/serial-servo-method-tech
+//
+// Serial FIFO (16bytes)
+// http://qiita.com/matsujirushi/items/b6801887f751a8f5e6cd
+//
+// Servo ICS Command Reference
+// http://kondo-robot.com/faq/serial-servo-method-tech-2
+//
+// Logic Level Converter 
+// https://www.switch-science.com/catalog/1193/
+
+#define PI 3.14159
+
+#include "mbed.h"
+#include "math.h"
+#include "sequence.h"
+
+Serial pc(USBTX,USBRX);   // tx, rx
+Serial master(p9,p10); // tx, rx
+Serial master2(p13,p14); // tx, rx
+Serial master3(p28,p27); // tx, rx
+
+int ICS_set_pos(const char id, const int pos)
+{
+    // out of range error
+    // http://kondo-robot.com/faq/serial-servo-method-tech-2
+    if (pos > 11500 || pos < 3500) return -1; 
+    
+    b[0] = 0x80 | id;           // Set Pos Command "0b100 = 0x04"
+    b[1] = (pos & 0x3F80)>>7;   // Position (High 7 Bytes)
+    b[2] =  pos & 0x7f;         // Position (Low 7 Bytes)
+        
+    master.putc(b[0]);
+    master.putc(b[1]);
+    master.putc(b[2]);
+    wait_us(800);
+    
+    //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]);
+    
+    return 0;
+}
+
+int ICS_set_pos2(const char id, const int pos)
+{
+    // out of range error
+    // http://kondo-robot.com/faq/serial-servo-method-tech-2
+    if (pos > 11500 || pos < 3500) return -1; 
+    
+    b[0] = 0x80 | id;           // Set Pos Command "0b100 = 0x04"
+    b[1] = (pos & 0x3F80)>>7;   // Position (High 7 Bytes)
+    b[2] =  pos & 0x7f;         // Position (Low 7 Bytes)
+        
+    master2.putc(b[0]);
+    master2.putc(b[1]);
+    master2.putc(b[2]);
+    wait_us(800);
+    
+    //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]);
+    
+    return 0;
+}
+
+int ICS_set_pos3(const char id, const int pos)
+{
+    // out of range error
+    // http://kondo-robot.com/faq/serial-servo-method-tech-2
+    if (pos > 11500 || pos < 3500) return -1; 
+    
+    b[0] = 0x80 | id;           // Set Pos Command "0b100 = 0x04"
+    b[1] = (pos & 0x3F80)>>7;   // Position (High 7 Bytes)
+    b[2] =  pos & 0x7f;         // Position (Low 7 Bytes)
+        
+    master3.putc(b[0]);
+    master3.putc(b[1]);
+    master3.putc(b[2]);
+    wait_us(800);
+    
+    //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]);
+    
+    return 0;
+}
+
+int main()
+{
+    pc.baud(115200);
+    pc.printf("\n\r%s\n\r", TITLE);
+    
+    master.baud(115200);
+    master2.baud(115200);
+    master3.baud(115200);
+    master.format(8, Serial::Even, 1);      // data length = 8bit, parity = even, stop bit = 1bit
+        
+    double t=0;
+    int pos=0;
+    
+    while(1){
+        t=t+1;
+        
+        pos = (int)(4000.0 * sin(    t/180.0*PI) + 7500.0); ICS_set_pos(0,pos);    
+        pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos(2,pos);
+        pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos(6,pos);    
+        pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos(7,pos);
+        pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos(8,pos);    
+        pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos(9,pos);
+        pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos(10,pos);      
+        
+        pos = (int)(4000.0 * sin(    t/180.0*PI) + 7500.0); ICS_set_pos2(0,pos);    
+        pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos2(2,pos);
+        pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos2(6,pos);    
+        pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos2(7,pos);
+        pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos2(8,pos);    
+        pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos2(9,pos);
+        pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos2(10,pos);      
+        
+        pos = (int)(4000.0 * sin(    t/180.0*PI) + 7500.0); ICS_set_pos3(0,pos);    
+        pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos3(2,pos);
+        pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos3(6,pos);    
+        pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos3(7,pos);
+        pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos3(8,pos);    
+        pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos3(9,pos);
+        pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos3(10,pos);            
+    
+        //wait_ms(5);
+    }
+}
+
+
+
+// SerialHalfDuplex (revision 43 @mbed library)
+// http://d.hatena.ne.jp/rinie/20121125/1353770901
+//
\ No newline at end of file