|
#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; |
|
} |