delfia liew / KONDO_ICS_Servo_control_1

454

Dependencies:   mbed

859610213d81/main.cpp@0:d2be68a55537, 2018-11-16 (annotated)

Committer:
delfia
Date:
Fri Nov 16 04:59:24 2018 +0000
Revision:
0:d2be68a55537
ll

Who changed what in which revision?

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