For our final we are showing our code from our PComp robot. What we specifically find interesting about the code is the threshold which everything else is based on.
The robot is gyroscopically controlled, so tilting the robot steers it. Our threshold functions as a dead zone and functionally smooths out the the input of the gyroscope to make it less sensitive. This is nessecary because the robot needs to be less sensitive to meaningfully control it. The threshold functions as a keystone for this code, becuase in order for any movement decisions to occur, the threshold needs to be checked for the y and x axis. This manifests for pure straight/reverse, pure turning, and a mix of those.
Sana and I coded this pretty collaboraltively and we arrived on threshold initially as a means of avoiding using delay. I think it made the robot more accurate and added a foundation for a series of checks to make before deciding which motors the robot uses. I have added comments specifically to the threshold values in the motor code below in my own words outside of what Sana noted, as a means of talking about it in more detail in my own voice.
#include <ArduinoBLE.h>
// UUIDs that match what the peripheral is advertising
const char* serviceUUID = "19b10000-e8f2-537e-4f6c-d104768a1214";
const char* characteristicUUID = "19b10000-e8f2-537e-4f6c-d104768a1215";
const char* gyroDataCharacteristicUUID = "19b10000-e8f2-537e-4f6c-d104768a1216";
//mapping pins for the motor
const int AIN1 = 7;//right wheel
const int AIN2 = 6;//right wheel
const int PWMA = 4; // Motor driver PWM pin
const int BIN1 = 8;//left wheel
const int BIN2 = 9;//left wheel
const int PWMB = 10;
// Thresholds
int threshold = 5;//this threshold functions as a "deadzone" for the controller. Adjust to change behavior
// Update rate control (in milliseconds) - this acts functionally like a delay to keep movement smooth and communication consistent
const unsigned long UPDATE_INTERVAL = 200; // Adjust this value to control update frequency
void setup() {
Serial.begin(9600);
//pins for the motor
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
//initialize bluetooth
if (!BLE.begin()) {
Serial.println("* Starting Bluetooth® Low Energy module failed!");
while (1);
}
Serial.println("* Arduino Nano 33 IoT Central device started");
Serial.println("* Scanning for peripheral devices...");
BLE.scan();
}
void loop() {
BLEDevice peripheral = BLE.available();
if (peripheral) {
if (peripheral.advertisedServiceUuid() == serviceUUID) {
Serial.println("* Peripheral device found!");
Serial.println("* Stopping scan...");
BLE.stopScan();
processPeripheral(peripheral);
}
}
}
void processPeripheral(BLEDevice peripheral) {
Serial.println("* Connecting to peripheral...");
if (!peripheral.connect()) {
Serial.println("* Connection failed!");
BLE.scan();
return;
}
Serial.println("* Connected to peripheral!");
if (!peripheral.discoverAttributes()) {
Serial.println("* Attribute discovery failed!");
peripheral.disconnect();
BLE.scan();
return;
}
BLEService service = peripheral.service(serviceUUID);
if (!service) {
Serial.println("* Service not found!");
peripheral.disconnect();
BLE.scan();
return;
}
BLECharacteristic testCharacteristic = service.characteristic(characteristicUUID);
BLECharacteristic gyroDataCharacteristic = service.characteristic(gyroDataCharacteristicUUID);
if (!testCharacteristic || !gyroDataCharacteristic) {
Serial.println("* Characteristics not found!");
peripheral.disconnect();
BLE.scan();
return;
}
if (!testCharacteristic.subscribe() || !gyroDataCharacteristic.subscribe()) {
Serial.println("* Subscription failed!");
peripheral.disconnect();
BLE.scan();
return;
}
static unsigned long lastUpdate = 0; // Time of last processed update
while (peripheral.connected()) {
unsigned long currentTime = millis();
// Only process updates if enough time has elapsed since last update
if (gyroDataCharacteristic.valueUpdated() && (currentTime - lastUpdate) > UPDATE_INTERVAL) {
lastUpdate = currentTime; // Update the timestamp
// Read latest gyro data
char gyroValue[32];
int gyroLength = gyroDataCharacteristic.readValue(gyroValue, sizeof(gyroValue));
gyroValue[gyroLength] = '\\0';
String dataString = String(gyroValue);
int firstComma = dataString.indexOf(',');
int secondComma = dataString.indexOf(',', firstComma + 1);
float gyroX = dataString.substring(0, firstComma).toFloat();
float gyroY = dataString.substring(firstComma + 1, secondComma).toFloat();
// Direct motor control based on gyro values, two different conditions must be met for pure turning:
if (abs(gyroY) <= threshold) { // Y-axis must be within 0-to threshold, so fairly neutral position. "abs" is making sure the absolute y-value is used when checking in these instances so it can check if the y-axis is meaningfully engaged at all regardless of positive or negative direction, before exectuting pure turn
if (gyroX < -threshold) { // if x axis is smaller than negative threshold, itll to a pure left turn
int turnSpeed = map(abs(gyroX), threshold, 100, 40, 255);
// Right wheel forward, left wheel backward
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
analogWrite(PWMA, turnSpeed);
analogWrite(PWMB, turnSpeed);
}
else if (gyroX > threshold) { // if x axis is larger than threshold, itll do a pure right turn
int turnSpeed = map(abs(gyroX), threshold, 100, 40, 255);
// Left wheel forward, right wheel backward
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
analogWrite(PWMA, turnSpeed);
analogWrite(PWMB, turnSpeed);
}
else { // No turn
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
}
}
else if (gyroY < -threshold) { // Forward motion
int forwardSpeed = map(abs(gyroY), threshold, 100, 40, 255);
// Both wheels forward
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
if (abs(gyroX) > threshold) { // Forward + turn, if in forward motion gyro x is also engaged and the abs is more than threshold, the checks and mapping occur below
float turnFactor = map(abs(gyroX), threshold, 100, 0, 80) / 100.0; // Max 80% speed reduction, turnFactor reduces the speed of one of the wheels 80% resulting in a slight right or left
if (gyroX < 0) { // Left turn, same threshold logic as above
analogWrite(PWMA, forwardSpeed); // Right wheel full speed
analogWrite(PWMB, forwardSpeed * (1.0 - turnFactor)); // Left wheel reduced
}
else { // Right turn
analogWrite(PWMA, forwardSpeed * (1.0 - turnFactor)); // Right wheel reduced
analogWrite(PWMB, forwardSpeed); // Left wheel full speed
}
}
else { // Straight forward
analogWrite(PWMA, forwardSpeed);
analogWrite(PWMB, forwardSpeed);
}
}
else if (gyroY > threshold) { // Reverse motion
int reverseSpeed = map(abs(gyroY), threshold, 100, 40, 255);
// Both wheels reverse
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
if (abs(gyroX) > threshold) { // Reverse + turn
float turnFactor = map(abs(gyroX), threshold, 100, 0, 80) / 100.0; // Max 80% speed reduction
if (gyroX < 0) { // Left turn while reversing
analogWrite(PWMA, reverseSpeed); // Right wheel full speed
analogWrite(PWMB, reverseSpeed * (1.0 - turnFactor)); // Left wheel reduced
}
else { // Right turn while reversing
analogWrite(PWMA, reverseSpeed * (1.0 - turnFactor)); // Right wheel reduced
analogWrite(PWMB, reverseSpeed); // Left wheel full speed
}
}
else { // Straight reverse
analogWrite(PWMA, reverseSpeed);
analogWrite(PWMB, reverseSpeed);
}
}
else { // Stop if no significant tilt
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
}
}
}
// After connection is lost
Serial.println("* Peripheral disconnected!");
BLE.scan(); // Restart scanning
}