ゆっくりラズパイ活用講座

BugC

ESP32S3

GND

　--------------　

GND

xxxxxx

SCL

　--------------　

SCL

xxxxxx

SDA

　--------------　

SDA

BAT

　-- 3.7V→5V --　

3.3V

　--------------　

3.3V

xxxxxx

/*-------------------------------------------------------*/ | MOTOR SPEED REG | 0x00 | ------------------------------------------------------- | FRONT_LEFT_reg[0] | R/W | FRONT_LEFT SPEED | FRONT_RIGHT_reg[1] | R/W | FRONT_RIGHT SPEED | REAR_LEFT_reg[2] | R/W | REAR_LEFT SPEED | REAR_RIGHT_reg[3] | R/W | REAR_RIGHT SPEED /*------------------------------------------------------- Wire.beginTransmission(0x38); Wire.write(0x00); Wire.write(FRONT_LEFT SPEED); Wire.write(FRONT_RIGHT SPEED); Wire.write(REAR_LEFT SPEED); Wire.write(REAR_RIGHT SPEED); Wire.endTransmission(); /*-------------------------------------------------------*/ | RGB LED COLOR REG | 0x10 | ------------------------------------------------------- | left/right_reg[0] | R/W | left/right control | | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | | | R | R | R | R | R | R | R | SELECT | | | -SELECT: 0 FRONT_LEFT RGB LED | | 1 FRONT_RIGHT RGB LED | rgb_r_reg[1] | R/W | RED value | rgb_g_reg[2] | R/W | Green value | rgb_b_reg[3] | R/W | Blue value /*------------------------------------------------------- Wire.beginTransmission(0x38); Wire.write(0x10); Wire.write(0); // FRONT_LEFT RGB LED Wire.write(Red); Wire.write(Green); Wire.write(Blue); Wire.endTransmission(); Wire.beginTransmission(0x38); Wire.write(0x10); Wire.write(1); // FRONT_RIGHT RGB LED Wire.write(Red); Wire.write(Green); Wire.write(Blue); Wire.endTransmission();

/*
 * @Author: Sorzn
 * @Date: 2019-11-22 13:38:26
 * @LastEditTime: 2019-11-22 14:28:56
 * @Description: M5Stack Hat BUGC Lib
 * @FilePath: /M5StickC/examples/Hat/BUGC/bugC.cpp
 */

#include "bugC.h"

void BUGC::Init() {
    Wire.begin(SDA, SCL);
//  Wire.begin(0, 26);  // M5Stick-C

}

void BUGC::Write1Byte(uint8_t address, uint8_t Register_address, uint8_t data) {
    Wire.beginTransmission(address);
    Wire.write(Register_address);
    Wire.write(data);
    Wire.endTransmission();
}

void BUGC::Write2Byte(uint8_t address, uint8_t Register_address,
                      uint16_t data) {
    Wire.beginTransmission(address);
    Wire.write(Register_address);
    Wire.write(data >> 8);    // MSB
    Wire.write(data & 0xFF);  // LSB

    Wire.endTransmission();
}

void BUGC::WriteBytes(uint8_t address, uint8_t Register_address, uint8_t *data,
                      size_t size) {
    Wire.beginTransmission(address);
    Wire.write(Register_address);
    for (int i = 0; i < size; i++) {
        Wire.write(*(data + i));
    }
    Wire.endTransmission();
}

uint8_t BUGC::ReadBytes(uint8_t address, uint8_t subAddress, uint8_t count,
                        uint8_t *dest) {
    Wire.beginTransmission(address);  // Initialize the Tx buffer
    Wire.write(subAddress);           // Put slave register address in Tx buffer
    uint8_t i = 0;
    if (Wire.endTransmission(false) == 0 &&
        Wire.requestFrom(address, (uint8_t)count)) {
        while (Wire.available()) {
            dest[i++] = Wire.read();
        }
        return true;
    }
    return false;
}

//*******************************************************//

// if speed > 0; clockwise rotation
void BUGC::BugCSetSpeed(uint8_t pos, int8_t speed) {
    speed = (speed > 100) ? 100 : speed;
    speed = (speed < -100) ? -100 : speed;
    Write1Byte(BUGC_ADDR, pos, speed);
}

void BUGC::BugCSetAllSpeed(int8_t speed_0, int8_t speed_1, int8_t speed_2,
                           int8_t speed_3) {
    int8_t speed_out[4] = {speed_0, speed_1, speed_2, speed_3};
    for (uint8_t i = 0; i < 4; i++) {
        speed_out[i] = (speed_out[i] > 100) ? 100 : speed_out[i];
        speed_out[i] = (speed_out[i] < -100) ? -100 : speed_out[i];
    }
    WriteBytes(BUGC_ADDR, 0x00, (uint8_t *)speed_out, 4);
}

void BUGC::BugCSetColor(uint32_t color_left, uint32_t color_right) {
    uint8_t color_out[4];
    color_out[0] = 0;
    color_out[1] = (color_left & 0xff0000) >> 16;
    color_out[2] = (color_left & 0x00ff00) >> 8;
    color_out[3] = (color_left & 0x0000ff);
    WriteBytes(BUGC_ADDR, 0x10, color_out, 4);

    color_out[0] = 1;
    color_out[1] = (color_right & 0xff0000) >> 16;
    color_out[2] = (color_right & 0x00ff00) >> 8;
    color_out[3] = (color_right & 0x0000ff);
    WriteBytes(BUGC_ADDR, 0x10, color_out, 4);
}

/*
 * @Author: Sorzn
 * @Date: 2019-11-22 13:38:31
 * @LastEditTime: 2019-11-22 14:33:56
 * @Description: M5Stack Hat BUGC Lib
 * @FilePath: /M5StickC/examples/Hat/BUGC/bugC.h
 */

#ifndef __BUG_C_H
#define __BUG_C_H

#include "Arduino.h"
#include "Wire.h"

#define BUGC_ADDR 0x38

#define FRONT_LEFT  0
#define FRONT_RIGHT 1
#define REAR_LEFT   2
#define REAR_RIGHT  3

class BUGC {
   public:
    void Init();
    /**
     * @description: Set BugC single motor speed
     * @param pos: 0 ~ 3
     * @param speed: -100 ~ 100
     * @return:
     */
    void BugCSetSpeed(uint8_t pos, int8_t speed);

    /**
     * @description: Set BugC motor speed
     * @param speed_0: -100 ~ 100
     * @param speed_1: -100 ~ 100
     * @param speed_2: -100 ~ 100
     * @param speed_3: -100 ~ 100
     * @return:
     */
    void BugCSetAllSpeed(int8_t speed_0, int8_t speed_1, int8_t speed_2,
                         int8_t speed_3);

    /**
     * @description: Set BugC sk6812 color
     * @param color_left: (r << 16) | (g << 8) | b
     * @param color_right: (r << 16) | (g << 8) | b
     * @return:
     */
    void BugCSetColor(uint32_t color_left, uint32_t color_right);

   private:
    void Write1Byte(uint8_t address, uint8_t Register_address, uint8_t data);
    void Write2Byte(uint8_t address, uint8_t Register_address, uint16_t data);
    void WriteBytes(uint8_t address, uint8_t Register_address, uint8_t* data,
                    size_t size);
    uint8_t ReadBytes(uint8_t address, uint8_t subAddress, uint8_t count,
                      uint8_t* dest);
};

#endif

#include "bugC.h" BUGC BugC; #define BUGC_ADDR 0x38 #define BUGC_LED_REG 0x10 #define BUGC_LED_LEFT 0x00 #define BUGC_LED_RIGHT 0x01 #define WAIT 20 void color(int velocity) { uint8_t R, G, B; velocity = velocity % 255; if (velocity < 85) { R = 255 - velocity * 3; G = 0; B = velocity * 3; } else if (velocity < 170 ) { velocity -= 85; R = 0; G = velocity * 3; B = 255 - velocity * 3; } else { velocity -= 170; R = velocity * 3; G = 255 - velocity * 3; B = 0; } Wire.beginTransmission(BUGC_ADDR); Wire.write(BUGC_LED_REG); Wire.write(BUGC_LED_LEFT); Wire.write(R); Wire.write(G); Wire.write(B); Wire.endTransmission(); Wire.beginTransmission(BUGC_ADDR); Wire.write(BUGC_LED_REG); Wire.write(BUGC_LED_RIGHT); Wire.write(R); Wire.write(G); Wire.write(B); Wire.endTransmission(); delay(WAIT); } void shou_led() { int i = 0; while(1) { color(i); if ((++i) == 255) i = 0; } } void stop() { BugC.BugCSetColor(0x100000, 0x100000); // RED BugC.BugCSetAllSpeed(0, 0, 0, 0); delay(1000); } void forward(float speed) { int8_t leftF = 100 * speed; int8_t rightF = -60 * speed; int8_t leftR = 100 * speed; int8_t rightR = -60 * speed; BugC.BugCSetColor(0x000010, 0x000010); // BLUE BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR); delay(1000); } void back(float speed) { int8_t leftF = -90 * speed; int8_t rightF = 80 * speed; int8_t leftR = -90 * speed; int8_t rightR = 80 * speed; BugC.BugCSetColor(0x101000, 0x101000); // YELLOW BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR); delay(1000); } void forward_left(float speed) { int8_t leftF = 0 * speed; int8_t rightF = -100 * speed; int8_t leftR = 0 * speed; int8_t rightR = -50 * speed; BugC.BugCSetColor(0x001000, 0x000000); // GREEN BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR); delay(1000); } void forward_right(float speed) { int8_t leftF = 100 * speed; int8_t rightF = 0 * speed; int8_t leftR = 50 * speed; int8_t rightR = 0 * speed; BugC.BugCSetColor(0x000000, 0x001000); // GREEN BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR); delay(1000); } void back_left(float speed) { int8_t leftF = 0 * speed; int8_t rightF = 50 * speed; int8_t leftR = 0 * speed; int8_t rightR = 100 * speed; BugC.BugCSetColor(0x001000, 0x000000); // GREEN BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR); delay(1000); } void back_right(float speed) { int8_t leftF = -50 * speed; int8_t rightF = 0 * speed; int8_t leftR = -100 * speed; int8_t rightR = 0 * speed; BugC.BugCSetColor(0x000000, 0x001000); // GREEN BugC.BugCSetAllSpeed(leftF, rightF, leftR, rightR); delay(1000); } void rotate_left(float speed) { int8_t all = -30 * speed; BugC.BugCSetColor(0x001000, 0x000000); // GREEN BugC.BugCSetAllSpeed(all, all, all, all); delay(1000); } void rotate_right(float speed) { int8_t all = 30 * speed; BugC.BugCSetColor(0x000000, 0x001000); // GREEN BugC.BugCSetAllSpeed(all, all, all, all); delay(1000); } void setup() { delay(10000); BugC.Init(); stop(); forward(0.8); back(0.8); rotate_left(0.8); rotate_right(0.8); forward_left(0.8); forward_right(0.8); back_left(0.8); back_right(0.8); stop(); while (1); } void loop() {}