// Pin assignments #define LEFT_PIN 13 // red/black #define TURN_ENABLE_PIN 12 // white #define DRIVE_ENABLE_PIN 11 // white/black #define RIGHT_PIN 10 // red #define BACKWARD_PIN 9 // yellow/black #define FORWARD_PIN 8 // yellow #define LEFT_FRONT_BUMPER_PIN 7 // white #define RIGHT_FRONT_BUMPER_PIN 6 // yellow #define REAR_BUMPER_PIN 5 // red // Input Commands #define BEGIN_COMMAND 0x7F #define FORWARD 0x1 #define BACKWARD 0x2 #define LEFT 0x4 #define RIGHT 0x8 // Sensor Data #define LEFT_FRONT_BUMPER 0x1 #define RIGHT_FRONT_BUMPER 0x2 #define REAR_BUMPER 0x4 #define DEBOUNCE_THRESHOLD 50 int sensorData = 0; int lastSensorData = 0; int lastLeftFrontBumperReading; int lastRightFrontBumperReading; int lastRearBumperReading; int leftFrontBumperReadingTime; int rightFrontBumperReadingTime; int rearBumperReadingTime; int command[2]; void setup() { Serial.begin(9600); pinMode(DRIVE_ENABLE_PIN, OUTPUT); pinMode(TURN_ENABLE_PIN, OUTPUT); pinMode(LEFT_PIN, OUTPUT); pinMode(RIGHT_PIN, OUTPUT); pinMode(BACKWARD_PIN, OUTPUT); pinMode(FORWARD_PIN, OUTPUT); pinMode(LEFT_FRONT_BUMPER_PIN, INPUT); digitalWrite(LEFT_FRONT_BUMPER_PIN, HIGH); pinMode(RIGHT_FRONT_BUMPER_PIN, INPUT); digitalWrite(RIGHT_FRONT_BUMPER_PIN, HIGH); pinMode(REAR_BUMPER_PIN, INPUT); digitalWrite(REAR_BUMPER_PIN, HIGH); } void loop() { // Read sensors and transmit readSensors(); if (sensorData != lastSensorData) { Serial.write(sensorData); lastSensorData = sensorData; } // listen for wireless commands if (Serial.available() > 0) { if (readCommand() > 0) { executeCommand(); } } } void readSensors() { int s; s = digitalRead(LEFT_FRONT_BUMPER_PIN); if (s != lastLeftFrontBumperReading) { leftFrontBumperReadingTime = millis(); } if ((millis() - leftFrontBumperReadingTime) > DEBOUNCE_THRESHOLD) { if (s == LOW) { sensorData |= LEFT_FRONT_BUMPER; } else { sensorData &= ~LEFT_FRONT_BUMPER; } } lastLeftFrontBumperReading = s; s = digitalRead(RIGHT_FRONT_BUMPER_PIN); if (s != lastRightFrontBumperReading) { rightFrontBumperReadingTime = millis(); } if ((millis() - rightFrontBumperReadingTime) > DEBOUNCE_THRESHOLD) { if (s == LOW) { sensorData |= RIGHT_FRONT_BUMPER; } else { sensorData &= ~RIGHT_FRONT_BUMPER; } } lastRightFrontBumperReading = s; s = digitalRead(REAR_BUMPER_PIN); if (s != lastRearBumperReading) { rearBumperReadingTime = millis(); } if ((millis() - rearBumperReadingTime) > DEBOUNCE_THRESHOLD) { if (s == LOW) { sensorData |= REAR_BUMPER; } else { sensorData &= ~REAR_BUMPER; } } lastRearBumperReading = s; } int readCommand() { int b = Serial.read(); if (b == BEGIN_COMMAND) { command[0] = readByte(); command[1] = readByte(); return 1; } else { return 0; } } // blocking read int readByte() { while (true) { if (Serial.available() > 0) { return Serial.read(); } } } void executeCommand() { int c = command[0]; int speed = command[1]; digitalWrite(DRIVE_ENABLE_PIN, LOW); if (c & FORWARD) { digitalWrite(BACKWARD_PIN, LOW); digitalWrite(FORWARD_PIN, HIGH); } if (c & BACKWARD) { digitalWrite(FORWARD_PIN, LOW); digitalWrite(BACKWARD_PIN, HIGH); } if (c & (FORWARD | BACKWARD)) { // digitalWrite(DRIVE_ENABLE_PIN, HIGH); analogWrite(DRIVE_ENABLE_PIN, speed); } digitalWrite(TURN_ENABLE_PIN, LOW); if (c & LEFT) { digitalWrite(RIGHT_PIN, LOW); digitalWrite(LEFT_PIN, HIGH); } if (c & RIGHT) { digitalWrite(LEFT_PIN, LOW); digitalWrite(RIGHT_PIN, HIGH); } if (c & (LEFT | RIGHT)) { digitalWrite(TURN_ENABLE_PIN, HIGH); } }