Masahiro Furukawa / Mbed 2 deprecated KONDO_ICS_Servo_control

// 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/

Dependencies:   mbed

Masahiro Furukawa - m.furukawa@ist.osaka-u.ac.jp Apr 25, 2017

reference :

half dublex communication http://kondo-robot.com/faq/serial-servo-method-tech

main.cpp@1:859610213d81, 2017-04-25 (annotated)

Committer:
mfurukawa
Date:
Tue Apr 25 05:58:33 2017 +0000
Revision:
1:859610213d81
Parent:
0:19618cb94138 
stable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfurukawa 0:19618cb94138 1 // Masahiro Furukawa - m.furukawa@ist.osaka-u.ac.jp
mfurukawa 0:19618cb94138 2 // Apr 25, 2017
mfurukawa 0:19618cb94138 3 //
mfurukawa 0:19618cb94138 4 #define TITLE "KONDO Kagaku ICS Servo Controller rev0.1"
mfurukawa 0:19618cb94138 5 //
mfurukawa 0:19618cb94138 6 //
mfurukawa 0:19618cb94138 7 // reference :
mfurukawa 0:19618cb94138 8 // Serial Control
mfurukawa 0:19618cb94138 9 // http://d.hatena.ne.jp/rinie/20110408/1302265191
mfurukawa 0:19618cb94138 10 // https://developer.mbed.org/users/okini3939/notebook/Serial_jp/
mfurukawa 0:19618cb94138 11 //
mfurukawa 0:19618cb94138 12 // half dublex communication
mfurukawa 0:19618cb94138 13 // http://kondo-robot.com/faq/serial-servo-method-tech
mfurukawa 0:19618cb94138 14 //
mfurukawa 0:19618cb94138 15 // Serial FIFO (16bytes)
mfurukawa 0:19618cb94138 16 // http://qiita.com/matsujirushi/items/b6801887f751a8f5e6cd
mfurukawa 0:19618cb94138 17 //
mfurukawa 0:19618cb94138 18 // Servo ICS Command Reference
mfurukawa 0:19618cb94138 19 // http://kondo-robot.com/faq/serial-servo-method-tech-2
mfurukawa 0:19618cb94138 20 //
mfurukawa 0:19618cb94138 21 // Logic Level Converter
mfurukawa 0:19618cb94138 22 // https://www.switch-science.com/catalog/1193/
mfurukawa 0:19618cb94138 23
mfurukawa 0:19618cb94138 24 #define PI 3.14159
mfurukawa 0:19618cb94138 25
mfurukawa 0:19618cb94138 26 #include "mbed.h"
mfurukawa 0:19618cb94138 27 #include "math.h"
mfurukawa 0:19618cb94138 28 #include "sequence.h"
mfurukawa 0:19618cb94138 29
mfurukawa 0:19618cb94138 30 Serial pc(USBTX,USBRX); // tx, rx
mfurukawa 1:859610213d81 31 Serial master1(p9,p10); // tx, rx
mfurukawa 0:19618cb94138 32 Serial master2(p13,p14); // tx, rx
mfurukawa 0:19618cb94138 33 Serial master3(p28,p27); // tx, rx
mfurukawa 0:19618cb94138 34
mfurukawa 1:859610213d81 35 int ICS_set_pos1(const char id, const int pos)
mfurukawa 0:19618cb94138 36 {
mfurukawa 0:19618cb94138 37 // out of range error
mfurukawa 0:19618cb94138 38 // http://kondo-robot.com/faq/serial-servo-method-tech-2
mfurukawa 0:19618cb94138 39 if (pos > 11500 || pos < 3500) return -1;
mfurukawa 0:19618cb94138 40
mfurukawa 0:19618cb94138 41 b[0] = 0x80 | id; // Set Pos Command "0b100 = 0x04"
mfurukawa 0:19618cb94138 42 b[1] = (pos & 0x3F80)>>7; // Position (High 7 Bytes)
mfurukawa 0:19618cb94138 43 b[2] = pos & 0x7f; // Position (Low 7 Bytes)
mfurukawa 0:19618cb94138 44
mfurukawa 1:859610213d81 45 master1.putc(b[0]);
mfurukawa 1:859610213d81 46 master1.putc(b[1]);
mfurukawa 1:859610213d81 47 master1.putc(b[2]);
mfurukawa 0:19618cb94138 48 wait_us(800);
mfurukawa 0:19618cb94138 49
mfurukawa 0:19618cb94138 50 //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]);
mfurukawa 0:19618cb94138 51
mfurukawa 0:19618cb94138 52 return 0;
mfurukawa 0:19618cb94138 53 }
mfurukawa 0:19618cb94138 54
mfurukawa 0:19618cb94138 55 int ICS_set_pos2(const char id, const int pos)
mfurukawa 0:19618cb94138 56 {
mfurukawa 0:19618cb94138 57 // out of range error
mfurukawa 0:19618cb94138 58 // http://kondo-robot.com/faq/serial-servo-method-tech-2
mfurukawa 0:19618cb94138 59 if (pos > 11500 || pos < 3500) return -1;
mfurukawa 0:19618cb94138 60
mfurukawa 0:19618cb94138 61 b[0] = 0x80 | id; // Set Pos Command "0b100 = 0x04"
mfurukawa 0:19618cb94138 62 b[1] = (pos & 0x3F80)>>7; // Position (High 7 Bytes)
mfurukawa 0:19618cb94138 63 b[2] = pos & 0x7f; // Position (Low 7 Bytes)
mfurukawa 0:19618cb94138 64
mfurukawa 0:19618cb94138 65 master2.putc(b[0]);
mfurukawa 0:19618cb94138 66 master2.putc(b[1]);
mfurukawa 0:19618cb94138 67 master2.putc(b[2]);
mfurukawa 0:19618cb94138 68 wait_us(800);
mfurukawa 0:19618cb94138 69
mfurukawa 0:19618cb94138 70 //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]);
mfurukawa 0:19618cb94138 71
mfurukawa 0:19618cb94138 72 return 0;
mfurukawa 0:19618cb94138 73 }
mfurukawa 0:19618cb94138 74
mfurukawa 0:19618cb94138 75 int ICS_set_pos3(const char id, const int pos)
mfurukawa 0:19618cb94138 76 {
mfurukawa 0:19618cb94138 77 // out of range error
mfurukawa 0:19618cb94138 78 // http://kondo-robot.com/faq/serial-servo-method-tech-2
mfurukawa 0:19618cb94138 79 if (pos > 11500 || pos < 3500) return -1;
mfurukawa 0:19618cb94138 80
mfurukawa 0:19618cb94138 81 b[0] = 0x80 | id; // Set Pos Command "0b100 = 0x04"
mfurukawa 0:19618cb94138 82 b[1] = (pos & 0x3F80)>>7; // Position (High 7 Bytes)
mfurukawa 0:19618cb94138 83 b[2] = pos & 0x7f; // Position (Low 7 Bytes)
mfurukawa 0:19618cb94138 84
mfurukawa 0:19618cb94138 85 master3.putc(b[0]);
mfurukawa 0:19618cb94138 86 master3.putc(b[1]);
mfurukawa 0:19618cb94138 87 master3.putc(b[2]);
mfurukawa 1:859610213d81 88 wait_us(1800);
mfurukawa 0:19618cb94138 89
mfurukawa 0:19618cb94138 90 //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]);
mfurukawa 0:19618cb94138 91
mfurukawa 0:19618cb94138 92 return 0;
mfurukawa 0:19618cb94138 93 }
mfurukawa 0:19618cb94138 94
mfurukawa 0:19618cb94138 95 int main()
mfurukawa 0:19618cb94138 96 {
mfurukawa 0:19618cb94138 97 pc.baud(115200);
mfurukawa 0:19618cb94138 98 pc.printf("\n\r%s\n\r", TITLE);
mfurukawa 0:19618cb94138 99
mfurukawa 1:859610213d81 100 master1.baud(115200);
mfurukawa 0:19618cb94138 101 master2.baud(115200);
mfurukawa 0:19618cb94138 102 master3.baud(115200);
mfurukawa 1:859610213d81 103 master1.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit
mfurukawa 1:859610213d81 104 master2.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit
mfurukawa 1:859610213d81 105 master3.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit
mfurukawa 0:19618cb94138 106
mfurukawa 0:19618cb94138 107 double t=0;
mfurukawa 0:19618cb94138 108 int pos=0;
mfurukawa 0:19618cb94138 109
mfurukawa 0:19618cb94138 110 while(1){
mfurukawa 0:19618cb94138 111 t=t+1;
mfurukawa 0:19618cb94138 112
mfurukawa 1:859610213d81 113 pos = (int)(4000.0 * sin( t/180.0*PI) + 7500.0); ICS_set_pos1(0,pos);
mfurukawa 1:859610213d81 114 pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos1(2,pos);
mfurukawa 1:859610213d81 115 pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos1(6,pos);
mfurukawa 1:859610213d81 116 pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos1(7,pos);
mfurukawa 1:859610213d81 117 pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos1(8,pos);
mfurukawa 1:859610213d81 118 pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos1(9,pos);
mfurukawa 1:859610213d81 119 pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos1(10,pos);
mfurukawa 0:19618cb94138 120
mfurukawa 0:19618cb94138 121 pos = (int)(4000.0 * sin( t/180.0*PI) + 7500.0); ICS_set_pos2(0,pos);
mfurukawa 0:19618cb94138 122 pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos2(2,pos);
mfurukawa 0:19618cb94138 123 pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos2(6,pos);
mfurukawa 0:19618cb94138 124 pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos2(7,pos);
mfurukawa 0:19618cb94138 125 pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos2(8,pos);
mfurukawa 0:19618cb94138 126 pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos2(9,pos);
mfurukawa 0:19618cb94138 127 pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos2(10,pos);
mfurukawa 0:19618cb94138 128
mfurukawa 0:19618cb94138 129 pos = (int)(4000.0 * sin( t/180.0*PI) + 7500.0); ICS_set_pos3(0,pos);
mfurukawa 0:19618cb94138 130 pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos3(2,pos);
mfurukawa 0:19618cb94138 131 pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos3(6,pos);
mfurukawa 0:19618cb94138 132 pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos3(7,pos);
mfurukawa 0:19618cb94138 133 pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos3(8,pos);
mfurukawa 0:19618cb94138 134 pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos3(9,pos);
mfurukawa 0:19618cb94138 135 pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos3(10,pos);
mfurukawa 0:19618cb94138 136
mfurukawa 0:19618cb94138 137 //wait_ms(5);
mfurukawa 0:19618cb94138 138 }
mfurukawa 0:19618cb94138 139 }
mfurukawa 0:19618cb94138 140
mfurukawa 0:19618cb94138 141
mfurukawa 0:19618cb94138 142
mfurukawa 0:19618cb94138 143 // SerialHalfDuplex (revision 43 @mbed library)
mfurukawa 0:19618cb94138 144 // http://d.hatena.ne.jp/rinie/20121125/1353770901
mfurukawa 0:19618cb94138 145 //