delfia liew / KONDO_ICS_Servo_control_1

454

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Revision 0:d2be68a55537, committed 2018-11-16

Comitter:
delfia
Date:
Fri Nov 16 04:59:24 2018 +0000
Commit message:
ll

Changed in this revision

859610213d81/.hg_archival.txt Show annotated file Show diff for this revision Revisions of this file
859610213d81/main.cpp Show annotated file Show diff for this revision Revisions of this file
859610213d81/mbed.bld Show annotated file Show diff for this revision Revisions of this file
859610213d81/sequence.h Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/859610213d81/.hg_archival.txt	Fri Nov 16 04:59:24 2018 +0000
@@ -0,0 +1,5 @@
+repo: 19618cb9413818a9e0eb4ca27bcb90f07ee7d65a
+node: 859610213d81e5d0faff025521e73ac958b13c35
+branch: default
+latesttag: null
+latesttagdistance: 2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/859610213d81/main.cpp	Fri Nov 16 04:59:24 2018 +0000
@@ -0,0 +1,145 @@
+// 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 master1(p9,p10); // tx, rx
+Serial master2(p13,p14); // tx, rx
+Serial master3(p28,p27); // tx, rx
+
+int ICS_set_pos1(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)
+        
+    master1.putc(b[0]);
+    master1.putc(b[1]);
+    master1.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(1800);
+    
+    //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);
+    
+    master1.baud(115200);
+    master2.baud(115200);
+    master3.baud(115200);
+    master1.format(8, Serial::Even, 1);      // data length = 8bit, parity = even, stop bit = 1bit
+    master2.format(8, Serial::Even, 1);      // data length = 8bit, parity = even, stop bit = 1bit
+    master3.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_pos1(0,pos);    
+        pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos1(2,pos);
+        pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos1(6,pos);    
+        pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos1(7,pos);
+        pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos1(8,pos);    
+        pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos1(9,pos);
+        pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos1(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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/859610213d81/mbed.bld	Fri Nov 16 04:59:24 2018 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/e2ed12d17f06
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/859610213d81/sequence.h	Fri Nov 16 04:59:24 2018 +0000
@@ -0,0 +1,13 @@
+// Masahiro Furukawa - m.furukawa@ist.osaka-u.ac.jp
+// Apr 25, 2017
+//
+// KONDO Kagaku ICS Servo Controller
+//
+//
+// reference :
+// Serial Control
+// https://developer.mbed.org/users/okini3939/notebook/Serial_jp/
+
+
+char b[256];
+int size = 9;
\ No newline at end of file