yuto kawamura / Mbed 2 deprecated George_Slave_BOTHMOVE

right and left move at the same time

Dependencies:   mbed robot

KondoServoLibrary/KondoServo.cpp@13:678870d8f851, 2019-05-15 (annotated)

Committer:
yuto17320508
Date:
Wed May 15 10:19:13 2019 +0000
Revision:
13:678870d8f851
Parent:
0:411ab20ce87d 
l

Who changed what in which revision?

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