//INTEL Intro To Robotics Demo #define PIN_MOTOR1IN1 4 #define PIN_MOTOR1IN2 5 #define PIN_MOTOR1PWM 6 #define PIN_MOTOR2IN1 7 #define PIN_MOTOR2IN2 8 #define PIN_MOTOR2PWM 9 #define PIN_SENSOR_FRONT_L 0 // front left #define PIN_SENSOR_FRONT_R 1 #define PIN_SENSOR_REAR_L 2 // rear left #define PIN_SENSOR_REAR_R 3 #define STATE_TURNING_L 0 #define STATE_TURNING_R 1 #define STATE_FORWARD 2 #define STATE_REVERSE 3 #define STATE_IDLE 4 boolean sensorFrontLActive = false; boolean sensorFrontRActive = false; boolean sensorRearLActive = false; boolean sensorRearRActive = false; int driveTime = 4000; // milliseconds unsigned long curTime = 0; unsigned long lastTime = 0; int currentState = STATE_FORWARD; int lastState = STATE_IDLE; byte motorSpeed = 64; void setup(){ pinMode(PIN_MOTOR1IN1, OUTPUT); pinMode(PIN_MOTOR1IN2, OUTPUT); pinMode(PIN_MOTOR2IN1, OUTPUT); pinMode(PIN_MOTOR2IN2, OUTPUT); pinMode(PIN_SENSOR_FRONT_L, INPUT); pinMode(PIN_SENSOR_FRONT_R, INPUT); pinMode(PIN_SENSOR_REAR_L, INPUT); pinMode(PIN_SENSOR_REAR_R, INPUT); } // END SETUP ROUTINE void loop(){ getSensorInput(); // record the raw status of the IR detectors currentState = processSensorData(); // decide which state to enter /*curTime = millis(); // tiny test routine to run through the states if (curTime - lastTime > driveTime){ lastTime = curTime; currentState++; if (currentState > 4){ currentState = 0; } }*/ if (currentState != lastState){ // only update the driver if something has changed lastState = currentState; switch (currentState){ case STATE_FORWARD: moveForward(); break; case STATE_REVERSE: moveReverse(); break; case STATE_IDLE: idle(); break; case STATE_TURNING_L: moveLeft(); break; case STATE_TURNING_R: moveRight(); break; default: // idle if processSensorData() made a mistake idle(); break; } // END SWITCHCASE } // END IF currentState } // END MAIN LOOP void getSensorInput(){ sensorFrontLActive = !digitalRead(PIN_SENSOR_FRONT_L); // save the logical NOT state of the digitalRead() because the sensors are active LOW sensorFrontRActive = !digitalRead(PIN_SENSOR_FRONT_R); sensorRearLActive = !digitalRead(PIN_SENSOR_REAR_L); sensorRearRActive = !digitalRead(PIN_SENSOR_REAR_R); } // END FUNCTION getSensorInput() int processSensorData(){ //If there is no input, just move forward if (!sensorFrontLActive && !sensorFrontRActive && !sensorRearLActive && !sensorRearRActive){ return STATE_FORWARD; } // If something is in front of the robot, reverse if (sensorFrontLActive && sensorFrontRActive){ return STATE_REVERSE; } //If something is on the right while moving forward, turn left if (currentState == STATE_FORWARD && sensorFrontRActive){ return STATE_TURNING_L; } //If something is on the left while moving forward, turn right if (currentState == STATE_FORWARD && sensorFrontLActive){ return STATE_TURNING_R; } //If something is on the right while moving in reverse, turn right if (currentState == STATE_REVERSE && sensorFrontLActive){ return STATE_TURNING_R; } //If something is only on the left while moving in reverse, turn left if (currentState == STATE_REVERSE && sensorFrontLActive){ return STATE_TURNING_L; } //If all sensors are active, idle in place if (sensorFrontLActive && sensorFrontRActive && sensorRearLActive && sensorRearRActive){ return STATE_IDLE; } //(not implemented) Run the new action if the previous one isn't complete //(not implemented) If something is in front of the robot while reversing, speed up! //(not implemented) If something is detected behind the robot, speed up! } // END FUNCTION processSensorData() void moveForward(){ // left = forward, right = reverse digitalWrite(PIN_MOTOR1IN1, HIGH); digitalWrite(PIN_MOTOR1IN2, LOW); analogWrite(PIN_MOTOR1PWM, motorSpeed); digitalWrite(PIN_MOTOR2IN1, HIGH); digitalWrite(PIN_MOTOR2IN2, LOW); analogWrite(PIN_MOTOR2PWM, motorSpeed); } void moveReverse(){ // left = reverse, right = forward digitalWrite(PIN_MOTOR1IN1, LOW); digitalWrite(PIN_MOTOR1IN2, HIGH); analogWrite(PIN_MOTOR1PWM, motorSpeed); digitalWrite(PIN_MOTOR2IN1, LOW); digitalWrite(PIN_MOTOR2IN2, HIGH); analogWrite(PIN_MOTOR2PWM, motorSpeed); } void moveLeft(){ // left = reverse, right = reverse digitalWrite(PIN_MOTOR1IN1, LOW); digitalWrite(PIN_MOTOR1IN2, HIGH); analogWrite(PIN_MOTOR1PWM, motorSpeed); digitalWrite(PIN_MOTOR2IN1, HIGH); digitalWrite(PIN_MOTOR2IN2, LOW); analogWrite(PIN_MOTOR2PWM, motorSpeed); } void moveRight(){ // left = forward, right = forward digitalWrite(PIN_MOTOR1IN1, HIGH); digitalWrite(PIN_MOTOR1IN2, LOW); analogWrite(PIN_MOTOR1PWM, motorSpeed); digitalWrite(PIN_MOTOR2IN1, LOW); digitalWrite(PIN_MOTOR2IN2, HIGH); analogWrite(PIN_MOTOR2PWM, motorSpeed); } void idle(){ // motor inputs LOW digitalWrite(PIN_MOTOR1IN1, HIGH); digitalWrite(PIN_MOTOR1IN2, LOW); digitalWrite(PIN_MOTOR1PWM, LOW); digitalWrite(PIN_MOTOR2IN1, HIGH); digitalWrite(PIN_MOTOR2IN2, LOW); digitalWrite(PIN_MOTOR2PWM, LOW); }