Revision 0:f18c41222633, committed 2021-08-23

Comitter:

o2132613

Date:

Mon Aug 23 07:41:57 2021 +0000

Commit message:

aaaa

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KondoServoLibrary/KondoServo.cpp	Mon Aug 23 07:41:57 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);
+}
+void 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KondoServoLibrary/KondoServo.h	Mon Aug 23 07:41:57 2021 +0000
@@ -0,0 +1,29 @@
+#ifndef __KONDO_SERVO_H__
+#define __KONDO_SERVO_H__
+
+#include "mbed.h"
+#include "SerialHalfDuplex.h"
+
+typedef enum ServoMode{single = 0,multi = 1}s_mode;
+typedef unsigned char u8;
+
+class KondoServo{
+    protected:
+        unsigned int baudrate;
+        s_mode mode; 
+        SerialHalfDuplex master;
+
+    public:        
+        void init(int baud);
+        void init();
+        
+        void setSpeed(int id, u8 speed);
+        float readSpeed(int id);
+        void set_degree(int id,float degree);
+        void setID(u8 id);
+        u8 readID();
+        ~KondoServo();
+        KondoServo(PinName txPin,PinName rxPin, int quantity=2, unsigned int baud=115200);
+
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KondoServoLibrary/SerialHalfDuplex_HM/SerialHalfDuplex.cpp	Mon Aug 23 07:41:57 2021 +0000
@@ -0,0 +1,31 @@
+/* mbed Microcontroller Library - SerialHalfDuplex
+ * Copyright (c) 2010-2011 ARM Limited. All rights reserved.
+ * written and changed by Hiroki Mineshita at 2016-04-12
+ * by https://github.com/mbedmicro/mbed/tree/master/libraries/tests/libs/SerialHalfDuplex
+ */
+#include "SerialHalfDuplex.h"
+
+#if DEVICE_SERIAL
+
+namespace mbed {
+
+SerialHalfDuplex::SerialHalfDuplex(PinName tx, PinName rx,const char *name)
+    : Serial(tx,rx,name) {
+}
+
+int SerialHalfDuplex::_putc(int c) {
+    int retc;
+    Serial::_putc(c);
+    if(Serial::readable())retc = Serial::getc();       // reading also clears any interrupt
+    else retc=-1000;
+    return retc;
+}
+
+int SerialHalfDuplex::_getc(void) {
+    if(Serial::readable()) return Serial::_getc();
+    else return -1000;
+}
+
+} // End namespace
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KondoServoLibrary/SerialHalfDuplex_HM/SerialHalfDuplex.h	Mon Aug 23 07:41:57 2021 +0000
@@ -0,0 +1,84 @@
+/* mbed Microcontroller Library - SerialHalfDuplex
+ * Copyright (c) 2010-2011 ARM Limited. All rights reserved.
+ */
+#ifndef MBED_SERIALHALFDUPLEX_H
+#define MBED_SERIALHALFDUPLEX_H
+
+#include "platform.h"
+
+#if DEVICE_SERIAL
+
+#include "Serial.h"
+
+namespace mbed {
+
+/** A serial port (UART) for communication with other devices using
+ * Half-Duplex, allowing transmit and receive on a single
+ * shared transmit and receive line. Only one end should be transmitting
+ * at a time.
+ *
+ * Both the tx and rx pin should be defined, and wired together.
+ * This is in addition to them being wired to the other serial
+ * device to allow both read and write functions to operate.
+ *
+ *  For Simplex and Full-Duplex Serial communication, see Serial()
+ *
+ *  Example:
+ * @code
+ * // Send a byte to a second HalfDuplex device, and read the response
+ *
+ * #include "mbed.h"
+ *
+ * // p9 and p10 should be wired together to form "a"
+ * // p28 and p27 should be wired together to form "b"
+ * // p9/p10 should be wired to p28/p27 as the Half Duplex connection
+ *
+ * SerialHalfDuplex a(p9, p10);
+ * SerialHalfDuplex b(p28, p27);
+ *
+ * void b_rx() { // second device response
+ *     b.putc(b.getc() + 4);
+ * }
+ *
+ * int main() {
+ *     b.attach(&b_rx);
+ *     for (int c = 'A'; c < 'Z'; c++) {
+ *         a.putc(c);
+ *         printf("sent [%c]\n", c);
+ *         wait(0.5);   // b should respond
+ *         if (a.readable()) {
+ *             printf("received [%c]\n", a.getc());
+ *         }
+ *     }
+ * }
+ * @endcode
+ */
+class SerialHalfDuplex : public Serial {
+
+public:
+
+    /** Create a half-duplex serial port, connected to the specified transmit
+     * and receive pins.
+     *
+     * These pins should be wired together, as well as to the target device
+     *
+     *  @param tx Transmit pin
+     *  @param rx Receive pin
+     *
+     *  @note
+     *    Either tx or rx may be specified as NC if unused
+     */
+    SerialHalfDuplex(PinName tx=p13, PinName rx=p14, const char *name=NULL);
+
+protected:
+
+    virtual int _putc(int c);
+    virtual int _getc(void);
+
+}; // End class SerialHalfDuplex
+
+} // End namespace
+
+#endif
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Aug 23 07:41:57 2021 +0000
@@ -0,0 +1,28 @@
+#include "mbed.h"
+#include "KondoServo.h"
+
+KondoServo servo(p13,p14,1,115200);
+
+int main()
+{
+    while(1){
+        servo.set_degree(0,39);
+        wait_ms(2);
+        servo.set_degree(1,39);
+        wait_ms(2);
+        servo.set_degree(2,39);
+        wait_ms(2);
+        servo.set_degree(3,39);
+        wait(1);
+        servo.set_degree(0,80);
+        wait_ms(2);
+        servo.set_degree(1,80);
+        wait_ms(2);
+        servo.set_degree(2,80);
+        wait_ms(2);
+        servo.set_degree(3,80);
+        wait_ms(2);        
+        wait(1);
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Aug 23 07:41:57 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file