Masahiro Furukawa
// 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 // // Servo ICS Command Reference // http://kondo-robot.com/faq/serial-servo-method-tech-2 // // Logic Level Converter // https://www.switch-science.com/catalog/1193/
Masahiro Furukawa - m.furukawa@ist.osaka-u.ac.jp Apr 25, 2017
reference :
- Serial Control
half dublex communication http://kondo-robot.com/faq/serial-servo-method-tech
- Servo ICS Command Reference
- ogic Level Converter
Revision 1:859610213d81, committed 2017-04-25
- Comitter:
- mfurukawa
- Date:
- Tue Apr 25 05:58:33 2017 +0000
- Parent:
- 0:19618cb94138
- Commit message:
- stable
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 19618cb94138 -r 859610213d81 main.cpp
--- a/main.cpp Tue Apr 25 05:41:38 2017 +0000
+++ b/main.cpp Tue Apr 25 05:58:33 2017 +0000
@@ -28,11 +28,11 @@
#include "sequence.h"
Serial pc(USBTX,USBRX); // tx, rx
-Serial master(p9,p10); // tx, rx
+Serial master1(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)
+int ICS_set_pos1(const char id, const int pos)
{
// out of range error
// http://kondo-robot.com/faq/serial-servo-method-tech-2
@@ -42,9 +42,9 @@
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]);
+ 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]);
@@ -85,7 +85,7 @@
master3.putc(b[0]);
master3.putc(b[1]);
master3.putc(b[2]);
- wait_us(800);
+ wait_us(1800);
//pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]);
@@ -97,10 +97,12 @@
pc.baud(115200);
pc.printf("\n\r%s\n\r", TITLE);
- master.baud(115200);
+ master1.baud(115200);
master2.baud(115200);
master3.baud(115200);
- master.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit
+ 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;
@@ -108,13 +110,13 @@
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_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);