#include

$//$ Left Side Wheels

const int motor$1$pin$1=2$;

const int motor$1$pin$2=3$;

$//$ RightSide Wheels

const int motor$2$pin$1=4$;

const int motor$2$pin$2=5$;

$//$ SuperSonic Sensor

const int trigPin $=6$;

const int echoPin $=7$;

const char$*$ deviceServiceUuid $="12345678-1234-5678-1234-56789$abcdef$0"$;

const char$*$ deviceServiceCharacteristicUuid $="12345678-1234-5678-1234-56789$abcdef$3"$;

int brake $=0$;

int$16\_$t previousCondition $=0$;

long duration $=0$;

int distance $=0$;

void setup$()\{$

$//$ put your setup code here, to run once:

Serial.begin$(9600)$;

while $(!$Serial$)$;

if $(!$BLE.begin$())$

$\{$

Serial.println$("$Failed to initialize BLE!"$)$;

while $(1)$;

$\}$

$//$ TODO Setup gyro pins to INPUT

$//$ Setting motor pins for output

Serial.println$("$Initializing motor pins"$)$;

pinMode$($motor$1$pin$1,$ OUTPUT$)$;

pinMode$($motor$1$pin$2,$ OUTPUT$)$;

pinMode$($motor$2$pin$1,$ OUTPUT$)$;

pinMode$($motor$2$pin$2,$ OUTPUT$)$;

$//$ Setting SSS pins for OUTPUT$/$INPUT

pinMode$($trigPin$,$ OUTPUT$)$;

pinMode$($echoPin$,$ INPUT$)$;

$\}$

void loop$()\{$

Serial.println$("$UNO Wheelchair is scanning..."$)$;

BLEDevice peripheral;

BLE.scanForUuid$($deviceServiceUuid$)$;

while$(!($peripheral $=$ BLE.available$()))$

$\{$

$//$ Uncomment if needed for debugging

Serial.println$("$Scanning$...")$;

delay$(9000)$;

$\}$

BLE.stopScan$()$;

Serial.print$("$Peripheral found: $")$;

Serial.println$($peripheral$.$localName$())$;

if $(!$peripheral.connect$())$

$\{$

Serial.println$("$Failed to connect to Peripheral..."$)$;

return; $//$ IF THIS CAUSES PROBLEMS, USE while$(1)$; instead

$\}$

Serial.println$("$Connected to Peripheral!"$)$;

if $(!$peripheral.discoverAttributes$())$

$\{$

Serial.println$("$Failed to discover Attributes..."$)$;

peripheral.disconnect$()$;

return;

$\}$

Serial.println$("$Attributes Discovered!"$)$;

BLECharacteristic conditionCharacteristic $=$ peripheral.characteristic$($deviceServiceCharacteristicUuid$)$;

if $($conditionCharacteristic && $($conditionCharacteristic$.$properties$()$ & BLENotify$))$

$\{$

Serial.println$("$Subscribing to Condition Characteristic..."$)$;

conditionCharacteristic.subscribe$()$;

$\}$

else

$\{$

Serial.println$("$Condition Characteristic not found or not notifiable."$)$;

return;

$\}$

$//$ Prints the distance on the Serial Monitor

Serial.print$("$Distance: $")$;

Serial.println$($distance$)$;

$//$ Clears the trigPin

digitalWrite$($trigPin$,$ LOW$)$;

delayMicroseconds$(2)$;

$//$ Sets the trigPin on HIGH state for $10$ micro seconds

digitalWrite$($trigPin$,$ HIGH$)$;

delayMicroseconds$(10)$;

digitalWrite$($trigPin$,$ LOW$)$;

$//$ Reads the echoPin, returns the sound wave travel time in microseconds

duration $=$ pulseIn$($echoPin$,$ HIGH$)$;

$//$ Calculating the distance

distance $=$ duration $*0\mathrm{.}034/2$;

while $($peripheral$.$connected$())$

$\{$

if $(!$conditionCharacteristic.valueUpdated$())$

$\{$

continue;

$\}$

$//$ Getting value from Bluetooth

int$16\_$t condition $=0$;

if $(!$conditionCharacteristic.readValue$($condition$))$

$\{$

Serial.println$("$Failed to read condition value."$)$;

continue;

$\}$

Serial.print$("$Condition received: $")$;

Serial.println$($condition$)$;

if $($previousCondition $==0)$

$\{$

previousCondition $=$ condition;

$\}$

else if $(($previousCondition $==1||$ previousCondition $==2)$ && condition $!=0)$

$\{$

previousCondition $=0$;

$\}$

else if $(($previousCondition $==3||$ previousCondition $==4)$ && condition $==0)$

$\{$

previousCondition $=0$;

$\}$

if $(!$distance $>=3)$

$\{$

brake $=0$;

$\}$

else if $($distance $>=3)$

$\{$

brake $=1$;

$\}$

$//$ Check what to do with the value of brake

switch $($brake$)$

$\{$

case $0$: $//$ CLOSE

analogWrite$(9,0)$;

analogWrite$(10,0)$;

break;

default:

analogWrite$(9,255)$;

analogWrite$(10,255)$;

break;

$\}$

How can we fix the ultrasonic component so it can brake when there$$s an object