Diff: KondoServoLibrary/KondoServo.cpp

Revision:

0:83b2c6a67cce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KondoServoLibrary/KondoServo.cpp	Thu Jul 08 03:46:29 2021 +0000
@@ -0,0 +1,106 @@
+#include "KondoServo.h"
+
+/*KondoServo::KondoServo():master(tx,rx)
+{
+}*/
+KondoServo::~KondoServo()
+{
+}
+
+
+KondoServo::KondoServo(PinName txPin,PinName rxPin, int quantity, unsigned int baud):master(txPin,rxPin)
+{
+    baudrate = baud;
+    master.baud(baudrate);
+    master.format(8,Serial::Even,1);
+    mode = (quantity==1) ? single : multi;
+}
+
+void KondoServo::init(int baud)
+{
+    baudrate = baud;
+    master.baud(baudrate);
+    master.format(8,Serial::Even,1);
+}
+void KondoServo::init()
+{
+    master.baud(baudrate);
+    master.format(8,Serial::Even,1);
+}
+int KondoServo::set_degree(int id,float degree)
+{
+    u8 pos_h,pos_l;
+    long pos = 8000 * degree / 270 + 3500; // (11500-3500)÷270 = 29.62 ≒30
+    pos_h = pos / 128;              //上位7bit
+    pos_l = pos % 128;              //下位7bit
+    master.putc(0x80+id);// ID
+    master.putc(pos_h);
+    master.putc(pos_l);
+
+    int ret1 = master.getc() - 128;
+    int ret2 = master.getc();
+    int ret3 = master.getc();
+
+    return ((ret2*128+ret3)-3500)*270/8000;
+}
+void KondoServo::setID(u8 id)
+{
+    if(mode == single) {
+        master.putc(0xE0+id);
+        master.putc(0x01);
+        master.putc(0x01);
+        master.putc(0x01);
+    }
+}
+u8 KondoServo::readID()
+{
+    if(mode == single) {
+        master.putc(0xFF);
+        master.putc(0x00);
+        master.putc(0x00);
+        master.putc(0x00);
+        u8 readID = master.getc() - 224;
+        return readID;
+    } else {
+        return 0xff;
+    }
+}
+
+void KondoServo::setSpeed(int id, u8 speed)
+{
+    master.putc(0xC0+id);
+    master.putc(0x02);
+    master.putc(speed);
+}
+
+float KondoServo::readSpeed(int id)
+{
+    master.putc(0xA0);
+    master.putc(0x02);
+
+    /*int ret1 = master.getc();
+    int ret2 = master.getc();
+    int ret3 = master.getc();
+    int ret4 = master.getc();
+    int ret5 = master.getc();
+    int ret6 = master.getc();
+
+    printf("%d\n%d\n%d\n%d\n%d\n%d\n",ret1,ret2,ret3,ret4,ret5,ret6);
+    */
+
+    wait_ms(10);
+
+    for(int i=0; i<10; i++) {
+        //wait_ms(10);
+        int ret = master.getc();
+        printf("%d\n",ret);
+    }
+
+    /*int ret=0;
+    while(ret != 0x20+id){
+        ret = master.getc();
+        printf("%d\n",ret);
+    }*/
+    //return (float)(((ret5*128+ret6)-3500)*270/8000);
+    return 0;
+}
\ No newline at end of file