Stap 8: Softwareontwerp
De rover is een obstakel robot die geleidelijk wenden zal tot het vermijden van een nabijgelegen obstakel en bevriezen als geheel niet in staat om te bewegen, waardoor schade moet vermijden. De software voor de robot is een fundamentele staat-machine, wat voor duidelijke en eenvoudige controle zorgt. Na het initialiseren van de GPIO richtingen, de Edison zal continu zin, verwerken en reageren eindeloos. In dit geval is de robot de enige output een verandering in de richting van de motor. De IR-sensoren hebben schoon digitale uitgangen die zijn hoog of laag, waardoor ze te converteren naar eenvoudige gebeurtenissen die gemakkelijk kunnen worden opgeslagen als een Boole-waarde staat. Vanwege de aard van de korte afstand van de sensoren is de robot software alleen reactief op externe gebeurtenissen. Als uitgerust met een IR of ultrasone afstandsmeter, zou de robot kunnen proactief een pad, waardoor voor meer interessante gedrag bepalen. Wanneer u begint, ik stel voor werken met een enkel type van sensor en leren hoe goed kunt u de gegevens uit het verwerken. Zodra u vertrouwd bent met een enkele soort input, langzaam meer sensoren toevoegen en test hen langs de weg. Deze aanpak is veel meer methodisch, maar bespaart u de hoofdpijn van te proberen te integreren van vele verschillende sensoren tegelijkertijd, die kan moeilijk zijn om te zuiveren.
//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; // max is 255 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) Accelerate if no obstacles are detected, default to a slow speed if something is detected! If something is in front of the robot while reversing, speed up! If something is detected behind the robot, speed up! */<br> } // 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); }
