Skip to content

Instantly share code, notes, and snippets.

@nico-martin
Last active March 31, 2022 21:26
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save nico-martin/d3d0636835955ca956c91743df26cda3 to your computer and use it in GitHub Desktop.
Save nico-martin/d3d0636835955ca956c91743df26cda3 to your computer and use it in GitHub Desktop.
The C++ code I'm using on the Arduino Nano RP2040 to control the two drv8871 and the Bluetooth LE server.

Arduino Nano Circuit

Speed Wheels Arduino Nano Circuit For the DRV8871 Motor Driver I'm using the Pins D2, D3, D5 und D6.

BLE Server

Furthermore I'm using the ArduinoBLE package to provide a BLE Server that let's you control the motor.

UUIDS:

  • motorControlService: c10e3e56-fdd3-11eb-9a03-0242ac130003
  • motorCharacteristic: 35a1022c-fdd3-11eb-9a03-0242ac130003

motorCharacteristic

Here's how the motor characterisctic works: It basicly consists of two bytes. The first byte controls the left wheel, the second byte controls the right wheel.
100 means standstill, 101-200 forward (where 200 is the fastest), 100-0 backward (where 0 is the fastest).

Demo Frontend

There is a web interface that provides controls wia WebBluetooth:
Demo: https://speed-wheels.nico.dev/
Code: https://github.com/nico-martin/speed-wheels-ble-ui

#include <ArduinoBLE.h>
#define MOTOR_LEFT_IN1 6
#define MOTOR_LEFT_IN2 7
#define MOTOR_RIGHT_IN1 2
#define MOTOR_RIGHT_IN2 3
unsigned long previousMillis = 0;
BLEService motorControlService("c10e3e56-fdd3-11eb-9a03-0242ac130003");
BLECharacteristic motorCharacteristic(
"35a1022c-fdd3-11eb-9a03-0242ac130003",
BLERead | BLEWrite,
2
);
void setup() {
Serial.println("starting..");
if (!BLE.begin()) {
Serial.println("starting BLE module failed!");
while (1);
}
BLE.setLocalName("SpeedWheels");
BLE.setAdvertisedService(motorControlService);
motorControlService.addCharacteristic(motorCharacteristic);
BLE.addService(motorControlService);
BLE.setEventHandler(BLEConnected, blePeripheralConnectHandler);
BLE.setEventHandler(BLEDisconnected, blePeripheralDisconnectHandler);
motorCharacteristic.setEventHandler(BLEWritten, motorCharacteristicWritten);
motorCharacteristic.writeValue((word) 0x0000);
pinMode(MOTOR_LEFT_IN1, OUTPUT);
pinMode(MOTOR_LEFT_IN2, OUTPUT);
pinMode(MOTOR_RIGHT_IN1, OUTPUT);
pinMode(MOTOR_RIGHT_IN2, OUTPUT);
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, LOW);
BLE.advertise();
Serial.println("'SpeedWheels' is now active..");
}
void loop() {
BLE.poll();
blink();
}
void blink() {
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= (BLE.central() ? 1000 : 100)) {
previousMillis = currentMillis;
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
}
}
void blePeripheralConnectHandler(BLEDevice central) {
Serial.print("Connected event, central: ");
Serial.println(central.address());
}
void blePeripheralDisconnectHandler(BLEDevice central) {
Serial.print("Disconnected event, central: ");
Serial.println(central.address());
}
void motorCharacteristicWritten(BLEDevice central, BLECharacteristic characteristic) {
word data;
motorCharacteristic.readValue(data);
moveLeftWheel((data >> (8*0)) & 0xff);
moveRightWheel((data >> (8*1)) & 0xff);
}
void moveLeftWheel(int speed) {
moveWheel(speed, MOTOR_LEFT_IN1, MOTOR_LEFT_IN2);
}
void moveRightWheel(int speed) {
moveWheel(speed, MOTOR_RIGHT_IN1, MOTOR_RIGHT_IN2);
}
void moveWheel(int speed, int MOTOR_IN1, int MOTOR_IN2) {
int byteSpeed = speedToByteInt(speed);
if(speed == 100) {
analogWrite(MOTOR_IN1, 0);
analogWrite(MOTOR_IN2, 0);
} else if(speed > 100) {
analogWrite(MOTOR_IN1, byteSpeed);
analogWrite(MOTOR_IN2, 0);
} else {
analogWrite(MOTOR_IN1, 0);
analogWrite(MOTOR_IN2, byteSpeed);
}
}
int speedToByteInt(int speed) {
int bidirectionalSpeed;
if(speed >= 100){
bidirectionalSpeed = speed - 100;
} else {
bidirectionalSpeed = (speed - 100) * -1;
}
// in low speeds, the wheels won't move. Thats why I added a min value
int minByteValue = 50;
return ((255 - minByteValue) / 100 * bidirectionalSpeed) + minByteValue;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment