kohei matsumoto / Mbed 2 deprecated NHK_CenterArm1

DRのPS3での操作用(アームの開閉をコントローラで制御可能)

Dependencies:   mbed CalPID motorout ros_lib_melodic MotorController_AbsEC

KondoServoLibrary/KondoServo.cpp@12:c2bb0a9587b5, 2021-07-30 (annotated)

Committer:
koheim
Date:
Fri Jul 30 11:36:46 2021 +0000
Revision:
12:c2bb0a9587b5
Parent:
0:83b2c6a67cce 
a

Who changed what in which revision?

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