岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

KondoServoLibrary_old/KondoServo.cpp@2:a92568bdeb5c, 2019-02-08 (annotated)

Committer:
shimizuta
Date:
Fri Feb 08 12:14:19 2019 +0000
Revision:
2:a92568bdeb5c
made class Leg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 2:a92568bdeb5c 1 #include "KondoServo.h"
shimizuta 2:a92568bdeb5c 2
shimizuta 2:a92568bdeb5c 3 /*KondoServo::KondoServo():master(tx,rx)
shimizuta 2:a92568bdeb5c 4 {
shimizuta 2:a92568bdeb5c 5 }*/
shimizuta 2:a92568bdeb5c 6 KondoServo::~KondoServo()
shimizuta 2:a92568bdeb5c 7 {
shimizuta 2:a92568bdeb5c 8 }
shimizuta 2:a92568bdeb5c 9
shimizuta 2:a92568bdeb5c 10
shimizuta 2:a92568bdeb5c 11 KondoServo::KondoServo(PinName txPin,PinName rxPin, int quantity, unsigned int baud):master(txPin,rxPin)
shimizuta 2:a92568bdeb5c 12 {
shimizuta 2:a92568bdeb5c 13 baudrate = baud;
shimizuta 2:a92568bdeb5c 14 master.baud(baudrate);
shimizuta 2:a92568bdeb5c 15 master.format(8,Serial::Even,1);
shimizuta 2:a92568bdeb5c 16 mode = (quantity==1) ? single : multi;
shimizuta 2:a92568bdeb5c 17 }
shimizuta 2:a92568bdeb5c 18
shimizuta 2:a92568bdeb5c 19 void KondoServo::init(int baud)
shimizuta 2:a92568bdeb5c 20 {
shimizuta 2:a92568bdeb5c 21 baudrate = baud;
shimizuta 2:a92568bdeb5c 22 master.baud(baudrate);
shimizuta 2:a92568bdeb5c 23 master.format(8,Serial::Even,1);
shimizuta 2:a92568bdeb5c 24 }
shimizuta 2:a92568bdeb5c 25 void KondoServo::init()
shimizuta 2:a92568bdeb5c 26 {
shimizuta 2:a92568bdeb5c 27 master.baud(baudrate);
shimizuta 2:a92568bdeb5c 28 master.format(8,Serial::Even,1);
shimizuta 2:a92568bdeb5c 29 }
shimizuta 2:a92568bdeb5c 30 int KondoServo::set_degree(int id,float degree)
shimizuta 2:a92568bdeb5c 31 {
shimizuta 2:a92568bdeb5c 32 u8 pos_h,pos_l;
shimizuta 2:a92568bdeb5c 33 long pos = 8000 * degree / 270 + 3500; // (11500-3500)÷270 = 29.62 ≒30
shimizuta 2:a92568bdeb5c 34 pos_h = pos / 128; //上位7bit
shimizuta 2:a92568bdeb5c 35 pos_l = pos % 128; //下位7bit
shimizuta 2:a92568bdeb5c 36 master.putc(0x80+id);// ID
shimizuta 2:a92568bdeb5c 37 master.putc(pos_h);
shimizuta 2:a92568bdeb5c 38 master.putc(pos_l);
shimizuta 2:a92568bdeb5c 39 /*
shimizuta 2:a92568bdeb5c 40 int ret1 = master.getc() - 128;
shimizuta 2:a92568bdeb5c 41 int ret2 = master.getc();
shimizuta 2:a92568bdeb5c 42 int ret3 = master.getc();
shimizuta 2:a92568bdeb5c 43
shimizuta 2:a92568bdeb5c 44 return ((ret2*128+ret3)-3500)*270/8000;
shimizuta 2:a92568bdeb5c 45 */
shimizuta 2:a92568bdeb5c 46 return 0;
shimizuta 2:a92568bdeb5c 47 }
shimizuta 2:a92568bdeb5c 48 void KondoServo::setID(u8 id)
shimizuta 2:a92568bdeb5c 49 {
shimizuta 2:a92568bdeb5c 50 if(mode == single) {
shimizuta 2:a92568bdeb5c 51 master.putc(0xE0+id);
shimizuta 2:a92568bdeb5c 52 master.putc(0x01);
shimizuta 2:a92568bdeb5c 53 master.putc(0x01);
shimizuta 2:a92568bdeb5c 54 master.putc(0x01);
shimizuta 2:a92568bdeb5c 55 }
shimizuta 2:a92568bdeb5c 56 }
shimizuta 2:a92568bdeb5c 57 u8 KondoServo::readID()
shimizuta 2:a92568bdeb5c 58 {
shimizuta 2:a92568bdeb5c 59 if(mode == single) {
shimizuta 2:a92568bdeb5c 60 master.putc(0xFF);
shimizuta 2:a92568bdeb5c 61 master.putc(0x00);
shimizuta 2:a92568bdeb5c 62 master.putc(0x00);
shimizuta 2:a92568bdeb5c 63 master.putc(0x00);
shimizuta 2:a92568bdeb5c 64 u8 readID = master.getc() - 224;
shimizuta 2:a92568bdeb5c 65 return readID;
shimizuta 2:a92568bdeb5c 66 } else {
shimizuta 2:a92568bdeb5c 67 return 0xff;
shimizuta 2:a92568bdeb5c 68 }
shimizuta 2:a92568bdeb5c 69 }
shimizuta 2:a92568bdeb5c 70
shimizuta 2:a92568bdeb5c 71 void KondoServo::setSpeed(int id, u8 speed)
shimizuta 2:a92568bdeb5c 72 {
shimizuta 2:a92568bdeb5c 73 master.putc(0xC0+id);
shimizuta 2:a92568bdeb5c 74 master.putc(0x02);
shimizuta 2:a92568bdeb5c 75 master.putc(speed);
shimizuta 2:a92568bdeb5c 76 }
shimizuta 2:a92568bdeb5c 77
shimizuta 2:a92568bdeb5c 78 float KondoServo::readSpeed(int id)
shimizuta 2:a92568bdeb5c 79 {
shimizuta 2:a92568bdeb5c 80 master.putc(0xA0);
shimizuta 2:a92568bdeb5c 81 master.putc(0x02);
shimizuta 2:a92568bdeb5c 82
shimizuta 2:a92568bdeb5c 83 /*int ret1 = master.getc();
shimizuta 2:a92568bdeb5c 84 int ret2 = master.getc();
shimizuta 2:a92568bdeb5c 85 int ret3 = master.getc();
shimizuta 2:a92568bdeb5c 86 int ret4 = master.getc();
shimizuta 2:a92568bdeb5c 87 int ret5 = master.getc();
shimizuta 2:a92568bdeb5c 88 int ret6 = master.getc();
shimizuta 2:a92568bdeb5c 89
shimizuta 2:a92568bdeb5c 90 printf("%d\n%d\n%d\n%d\n%d\n%d\n",ret1,ret2,ret3,ret4,ret5,ret6);
shimizuta 2:a92568bdeb5c 91 */
shimizuta 2:a92568bdeb5c 92
shimizuta 2:a92568bdeb5c 93 wait_ms(10);
shimizuta 2:a92568bdeb5c 94
shimizuta 2:a92568bdeb5c 95 for(int i=0; i<10; i++) {
shimizuta 2:a92568bdeb5c 96 //wait_ms(10);
shimizuta 2:a92568bdeb5c 97 int ret = master.getc();
shimizuta 2:a92568bdeb5c 98 printf("%d\n",ret);
shimizuta 2:a92568bdeb5c 99 }
shimizuta 2:a92568bdeb5c 100
shimizuta 2:a92568bdeb5c 101 /*int ret=0;
shimizuta 2:a92568bdeb5c 102 while(ret != 0x20+id){
shimizuta 2:a92568bdeb5c 103 ret = master.getc();
shimizuta 2:a92568bdeb5c 104 printf("%d\n",ret);
shimizuta 2:a92568bdeb5c 105 }*/
shimizuta 2:a92568bdeb5c 106 //return (float)(((ret5*128+ret6)-3500)*270/8000);
shimizuta 2:a92568bdeb5c 107 return 0;
shimizuta 2:a92568bdeb5c 108 }