From be89e4a429f6ba9713a88900fd7f00f39f4e36e7 Mon Sep 17 00:00:00 2001 From: sanosuke216 Date: Mon, 13 Nov 2023 06:58:02 +0900 Subject: [PATCH] stickCplus_base --- arduino/include/UNIT_HBRIDGE.h | 50 ++++++++++++ arduino/src/UNIT_HBRIDGE.cpp | 135 +++++++++++++++++++++++++++++++++ 2 files changed, 185 insertions(+) create mode 100644 arduino/include/UNIT_HBRIDGE.h create mode 100644 arduino/src/UNIT_HBRIDGE.cpp diff --git a/arduino/include/UNIT_HBRIDGE.h b/arduino/include/UNIT_HBRIDGE.h new file mode 100644 index 0000000..d640e2d --- /dev/null +++ b/arduino/include/UNIT_HBRIDGE.h @@ -0,0 +1,50 @@ +#ifndef __UNIT_HBRIDGE_H +#define __UNIT_HBRIDGE_H + +#include "Arduino.h" +#include "Wire.h" + +#define HBRIDGE_ADDR 0x20 +#define DRIVER_CONFIG_REG 0x00 +#define MOTOR_ADC_8BIT_REG 0x10 +#define MOTOR_ADC_12BIT_REG 0x20 +#define MOTOR_CURRENT_REG 0x30 +#define JUMP_TO_BOOTLOADER_REG 0xFD +#define HBRIDGE_FW_VERSION_REG 0xFE +#define HBRIDGE_I2C_ADDRESS_REG 0xFF + +#define DIR_STOP 0 +#define DIR_FORWARD 1 +#define DIR_REVERSE 2 + +typedef enum { _8bit = 0, _12bit } hbridge_anolog_read_mode_t; + +class UNIT_HBRIDGE { + private: + uint8_t _addr; + TwoWire* _wire; + uint8_t _scl; + uint8_t _sda; + uint8_t _speed; + void writeBytes(uint8_t addr, uint8_t reg, uint8_t* buffer, uint8_t length); + void readBytes(uint8_t addr, uint8_t reg, uint8_t* buffer, uint8_t length); + + public: + bool begin(TwoWire* wire = &Wire, uint8_t addr = HBRIDGE_ADDR, + uint8_t sda = 21, uint8_t scl = 22, uint32_t speed = 100000L); + uint8_t getDriverDirection(void); + uint8_t getDriverSpeed8Bits(void); + uint16_t getDriverSpeed16Bits(void); + void setDriverDirection(uint8_t dir); + void setDriverSpeed8Bits(uint8_t speed); + void setDriverSpeed16Bits(uint16_t speed); + uint16_t getDriverPWMFreq(void); + void setDriverPWMFreq(uint16_t freq); + float getMotorCurrent(void); + uint16_t getAnalogInput(hbridge_anolog_read_mode_t bit); + void jumpBootloader(void); + uint8_t getFirmwareVersion(void); + uint8_t getI2CAddress(void); +}; + +#endif diff --git a/arduino/src/UNIT_HBRIDGE.cpp b/arduino/src/UNIT_HBRIDGE.cpp new file mode 100644 index 0000000..ab19756 --- /dev/null +++ b/arduino/src/UNIT_HBRIDGE.cpp @@ -0,0 +1,135 @@ +#include "UNIT_HBRIDGE.h" + +void UNIT_HBRIDGE::writeBytes(uint8_t addr, uint8_t reg, uint8_t *buffer, + uint8_t length) { + _wire->beginTransmission(addr); + _wire->write(reg); + for (int i = 0; i < length; i++) { + _wire->write(*(buffer + i)); + } + _wire->endTransmission(); +} + +void UNIT_HBRIDGE::readBytes(uint8_t addr, uint8_t reg, uint8_t *buffer, + uint8_t length) { + uint8_t index = 0; + _wire->beginTransmission(addr); + _wire->write(reg); + _wire->endTransmission(); + _wire->requestFrom(addr, length); + for (int i = 0; i < length; i++) { + buffer[index++] = _wire->read(); + } +} + +bool UNIT_HBRIDGE::begin(TwoWire *wire, uint8_t addr, uint8_t sda, uint8_t scl, + uint32_t speed) { + _wire = wire; + _addr = addr; + _sda = sda; + _scl = scl; + _speed = speed; + _wire->begin(_sda, _scl, _speed); + delay(10); + _wire->beginTransmission(_addr); + uint8_t error = _wire->endTransmission(); + if (error == 0) { + return true; + } else { + return false; + } +} + +uint8_t UNIT_HBRIDGE::getDriverDirection(void) { + uint8_t data[4]; + readBytes(_addr, DRIVER_CONFIG_REG, data, 1); + return data[0]; +} + +uint8_t UNIT_HBRIDGE::getDriverSpeed8Bits(void) { + uint8_t data[4]; + readBytes(_addr, DRIVER_CONFIG_REG + 1, data, 1); + return data[0]; +} + +uint16_t UNIT_HBRIDGE::getDriverSpeed16Bits(void) { + uint8_t data[4]; + readBytes(_addr, DRIVER_CONFIG_REG + 2, data, 2); + return (data[0] | (data[1] << 8)); +} + +uint16_t UNIT_HBRIDGE::getDriverPWMFreq(void) { + uint8_t data[4]; + readBytes(_addr, DRIVER_CONFIG_REG + 4, data, 2); + return (data[0] | (data[1] << 8)); +} + +void UNIT_HBRIDGE::setDriverPWMFreq(uint16_t freq) { + uint8_t data[4]; + data[0] = (freq & 0xff); + data[1] = ((freq >> 8) & 0xff); + writeBytes(_addr, DRIVER_CONFIG_REG + 4, data, 2); +} + +void UNIT_HBRIDGE::setDriverDirection(uint8_t dir) { + uint8_t data[4]; + data[0] = dir; + writeBytes(_addr, DRIVER_CONFIG_REG, data, 1); +} + +void UNIT_HBRIDGE::setDriverSpeed8Bits(uint8_t speed) { + uint8_t data[4]; + data[0] = speed; + writeBytes(_addr, DRIVER_CONFIG_REG + 1, data, 1); +} + +void UNIT_HBRIDGE::setDriverSpeed16Bits(uint16_t speed) { + uint8_t data[4]; + data[0] = speed; + data[1] = (speed >> 8); + writeBytes(_addr, DRIVER_CONFIG_REG + 2, data, 2); +} + +uint16_t UNIT_HBRIDGE::getAnalogInput(hbridge_anolog_read_mode_t bit) { + if (bit == _8bit) { + uint8_t data; + readBytes(_addr, MOTOR_ADC_8BIT_REG, &data, 1); + return data; + } else { + uint8_t data[2]; + readBytes(_addr, MOTOR_ADC_12BIT_REG, data, 2); + return (data[0] | (data[1] << 8)); + } +} + +// Only V1.1 can use this +float UNIT_HBRIDGE::getMotorCurrent(void) { + uint8_t data[4]; + float c; + uint8_t *p; + + readBytes(_addr, MOTOR_CURRENT_REG, data, 4); + p = (uint8_t *)&c; + memcpy(p, data, 4); + + return c; +} + +uint8_t UNIT_HBRIDGE::getFirmwareVersion(void) { + uint8_t data[4]; + readBytes(_addr, HBRIDGE_FW_VERSION_REG, data, 1); + return data[0]; +} + +uint8_t UNIT_HBRIDGE::getI2CAddress(void) { + uint8_t data[4]; + readBytes(_addr, HBRIDGE_I2C_ADDRESS_REG, data, 1); + return data[0]; +} + +// Only V1.1 can use this +void UNIT_HBRIDGE::jumpBootloader(void) { + uint8_t value = 1; + + writeBytes(_addr, JUMP_TO_BOOTLOADER_REG, (uint8_t *)&value, 1); +}