Revision 4:31695331ce17, committed 2022-01-10

Comitter:

marcodesilva

Date:

Mon Jan 10 20:38:41 2022 +0000

Parent:

3:7912ab1400c4

Commit message:

compila al 10/01/2022, da testare sul tool

Changed in this revision

diff -r 7912ab1400c4 -r 31695331ce17 RoboClaw.lib
--- a/RoboClaw.lib	Tue Sep 14 12:09:47 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/anfontanelli/code/RoboClaw/#6a4969d25627

diff -r 7912ab1400c4 -r 31695331ce17 RoboClaw/RoboClaw.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw/RoboClaw.cpp	Mon Jan 10 20:38:41 2022 +0000
@@ -0,0 +1,291 @@
+#include "RoboClaw.h"
+#include <stdarg.h>
+
+#define MAXTRY 1
+#define SetDWORDval(arg) (uint8_t)(arg>>24),(uint8_t)(arg>>16),(uint8_t)(arg>>8),(uint8_t)arg
+#define SetWORDval(arg) (uint8_t)(arg>>8),(uint8_t)arg
+
+RoboClaw::RoboClaw(uint8_t adr, int baudrate, PinName rx, PinName tx, double _read_timeout) : _roboclaw(rx, tx){
+    _roboclaw.set_baud(baudrate);//_roboclaw.baud(baudrate);
+       
+    rx_in = 0;
+    address = adr;
+    //readTime = 0.0;
+    //readTimer.start();
+    read_timeout = _read_timeout;
+    
+}
+
+void RoboClaw::crc_clear(){
+    crc = 0;
+}
+
+
+void RoboClaw::flushSerialBuffer(void) { 
+    char char1[1];
+     
+    while (_roboclaw.readable()) { 
+        _roboclaw.read_timeout(char1,1,read_timeout); 
+    } 
+return; }
+
+void RoboClaw::crc_update (uint8_t data){
+    int i;
+    crc = crc ^ ((uint16_t)data << 8);
+    for (i=0; i<8; i++) {
+        if (crc & 0x8000)
+            crc = (crc << 1) ^ 0x1021;
+        else
+            crc <<= 1;
+    }
+}
+
+uint16_t RoboClaw::crc_get(){
+    return crc;
+}
+
+uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes){
+    uint16_t crc_;
+    for (int byte = 0; byte < nBytes; byte++) {
+        crc_ = crc_ ^ ((uint16_t)packet[byte] << 8);
+        for (uint8_t bit = 0; bit < 8; bit++) {
+            if (crc_ & 0x8000) {
+                crc_ = (crc_ << 1) ^ 0x1021;
+            } else {
+                crc_ = crc_ << 1;
+            }
+        }
+    }
+    return crc_;
+}
+
+void RoboClaw::write_n(uint8_t cnt, ... ){
+    uint8_t count = 0;
+    char dat[1];
+    char r_dat[1];
+    r_dat[0] = ' ';
+    char crc_[2];
+    do {
+        _roboclaw.flush();
+
+        crc_clear();
+        va_list marker;
+        va_start( marker, cnt );
+        for(uint8_t index=0; index<cnt; index++) {
+            uint8_t data = va_arg(marker, unsigned int);
+            dat[0] = data;
+            crc_update(data);
+            _roboclaw.write(dat,1);
+                   }
+        va_end( marker );
+        uint16_t crc = crc_get();
+        crc_[0]=crc>>8;
+        crc_[1]=crc;
+        _roboclaw.write(crc_,2);        
+        _roboclaw.read_timeout(r_dat,1,read_timeout);
+        
+         count ++;
+         
+        }while((uint16_t)r_dat[0] != 0xFF&& count <=MAXTRY);
+
+}
+
+void RoboClaw::write_(uint8_t command, uint8_t data, bool reading, bool crcon){
+   // _roboclaw.putc(address);
+   // _roboclaw.putc(command);
+
+    if(reading == false) {
+        if(crcon == true) {
+            uint8_t packet[2] = {address, command};
+            uint16_t checksum = crc16(packet,2);
+           // _roboclaw.putc(checksum>>8);
+           // _roboclaw.putc(checksum);
+        } else {
+            uint8_t packet[3] = {address, command, data};
+            uint16_t checksum = crc16(packet,3);
+           // _roboclaw.putc(data);
+           // _roboclaw.putc(checksum>>8);
+           // _roboclaw.putc(checksum);
+        }
+    }
+}
+
+bool RoboClaw::read_n(uint8_t address, uint8_t cmd, int n_byte, int32_t &value){
+    uint16_t read_byte[n_byte];
+    uint16_t received_crc;
+    int dataRead=0;
+    char tx_buffer[2];
+    
+    tx_buffer[0] = address;
+    tx_buffer[1] = cmd;    
+
+    rx_in = 0;    
+    crc_clear();
+
+    _roboclaw.flush();
+    _roboclaw.write(tx_buffer,2);
+    
+    crc_update(address);
+    crc_update(cmd);
+    
+    dataRead = _roboclaw.read_timeout(rx_buffer,7,read_timeout);
+    //printf("d: %d\r\n", dataRead);
+    //printf("add: %x,   cmd: %x,   0: %x,   1: %x,   2: %x,   3: %x,   4: %x,   5: %x,   6: %x \r\n",(uint16_t)tx_buffer[0],(uint16_t)tx_buffer[1],(uint16_t)rx_buffer[0],(uint16_t)rx_buffer[1],(uint16_t)rx_buffer[2],(uint16_t)rx_buffer[3],(uint16_t)rx_buffer[4],(uint16_t)rx_buffer[5],(uint16_t)rx_buffer[6]); 
+    //printf("dopo del read\r\n");       
+    for(int i = 0; i<=n_byte-3; i++){
+        crc_update((uint16_t)rx_buffer[i]);
+    }
+    
+    received_crc = (uint16_t)rx_buffer[n_byte-2]<<8;
+    received_crc |= (uint16_t)rx_buffer[n_byte-1];
+    
+    if(received_crc == crc_get()){
+        value = (uint16_t)rx_buffer[0]<<24;
+        value |= (uint16_t)rx_buffer[1]<<16;
+        value |= (uint16_t)rx_buffer[2]<<8;
+        value |= (uint16_t)rx_buffer[3];
+        return true;
+    }else{
+        //printf("crc: %x \r\n", crc_get());
+        //printf("received crc: %x \r\n", received_crc);
+        printf("err \r\n");
+
+
+        return false;        
+    }
+}
+
+
+void RoboClaw::ForwardM1(int speed){
+    write_(M1FORWARD,speed,false,false);
+}
+
+void RoboClaw::BackwardM1(int speed){
+    write_(M1BACKWARD,speed,false,false);
+}
+
+void RoboClaw::ForwardM2(int speed){
+    write_(M2FORWARD,speed,false,false);
+}
+
+void RoboClaw::BackwardM2(int speed){
+    write_(M2BACKWARD,speed,false,false);
+}
+
+void RoboClaw::Forward(int speed){
+    write_(MIXEDFORWARD,speed,false,false);
+}
+
+void RoboClaw::Backward(int speed){
+    write_(MIXEDBACKWARD,speed,false,false);
+}
+
+void RoboClaw::ReadFirm(){
+    write_(GETVERSION,0x00,true,false);
+}
+
+bool RoboClaw::ReadEncM1(int32_t &encPulse){
+    int32_t value=0;
+    if (read_n(address, GETM1ENC, 7, value) == true){
+        encPulse = value;
+        return true;
+    }else{
+        return false;
+    }
+}
+
+bool RoboClaw::ReadEncM2(int32_t &encPulse){
+    int32_t value=0;
+    if (read_n(address, GETM2ENC, 7, value) == true){
+        encPulse = value;
+        return true;
+    }else{
+        return false;
+    }
+}
+
+bool RoboClaw::ReadSpeedM1(int32_t &speed){
+    int32_t value=0;
+    if (read_n(address, GETM1SPEED, 7, value) == true){
+        speed = value;
+        return true;
+    }else{
+        return false;
+    }
+}
+
+bool RoboClaw::ReadSpeedM2(int32_t &speed){
+    int32_t value=0;
+    if (read_n(address, GETM2SPEED, 7, value) == true){
+        speed = value;
+        return true;
+    }else{
+        return false;
+    }
+}
+
+
+bool RoboClaw::ReadCurrentM1M2(int32_t& currentM1, int32_t& currentM2){
+    int32_t value=0;
+    if (read_n(address, GETCURRENTS, 6, value) == true){
+        currentM1 = value>>16;
+        currentM2 = value&0xFFFF;
+        return true;
+    }else{
+        return false;
+    }
+
+}
+
+void RoboClaw::ResetEnc(){
+    write_n(2,address,RESETENC);
+}
+
+void RoboClaw::SpeedM1(int32_t speed){
+    write_n(6,address,M1SPEED,SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedM2(int32_t speed){
+    write_n(6,address,M2SPEED,SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM1(int32_t accel, int32_t speed){
+    write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM2(int32_t accel, int32_t speed){
+    write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM1M2(int32_t accel, int32_t speed1, int32_t speed2){
+    write_n(14,address,MIXEDSPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed1),SetDWORDval(speed2));
+}
+
+void RoboClaw::SpeedDistanceM1(int32_t speed, uint32_t distance, uint8_t buffer){
+    write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedDistanceM2(int32_t speed, uint32_t distance, uint8_t buffer){
+    write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedAccelDistanceM1(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
+    write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedAccelDistanceM2(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
+    write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedAccelDeccelPositionM1(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
+    write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
+}
+
+void RoboClaw::SpeedAccelDeccelPositionM2(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
+    write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
+}
+
+void RoboClaw::SpeedAccelDeccelPositionM1M2(uint32_t accel1,uint32_t speed1,uint32_t deccel1, int32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2, int32_t position2,uint8_t flag){
+    write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag);
+}
+

diff -r 7912ab1400c4 -r 31695331ce17 RoboClaw/RoboClaw.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw/RoboClaw.h	Mon Jan 10 20:38:41 2022 +0000
@@ -0,0 +1,121 @@
+#ifndef ROBOCLAW_H
+#define ROBOCLAW_H
+#include "mbed.h"
+#include "registers.h"
+#include "UARTSerial_mio.h"
+/** The RoboClaw class, yo ucan use the library with : http://www.ionmc.com/RoboClaw-2x15A-Motor-Controller_p_10.html
+*   Used to control one or two motors with (or not) encoders
+*  @code
+* #include "mbed.h"
+* #include "RoboClaw.h"
+*
+* RoboClaw roboclaw(115200, PA_11, PA_12);
+* 
+* int main() {
+*     roboclaw.ForwardM1(ADR, 127);
+*     while(1);
+* }
+* @endcode
+*/
+
+class RoboClaw
+{
+public:
+    /** Create RoboClaw instance
+    */
+    RoboClaw(uint8_t adr, int baudrate, PinName rx, PinName tx, double _read_timeout);
+    
+    /** Forward and Backward functions
+    * @param address address of the device
+    * @param speed speed of the motor (between 0 and 127)
+    * @note Forward and Backward functions
+    */
+    void ForwardM1(int speed);
+    void BackwardM1(int speed);
+    void ForwardM2(int speed);
+    void BackwardM2(int speed);
+    
+    /** Forward and Backward functions
+    * @param address address of the device
+    * @param speed speed of the motor (between 0 and 127)
+    * @note Forward and Backward functions, it turns the two motors
+    */
+    void Forward(int speed);
+    void Backward(int speed);
+    
+    /** Read the Firmware
+    * @param address address of the device
+    */
+    void ReadFirm();
+    
+    /** Read encoder and speed of M1 or M2
+    * @param address address of the device
+    * @note Read encoder in ticks
+    * @note Read speed in ticks per second
+    */
+    bool ReadEncM1(int32_t &encPulse);
+    bool ReadEncM2(int32_t &encPulse);
+    bool ReadSpeedM1(int32_t &speed);
+    bool ReadSpeedM2(int32_t &speed);
+    bool ReadCurrentM1M2(int32_t &currentM1, int32_t &currentM2);
+    
+    
+    /** Set both encoders to zero
+    * @param address address of the device
+    */
+    void ResetEnc();
+    
+    /** Set speed of Motor with different parameter (only in ticks)
+    * @param address address of the device
+    * @note Set the Speed
+    * @note Set the Speed and Accel
+    * @note Set the Speed and Distance
+    * @note Set the Speed, Accel and Distance
+    * @note Set the Speed, Accel, Decceleration and Position
+    */
+    void SpeedM1(int32_t speed);
+    void SpeedM2(int32_t speed);
+    void SpeedAccelM1(int32_t accel, int32_t speed);
+    void SpeedAccelM2(int32_t accel, int32_t speed);
+    void SpeedAccelM1M2(int32_t accel, int32_t speed1, int32_t speed2);
+    void SpeedDistanceM1(int32_t speed, uint32_t distance, uint8_t buffer);
+    void SpeedDistanceM2(int32_t speed, uint32_t distance, uint8_t buffer);
+    void SpeedAccelDistanceM1(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer);
+    void SpeedAccelDistanceM2(int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer);
+    void SpeedAccelDeccelPositionM1(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag);
+    void SpeedAccelDeccelPositionM2(uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag);
+    void SpeedAccelDeccelPositionM1M2(uint32_t accel1,uint32_t speed1,uint32_t deccel1, int32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2, int32_t position2,uint8_t flag);
+    
+private:
+    UARTSerial_mio _roboclaw;
+    uint16_t crc;
+    uint8_t address;
+    void crc_clear();
+    void crc_update(uint8_t data);
+    uint16_t crc_get();
+    
+    void write_n(uint8_t cnt, ...);
+    void write_(uint8_t command, uint8_t data, bool reading, bool crcon);
+    
+    uint16_t crc16(uint8_t *packet, int nBytes);
+    int16_t read_(void);
+    bool read_n(uint8_t address, uint8_t cmd, int n_byte, int32_t &value);
+    void flushSerialBuffer(void);
+    void Rx_interrupt();
+    // Circular buffers for serial TX and RX data - used by interrupt routines
+    const int buffer_size = 7;
+    // might need to increase buffer size for high baud rates
+    char rx_buffer[7];
+    // Circular buffer pointers
+    // volatile makes read-modify-write atomic 
+    //Timer readTimer;
+    //double readTime;
+    //double readTimePrec;
+    volatile int rx_in;
+    double read_timeout;
+    // Line buffers for sprintf and sscanf
+
+    char rx_line[80];
+};
+
+#endif
\ No newline at end of file

diff -r 7912ab1400c4 -r 31695331ce17 RoboClaw/UARTserial_mio/UARTSerial_mio.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw/UARTserial_mio/UARTSerial_mio.cpp	Mon Jan 10 20:38:41 2022 +0000
@@ -0,0 +1,482 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2017 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "UARTSerial_mio.h"
+
+#if (DEVICE_SERIAL && DEVICE_INTERRUPTIN)
+
+#include "platform/mbed_poll.h"
+
+#if MBED_CONF_RTOS_PRESENT
+#include "rtos/ThisThread.h"
+#else
+#include "platform/mbed_wait_api.h"
+#endif
+
+namespace mbed {
+
+UARTSerial_mio::UARTSerial_mio(PinName tx, PinName rx, int baud) :
+    SerialBase(tx, rx, baud),
+    _blocking(true),
+    _tx_irq_enabled(false),
+    _rx_irq_enabled(false),
+    _tx_enabled(true),
+    _rx_enabled(true),
+    _dcd_irq(NULL)
+{
+    /* Attatch IRQ routines to the serial device. */
+    enable_rx_irq();
+}
+
+UARTSerial_mio::~UARTSerial_mio()
+{
+    delete _dcd_irq;
+}
+
+void UARTSerial_mio::dcd_irq()
+{
+    wake();
+}
+
+void UARTSerial_mio::set_baud(int baud)
+{
+    SerialBase::baud(baud);
+}
+
+void UARTSerial_mio::set_format(int bits, Parity parity, int stop_bits)
+{
+    api_lock();
+    SerialBase::format(bits, parity, stop_bits);
+    api_unlock();
+}
+
+#if DEVICE_SERIAL_FC
+void UARTSerial_mio::set_flow_control(Flow type, PinName flow1, PinName flow2)
+{
+    api_lock();
+    SerialBase::set_flow_control(type, flow1, flow2);
+    api_unlock();
+}
+#endif
+
+int UARTSerial_mio::close(){return 0;}
+
+int UARTSerial_mio::isatty(){return 1;}
+
+
+off_t UARTSerial_mio::seek(off_t offset, int whence)
+{
+    /*XXX lseek can be done theoratically, but is it sane to mark positions on a dynamically growing/shrinking
+     * buffer system (from an interrupt context) */
+    return -ESPIPE;
+}
+
+int UARTSerial_mio::sync()
+{
+    api_lock();
+    while (!_txbuf.empty()) {
+        api_unlock();
+        // Doing better than wait would require TxIRQ to also do wake() when becoming empty. Worth it?
+        wait_us(500);
+        api_lock();
+    }
+    
+    api_unlock();
+
+    return 0;
+}
+
+int UARTSerial_mio::flush()
+{
+    api_lock();
+    char c;
+
+    while (!_rxbuf.empty()) {
+        api_unlock();
+        // Doing better than wait would require TxIRQ to also do wake() when becoming empty. Worth it?
+        wait_us(500);
+        _rxbuf.pop(c);
+        api_lock();
+    }
+
+    api_unlock();
+
+    return 0;
+}
+
+void UARTSerial_mio::sigio(Callback<void()> func)
+{
+    core_util_critical_section_enter();
+    _sigio_cb = func;
+    if (_sigio_cb) {
+        short current_events = poll(0x7FFF);
+        if (current_events) {
+            _sigio_cb();
+        }
+    }
+    core_util_critical_section_exit();
+}
+
+/* Special synchronous write designed to work from critical section, such
+ * as in mbed_error_vprintf.
+ */
+ssize_t UARTSerial_mio::write_unbuffered(const char *buf_ptr, size_t length)
+{
+    while (!_txbuf.empty()) {
+        tx_irq();
+    }
+
+    for (size_t data_written = 0; data_written < length; data_written++) {
+        SerialBase::_base_putc(*buf_ptr++);
+    }
+
+    return length;
+}
+
+ssize_t UARTSerial_mio::write(const void *buffer, size_t length)
+{
+    size_t data_written = 0;
+    const char *buf_ptr = static_cast<const char *>(buffer);
+
+    if (length == 0) {
+        return 0;
+    }
+
+    if (core_util_in_critical_section()) {
+        return write_unbuffered(buf_ptr, length);
+    }
+
+    api_lock();
+
+    // Unlike read, we should write the whole thing if blocking. POSIX only
+    // allows partial as a side-effect of signal handling; it normally tries to
+    // write everything if blocking. Without signals we can always write all.
+    while (data_written < length) {
+
+        if (_txbuf.full()) {
+            if (!_blocking) {
+                break;
+            }
+            do {
+                api_unlock();
+                wait_ms(1); // XXX todo - proper wait, WFE for non-rtos ?
+                api_lock();
+            } while (_txbuf.full());
+        }
+
+        while (data_written < length && !_txbuf.full()) {
+            _txbuf.push(*buf_ptr++);
+            data_written++;
+        }
+
+        core_util_critical_section_enter();
+        if (_tx_enabled && !_tx_irq_enabled) {
+            UARTSerial_mio::tx_irq();                // only write to hardware in one place
+            if (!_txbuf.empty()) {
+                enable_tx_irq();
+            }
+        }
+        core_util_critical_section_exit();
+    }
+
+    api_unlock();
+
+    return data_written != 0 ? (ssize_t) data_written : (ssize_t) - EAGAIN;
+}
+
+ssize_t UARTSerial_mio::read(void *buffer, size_t length)
+{
+    size_t data_read = 0;
+    
+    float timeout = 1.0; //ms
+    float tm = 0.0;
+
+    char *ptr = static_cast<char *>(buffer);
+
+    if (length == 0) {
+        return 0;
+    }
+
+    api_lock();
+
+    while (_rxbuf.size()!=length && tm <= timeout) {
+        if (!_blocking) {
+            api_unlock();
+            return -EAGAIN;
+        }
+        api_unlock();
+        wait_us(10);  // XXX todo - proper wait, WFE for non-rtos ?
+        api_lock();
+        tm = tm + 0.01; //10/1000
+    }
+
+    while (data_read < length && !_rxbuf.empty()) {
+        _rxbuf.pop(*ptr++);
+        data_read++;
+    }
+
+    core_util_critical_section_enter();
+    if (_rx_enabled && !_rx_irq_enabled) {
+        UARTSerial_mio::rx_irq();               // only read from hardware in one place
+        if (!_rxbuf.full()) {
+            enable_rx_irq();
+        }
+    }
+    core_util_critical_section_exit();
+
+    api_unlock();
+
+    return data_read;
+}
+
+ssize_t UARTSerial_mio::read_timeout(void *buffer, size_t length, double _timeOut)
+{
+    size_t data_read = 0;
+    
+    double timeout = _timeOut; //ms
+    double tm = 0.0;
+
+    char *ptr = static_cast<char *>(buffer);
+
+    if (length == 0) {
+        return 0;
+    }
+
+    api_lock();
+
+    while (_rxbuf.size()!=length && tm<=timeout) {
+        if (!_blocking) {
+            api_unlock();
+            return -EAGAIN;
+        }
+        api_unlock();
+        wait_us(1);  // XXX todo - proper wait, WFE for non-rtos ?
+        api_lock();
+        tm = tm + 0.001; //10/1000
+        
+    }
+    
+    //printf("tm: %f\r\n",tm);
+    tm = 0.0;
+    while (data_read < length && !_rxbuf.empty()  && tm<=timeout) {
+        _rxbuf.pop(*ptr++);
+        data_read++;
+        tm = tm + 0.001; //10/1000
+    }
+
+    core_util_critical_section_enter();
+    if (_rx_enabled && !_rx_irq_enabled) {
+        UARTSerial_mio::rx_irq();               // only read from hardware in one place
+        if (!_rxbuf.full()) {
+            enable_rx_irq();
+        }
+    }
+    core_util_critical_section_exit();
+
+    api_unlock();
+
+    return data_read;
+}
+
+bool UARTSerial_mio::hup() const
+{
+    return _dcd_irq && _dcd_irq->read() != 0;
+}
+
+void UARTSerial_mio::wake()
+{
+    if (_sigio_cb) {
+        _sigio_cb();
+    }
+}
+
+short UARTSerial_mio::poll(short events) const
+{
+
+    short revents = 0;
+    /* Check the Circular Buffer if space available for writing out */
+
+
+    if (!_rxbuf.empty()) {
+        revents |= POLLIN;
+    }
+
+    /* POLLHUP and POLLOUT are mutually exclusive */
+    if (hup()) {
+        revents |= POLLHUP;
+    } else if (!_txbuf.full()) {
+        revents |= POLLOUT;
+    }
+
+    /*TODO Handle other event types */
+
+    return revents;
+}
+
+void UARTSerial_mio::lock()
+{
+    // This is the override for SerialBase.
+    // No lock required as we only use SerialBase from interrupt or from
+    // inside our own critical section.
+}
+
+void UARTSerial_mio::unlock()
+{
+    // This is the override for SerialBase.
+}
+
+void UARTSerial_mio::api_lock(void)
+{
+    //_mutex.lock();
+}
+
+void UARTSerial_mio::api_unlock(void)
+{
+    //_mutex.unlock();
+}
+
+void UARTSerial_mio::rx_irq(void)
+{
+    bool was_empty = _rxbuf.empty();
+
+    /* Fill in the receive buffer if the peripheral is readable
+     * and receive buffer is not full. */
+    while (!_rxbuf.full() && SerialBase::readable()) {
+        char data = SerialBase::_base_getc();
+        _rxbuf.push(data);
+    }
+
+    if (_rx_irq_enabled && _rxbuf.full()) {
+        disable_rx_irq();
+    }
+
+    /* Report the File handler that data is ready to be read from the buffer. */
+    if (was_empty && !_rxbuf.empty()) {
+        wake();
+    }
+}
+
+// Also called from write to start transfer
+void UARTSerial_mio::tx_irq(void)
+{
+    bool was_full = _txbuf.full();
+    char data;
+
+    /* Write to the peripheral if there is something to write
+     * and if the peripheral is available to write. */
+    while (SerialBase::writeable() && _txbuf.pop(data)) {
+        SerialBase::_base_putc(data);
+    }
+
+    if (_tx_irq_enabled && _txbuf.empty()) {
+        disable_tx_irq();
+    }
+
+    /* Report the File handler that data can be written to peripheral. */
+    if (was_full && !_txbuf.full() && !hup()) {
+        wake();
+    }
+}
+
+/* These are all called from critical section */
+void UARTSerial_mio::enable_rx_irq()
+{
+    SerialBase::attach(callback(this, &UARTSerial_mio::rx_irq), RxIrq);
+    _rx_irq_enabled = true;
+}
+
+void UARTSerial_mio::disable_rx_irq()
+{
+    SerialBase::attach(NULL, RxIrq);
+    _rx_irq_enabled = false;
+}
+
+void UARTSerial_mio::enable_tx_irq()
+{
+    SerialBase::attach(callback(this, &UARTSerial_mio::tx_irq), TxIrq);
+    _tx_irq_enabled = true;
+}
+
+void UARTSerial_mio::disable_tx_irq()
+{
+    SerialBase::attach(NULL, TxIrq);
+    _tx_irq_enabled = false;
+}
+
+int UARTSerial_mio::enable_input(bool enabled)
+{
+    core_util_critical_section_enter();
+    if (_rx_enabled != enabled) {
+        if (enabled) {
+            UARTSerial_mio::rx_irq();
+            if (!_rxbuf.full()) {
+                enable_rx_irq();
+            }
+        } else {
+            disable_rx_irq();
+        }
+        _rx_enabled = enabled;
+    }
+    core_util_critical_section_exit();
+
+    return 0;
+}
+
+int UARTSerial_mio::enable_output(bool enabled)
+{
+    core_util_critical_section_enter();
+    if (_tx_enabled != enabled) {
+        if (enabled) {
+            UARTSerial_mio::tx_irq();
+            if (!_txbuf.empty()) {
+                enable_tx_irq();
+            }
+        } else {
+            disable_tx_irq();
+        }
+        _tx_enabled = enabled;
+    }
+    core_util_critical_section_exit();
+
+    return 0;
+}
+
+void UARTSerial_mio::wait_ms(uint32_t millisec)
+{
+    /* wait_ms implementation for RTOS spins until exact microseconds - we
+     * want to just sleep until next tick.
+     */
+#if MBED_CONF_RTOS_PRESENT
+    rtos::ThisThread::sleep_for(millisec);
+#else
+    ::wait_ms(millisec);
+#endif
+}
+
+void UARTSerial_mio::wait_us(uint32_t microseconds)
+{
+    /* wait_ms implementation for RTOS spins until exact microseconds - we
+     * want to just sleep until next tick.
+     */
+#if MBED_CONF_RTOS_PRESENT
+    rtos::ThisThread::sleep_for(microseconds/1000);
+#else
+    ::wait_us(microseconds);
+#endif
+}
+} //namespace mbed
+
+#endif //(DEVICE_SERIAL && DEVICE_INTERRUPTIN)
\ No newline at end of file

diff -r 7912ab1400c4 -r 31695331ce17 RoboClaw/UARTserial_mio/UARTSerial_mio.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw/UARTserial_mio/UARTSerial_mio.h	Mon Jan 10 20:38:41 2022 +0000
@@ -0,0 +1,308 @@
+
+#ifndef MBED_UARTSERIAL_MIO_H
+#define MBED_UARTSERIAL_MIO_H
+
+#include "platform/platform.h"
+
+#if (DEVICE_SERIAL && DEVICE_INTERRUPTIN) || defined(DOXYGEN_ONLY)
+
+#include "platform/FileHandle.h"
+#include "SerialBase.h"
+#include "InterruptIn.h"
+#include "platform/PlatformMutex.h"
+#include "hal/serial_api.h"
+#include "platform/CircularBuffer.h"
+#include "platform/NonCopyable.h"
+
+#include "mbed.h"
+
+#ifndef MBED_CONF_DRIVERS_UART_SERIAL_RXBUF_SIZE
+#define MBED_CONF_DRIVERS_UART_SERIAL_RXBUF_SIZE  256
+#endif
+
+#ifndef MBED_CONF_DRIVERS_UART_SERIAL_TXBUF_SIZE
+#define MBED_CONF_DRIVERS_UART_SERIAL_TXBUF_SIZE  256
+#endif
+
+namespace mbed {
+
+/** \addtogroup drivers */
+
+/** Class providing buffered UART communication functionality using separate circular buffer for send and receive channels
+ *
+ * @ingroup drivers
+ */
+
+class UARTSerial_mio : private SerialBase, public FileHandle, private NonCopyable<UARTSerial_mio> {
+
+public:
+
+    /** Create a UARTSerial_mio port, connected to the specified transmit and receive pins, with a particular baud rate.
+     *  @param tx Transmit pin
+     *  @param rx Receive pin
+     *  @param baud The baud rate of the serial port (optional, defaults to MBED_CONF_PLATFORM_DEFAULT_SERIAL_BAUD_RATE)
+     */
+    UARTSerial_mio(PinName tx, PinName rx, int baud = MBED_CONF_PLATFORM_DEFAULT_SERIAL_BAUD_RATE);
+    virtual ~UARTSerial_mio();
+
+    /** Equivalent to POSIX poll(). Derived from FileHandle.
+     *  Provides a mechanism to multiplex input/output over a set of file handles.
+     */
+    virtual short poll(short events) const;
+
+    /* Resolve ambiguities versus our private SerialBase
+     * (for writable, spelling differs, but just in case)
+     */
+    using FileHandle::readable;
+    using FileHandle::writable;
+
+    /** Write the contents of a buffer to a file
+     *
+     *  Follows POSIX semantics:
+     *
+     * * if blocking, block until all data is written
+     * * if no data can be written, and non-blocking set, return -EAGAIN
+     * * if some data can be written, and non-blocking set, write partial
+     *
+     *  @param buffer   The buffer to write from
+     *  @param length   The number of bytes to write
+     *  @return         The number of bytes written, negative error on failure
+     */
+    virtual ssize_t write(const void *buffer, size_t length);
+
+    /** Read the contents of a file into a buffer
+     *
+     *  Follows POSIX semantics:
+     *
+     *  * if no data is available, and non-blocking set return -EAGAIN
+     *  * if no data is available, and blocking set, wait until data is available
+     *  * If any data is available, call returns immediately
+     *
+     *  @param buffer   The buffer to read in to
+     *  @param length   The number of bytes to read
+     *  @return         The number of bytes read, 0 at end of file, negative error on failure
+     */
+    virtual ssize_t read(void *buffer, size_t length);
+    virtual ssize_t read_timeout(void *buffer, size_t length, double _timeOut);
+
+    /** Close a file
+     *
+     *  @return         0 on success, negative error code on failure
+     */
+    virtual int close();
+
+    /** Check if the file in an interactive terminal device
+     *
+     *  @return         True if the file is a terminal
+     *  @return         False if the file is not a terminal
+     *  @return         Negative error code on failure
+     */
+    virtual int isatty();
+
+    /** Move the file position to a given offset from from a given location
+     *
+     * Not valid for a device type FileHandle like UARTSerial_mio.
+     * In case of UARTSerial_mio, returns ESPIPE
+     *
+     *  @param offset   The offset from whence to move to
+     *  @param whence   The start of where to seek
+     *      SEEK_SET to start from beginning of file,
+     *      SEEK_CUR to start from current position in file,
+     *      SEEK_END to start from end of file
+     *  @return         The new offset of the file, negative error code on failure
+     */
+    virtual off_t seek(off_t offset, int whence);
+
+    /** Flush any buffers associated with the file
+     *
+     *  @return         0 on success, negative error code on failure
+     */
+    virtual int sync();
+    virtual int flush();
+    /** Set blocking or non-blocking mode
+     *  The default is blocking.
+     *
+     *  @param blocking true for blocking mode, false for non-blocking mode.
+     */
+    virtual int set_blocking(bool blocking)
+    {
+        _blocking = blocking;
+        return 0;
+    }
+
+    /** Check current blocking or non-blocking mode for file operations.
+     *
+     *  @return             true for blocking mode, false for non-blocking mode.
+     */
+    virtual bool is_blocking() const
+    {
+        return _blocking;
+    }
+
+    /** Enable or disable input
+     *
+     * Control enabling of device for input. This is primarily intended
+     * for temporary power-saving; the overall ability of the device to operate for
+     * input and/or output may be fixed at creation time, but this call can
+     * allow input to be temporarily disabled to permit power saving without
+     * losing device state.
+     *
+     *  @param enabled      true to enable input, false to disable.
+     *
+     *  @return             0 on success
+     *  @return             Negative error code on failure
+     */
+    virtual int enable_input(bool enabled);
+
+    /** Enable or disable output
+     *
+     * Control enabling of device for output. This is primarily intended
+     * for temporary power-saving; the overall ability of the device to operate for
+     * input and/or output may be fixed at creation time, but this call can
+     * allow output to be temporarily disabled to permit power saving without
+     * losing device state.
+     *
+     *  @param enabled      true to enable output, false to disable.
+     *
+     *  @return             0 on success
+     *  @return             Negative error code on failure
+     */
+    virtual int enable_output(bool enabled);
+
+    /** Register a callback on state change of the file.
+     *
+     *  The specified callback will be called on state changes such as when
+     *  the file can be written to or read from.
+     *
+     *  The callback may be called in an interrupt context and should not
+     *  perform expensive operations.
+     *
+     *  Note! This is not intended as an attach-like asynchronous api, but rather
+     *  as a building block for constructing  such functionality.
+     *
+     *  The exact timing of when the registered function
+     *  is called is not guaranteed and susceptible to change. It should be used
+     *  as a cue to make read/write/poll calls to find the current state.
+     *
+     *  @param func     Function to call on state change
+     */
+    virtual void sigio(Callback<void()> func);
+
+    /** Setup interrupt handler for DCD line
+     *
+     *  If DCD line is connected, an IRQ handler will be setup.
+     *  Does nothing if DCD is NC, i.e., not connected.
+     *
+     *  @param dcd_pin         Pin-name for DCD
+     *  @param active_high     a boolean set to true if DCD polarity is active low
+     */
+    void set_data_carrier_detect(PinName dcd_pin, bool active_high = false);
+
+    /** Set the baud rate
+     *
+     *  @param baud   The baud rate
+     */
+    void set_baud(int baud);
+
+    // Expose private SerialBase::Parity as UARTSerial_mio::Parity
+    using SerialBase::Parity;
+    // In C++11, we wouldn't need to also have using directives for each value
+    using SerialBase::None;
+    using SerialBase::Odd;
+    using SerialBase::Even;
+    using SerialBase::Forced1;
+    using SerialBase::Forced0;
+
+    /** Set the transmission format used by the serial port
+     *
+     *  @param bits The number of bits in a word (5-8; default = 8)
+     *  @param parity The parity used (None, Odd, Even, Forced1, Forced0; default = None)
+     *  @param stop_bits The number of stop bits (1 or 2; default = 1)
+     */
+    void set_format(int bits = 8, Parity parity = UARTSerial_mio::None, int stop_bits = 1);
+
+#if DEVICE_SERIAL_FC
+    // For now use the base enum - but in future we may have extra options
+    // such as XON/XOFF or manual GPIO RTSCTS.
+    using SerialBase::Flow;
+    // In C++11, we wouldn't need to also have using directives for each value
+    using SerialBase::Disabled;
+    using SerialBase::RTS;
+    using SerialBase::CTS;
+    using SerialBase::RTSCTS;
+
+    /** Set the flow control type on the serial port
+     *
+     *  @param type the flow control type (Disabled, RTS, CTS, RTSCTS)
+     *  @param flow1 the first flow control pin (RTS for RTS or RTSCTS, CTS for CTS)
+     *  @param flow2 the second flow control pin (CTS for RTSCTS)
+     */
+    void set_flow_control(Flow type, PinName flow1 = NC, PinName flow2 = NC);
+#endif
+
+private:
+
+    void wait_ms(uint32_t millisec);
+    void wait_us(uint32_t microsec);
+
+    /** SerialBase lock override */
+    virtual void lock(void);
+
+    /** SerialBase unlock override */
+    virtual void unlock(void);
+
+    /** Acquire mutex */
+    virtual void api_lock(void);
+
+    /** Release mutex */
+    virtual void api_unlock(void);
+
+    /** Unbuffered write - invoked when write called from critical section */
+    ssize_t write_unbuffered(const char *buf_ptr, size_t length);
+
+    void enable_rx_irq();
+    void disable_rx_irq();
+    void enable_tx_irq();
+    void disable_tx_irq();
+
+    /** Software serial buffers
+     *  By default buffer size is 256 for TX and 256 for RX. Configurable through mbed_app.json
+     */
+    CircularBuffer<char, MBED_CONF_DRIVERS_UART_SERIAL_RXBUF_SIZE> _rxbuf;
+    CircularBuffer<char, MBED_CONF_DRIVERS_UART_SERIAL_TXBUF_SIZE> _txbuf;
+
+    PlatformMutex _mutex;
+
+    Callback<void()> _sigio_cb;
+
+    bool _blocking;
+    bool _tx_irq_enabled;
+    bool _rx_irq_enabled;
+    bool _tx_enabled;
+    bool _rx_enabled;
+    InterruptIn *_dcd_irq;
+
+    /** Device Hanged up
+     *  Determines if the device hanged up on us.
+     *
+     *  @return True, if hanged up
+     */
+    bool hup() const;
+
+    /** ISRs for serial
+     *  Routines to handle interrupts on serial pins.
+     *  Copies data into Circular Buffer.
+     *  Reports the state change to File handle.
+     */
+    void tx_irq(void);
+    void rx_irq(void);
+
+    void wake(void);
+
+    void dcd_irq(void);
+
+};
+} //namespace mbed
+
+#endif //(DEVICE_SERIAL && DEVICE_INTERRUPTIN) || defined(DOXYGEN_ONLY)
+#endif //MBED_UARTSERIAL_H
\ No newline at end of file

diff -r 7912ab1400c4 -r 31695331ce17 RoboClaw/registers.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw/registers.h	Mon Jan 10 20:38:41 2022 +0000
@@ -0,0 +1,87 @@
+#define ADR 0x80
+#define M1FORWARD 0
+#define M1BACKWARD 1
+#define SETMINMB 2
+#define SETMAXMB 3
+#define M2FORWARD 4
+#define M2BACKWARD 5
+#define M17BIT 6
+#define M27BIT 7
+#define MIXEDFORWARD 8
+#define MIXEDBACKWARD 9
+#define MIXEDRIGHT 10
+#define MIXEDLEFT 11
+#define MIXEDFB 12
+#define MIXEDLR 13
+#define GETM1ENC 16
+#define GETM2ENC 17
+#define GETM1SPEED 18
+#define GETM2SPEED 19
+#define RESETENC 20
+#define GETVERSION 21
+#define SETM1ENCCOUNT  22
+#define SETM2ENCCOUNT  23
+#define GETMBATT  24
+#define GETLBATT  25
+#define SETMINLB  26
+#define SETMAXLB  27
+#define SETM1PID  28
+#define SETM2PID  29
+#define GETM1ISPEED  30
+#define GETM2ISPEED  31
+#define M1DUTY  32
+#define M2DUTY  33
+#define MIXEDDUTY  34
+#define M1SPEED  35
+#define M2SPEED  36
+#define MIXEDSPEED  37
+#define M1SPEEDACCEL  38
+#define M2SPEEDACCEL  39
+#define MIXEDSPEEDACCEL  40
+#define M1SPEEDDIST  41
+#define M2SPEEDDIST  42
+#define MIXEDSPEEDDIST  43
+#define M1SPEEDACCELDIST  44
+#define M2SPEEDACCELDIST  45
+#define MIXEDSPEEDACCELDIST  46
+#define GETBUFFERS  47
+#define GETCURRENTS  49
+#define MIXEDSPEED2ACCEL  50
+#define MIXEDSPEED2ACCELDIST  51
+#define M1DUTYACCEL  52
+#define M2DUTYACCEL  53
+#define MIXEDDUTYACCEL  54
+#define READM1PID  55
+#define READM2PID  56
+#define SETMAINVOLTAGES  57
+#define SETLOGICVOLTAGES  58
+#define GETMINMAXMAINVOLTAGES  59
+#define GETMINMAXLOGICVOLTAGES  60
+#define SETM1POSPID  61
+#define SETM2POSPID  62
+#define READM1POSPID  63
+#define READM2POSPID  64
+#define M1SPEEDACCELDECCELPOS  65
+#define M2SPEEDACCELDECCELPOS  66
+#define MIXEDSPEEDACCELDECCELPOS  67
+#define SETM1DEFAULTACCEL  68
+#define SETM2DEFAULTACCEL  69
+#define SETPINFUNCTIONS  74
+#define GETPINFUNCTIONS  75
+#define RESTOREDEFAULTS  80
+#define GETTEMP  82
+#define GETTEMP2  83
+#define GETERROR  90
+#define GETENCODERMODE  91
+#define SETM1ENCODERMODE  92
+#define SETM2ENCODERMODE  93
+#define WRITENVM  94
+#define READNVM  95
+#define SETCONFIG  98
+#define GETCONFIG  99
+#define SETM1MAXCURRENT  133
+#define SETM2MAXCURRENT  134
+#define GETM1MAXCURRENT  135
+#define GETM2MAXCURRENT  136
+#define SETPWMMODE  148
+#define GETPWMMODE 149
\ No newline at end of file