Hi There! My autonomous vehicle automates! This code is not final, but it does work. Safety (doesn't start moving until told), scanning, comparing, steering, driving, and all that good stuff. A light can be set to blink rate by a potentiometer knob, and another light gets brighter in the dark, like if I wanted to eventually have it take pictures... (the scanning is infra-red, so it works in the dark).
I'll definitely have follow-up to show this thing in action.
for now, Code!
// this sketch is for a custom PCB: i/o must be matched
//=======================LIBRARIES============================//============================================================//============================================================#include <Servo.h>//============================================================
//=======================VARIABLES============================//============================================================//============================================================// unused digital pins: 0, 1.// unused analog pins: A3, A4.// analog pin A5 used in setup for servo control output.
// inputsconst int button1 = 7; // on-board, button closest to digital pinsconst int button2 = 4; // on-board closest to analog pinsconst int button3 = 2; // on-board closest to analog pinsconst int button4 = 5; // on-board closest to analog pinsint buttonRead1 = 111;int buttonRead2 = 111;int buttonRead3 = 111;int buttonRead4 = 111;const int pot1 = A0; // on-board potint potRead1 = 111;const int distanceSensor1 = A1; // Sharp "GP2Y0A21YK0F"int sensorDelay = 100; // millis to allow voltage stabilization after dist readint distanceReads1[19] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 111};const int lightSensor1 = A2;int lightRead1 = 111;
// indicatorsconst int light1 = 10; // LED by USB jack; PWM-equipped pin allows brightness controlconst int light2 = 3; // LED by D0; PWM-equipped pin allows brightness controlbool lightOn1 = false;bool lightOn2 = false;bool lightState1 = LOW; // for blinking light1
// actuators - servo1Servo servo1; // create servo object to control a servoint servoPosition1 = 90; // position of servo motorint scanIncrement = 10; // degrees to change between scan samplesint servoDelay = 200; // millis to allow 10-degree servo travel
// actuators - motor1const int motorEnable1 = 11; // PWM-equipped (speed control)const int motorClockwise1 = 12;const int motorCounterClockwise1 = 13;int motorSpeed1 = 100; // percentage speed for matching 1&2int motorDirection1 = 111;
// actuators - motor2const int motorEnable2 = 9; // PWM-equipped (speed control)const int motorClockwise2 = 6;const int motorCounterClockwise2 = 8;int motorSpeed2 = 100; // percentage speed for matching 1&2int motorDirection2 = 111;
// administrative variablesint distanceScores1[17] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};bool movementOn = false; // turns on physical operations (movement&scanning)bool printCycle = false; // if things print to serial monitorint delayPrint = 1000; // delay to print to serial monitorint delayBlink = 111; // for blinking lightint delayMovement = 700; // interval to driveunsigned long currentMillis = 0;unsigned long previousMillis = 0;unsigned long currentMillisPrint = 0;unsigned long previousMillisPrint = 0;unsigned long currentMillisBlink = 0;unsigned long previousMillisBlink = 0;unsigned long currentMillisMovement = 0;unsigned long previousMillisMovement = 0;//============================================================
//=========================SETUP==============================//============================================================//============================================================void setup() { Serial.begin(9600); // Input pins: pinMode(button1, INPUT_PULLUP); pinMode(button2, INPUT_PULLUP); pinMode(button3, INPUT_PULLUP); pinMode(button4, INPUT_PULLUP); pinMode(pot1, INPUT); pinMode(distanceSensor1, INPUT); pinMode(lightSensor1, INPUT); // Output pins: pinMode(light1, OUTPUT); pinMode(light2, OUTPUT); // H-Bridge motor1 pins: pinMode(motorEnable1, OUTPUT); digitalWrite(motorEnable1, LOW); // turn off just in case pinMode(motorClockwise1, OUTPUT); digitalWrite(motorClockwise1, LOW); // turn off just in case pinMode(motorCounterClockwise1, OUTPUT); digitalWrite(motorCounterClockwise1, LOW); // turn off just in case // H-Bridge motor2 pins: pinMode(motorEnable2, OUTPUT); digitalWrite(motorEnable2, LOW); // turn off just in case pinMode(motorClockwise2, OUTPUT); digitalWrite(motorClockwise2, LOW); // turn off just in case pinMode(motorCounterClockwise2, OUTPUT); digitalWrite(motorCounterClockwise2, LOW); // turn off just in case // servo motor servo1.attach(A5); // attach servo on pin A5 to servo1 object. // conversions motorSpeed1 = map (motorSpeed1, 0, 100, 0, 255); motorSpeed2 = map (motorSpeed2, 0, 100, 0, 255); // set time variables currentMillis = millis(); previousMillis = millis(); currentMillisPrint = millis(); previousMillisPrint = millis(); currentMillisBlink = millis(); previousMillisBlink = millis(); currentMillisMovement = millis(); previousMillisMovement = millis();} // close setup.//============================================================
//=========================LOOP===============================//============================================================//============================================================void loop() { readInputs(); // read button and sensor inputs and writes to serial. safety(); // stop movement if bumper switches engaged. buttons(); // act on the button pushes. lights(); // turn on lights according to buttons() and ambient light (lightRead1)
if (movementOn == true) { scanning(); // sweep the servo and record values, drive in the most open direction for a short period. } // close "if(operating...)"} // close loop.//============================================================
//=======================FUNCTIONS============================//============================================================//============================================================//...................readInputs function......................//============================================================void readInputs() { printCycle = false; // read inputs buttonRead1 = digitalRead(button1); buttonRead2 = digitalRead(button2); buttonRead3 = digitalRead(button3); buttonRead4 = digitalRead(button4); potRead1 = analogRead(pot1); delay(2); // delay for analog read stability distanceReads1[0] = analogRead(distanceSensor1); delay(sensorDelay); // delay for analog read stability lightRead1 = analogRead(lightSensor1); delay(2); // delay for analog read stability // write results currentMillisPrint = millis(); if (currentMillisPrint > (previousMillisPrint + delayPrint)) { printCycle = false; Serial.println(""); Serial.println("new cycle: "); Serial.print("( buttonRead1 = "); Serial.print(buttonRead1); Serial.print(" )( buttonRead2 = "); Serial.print(buttonRead2); Serial.print(" )( buttonRead3 = "); Serial.print(buttonRead3); Serial.print(" )( buttonRead4 = "); Serial.print(buttonRead4); Serial.println(" )"); Serial.print("( potRead1 = "); Serial.print(potRead1); Serial.print(" )( distanceReads1[0] = "); Serial.print(distanceReads1[0]); Serial.print(" )( lightRead1 = "); Serial.print(lightRead1); Serial.print(" )( motorSpeed1 = "); Serial.print(map (motorSpeed1, 0, 255, 0, 100)); Serial.print(" )( motorSpeed2 = "); Serial.print(map (motorSpeed2, 0, 255, 0, 100)); Serial.println(" )"); previousMillisPrint = currentMillisPrint; } // close "if(currentMillisPrint...)" else { printCycle = false; } // close "else"} // close function "readInputs"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//....................safety function.........................//============================================================void safety() { if (buttonRead3 == 0 || buttonRead4 == 0) { motorDirection1 = 0; motorDirection2 = 0; Serial.println("safety bumper activated"); } // close "if(buttonRead3...)"} // close function "safety()"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//....................buttons function........................//============================================================void buttons() { if (buttonRead1 == 0 && buttonRead2 == 0) { lightOn1 = !lightOn1; lightOn2 = !lightOn2; Serial.print("toggle lightOn1 = "); Serial.println(lightOn1); Serial.print("toggle lightOn2 = "); Serial.println(lightOn2); } // close first "if" else { if (buttonRead1 == 0 && buttonRead2 == 1) { movementOn = false; Serial.print("movementOn = "); Serial.println(movementOn); } // close second "if" else { if (buttonRead1 == 1 && buttonRead2 == 0) { movementOn = true; Serial.print("movementOn = "); Serial.println(movementOn); } // close "if (buttonRead1...)" } // close second "else" } // close first "else"} // close function "buttons"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//.....................lights function........................//============================================================void lights() { // light1 if (lightOn1 == true) { currentMillisBlink = millis(); if (currentMillisBlink - previousMillisBlink > potRead1) { previousMillisBlink = currentMillisBlink; // if the LED is off turn it on and vice-versa: if (lightState1 == LOW) { lightState1 = HIGH; } // close "if(ledState...)" else { lightState1 = LOW; } // close "else" // set the LED with the ledState of the variable: digitalWrite(light1, lightState1 ); } // close "if(currentMillisBlink...)" else { digitalWrite(light1, HIGH); } // close "else" } // close "if(lightOn1...)" // light2 if (lightOn2 == true) { analogWrite(light2, (255 - (lightRead1 / 4)) ); // use "lightRead1" to turn on brighter in the dark } // close first "if" else { digitalWrite(light2, LOW); } // close "else"} // close function "lights"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//....................motors function.......................//============================================================void motors() { // motor1 if (motorDirection1 == 1) { if (printCycle = true) { Serial.println("motorDirection1 = 1"); } // close "if" analogWrite(motorEnable1, motorSpeed1); digitalWrite(motorClockwise1, HIGH); digitalWrite(motorCounterClockwise1, LOW); } // close first "if" else { if (motorDirection1 == 2) { if (printCycle = true) { Serial.println("motorDirection1 = 2"); } // close "if" analogWrite(motorEnable1, motorSpeed1); digitalWrite(motorClockwise1, LOW); digitalWrite(motorCounterClockwise1, HIGH); } // close second "if" else { if (motorDirection1 == 0) { if (printCycle = true) { Serial.println("motor1 STOPPING"); } // close "if" analogWrite(motorEnable1, LOW); digitalWrite(motorClockwise1, LOW); digitalWrite(motorCounterClockwise1, LOW); } // close "if (stopButtonState)..." } //close second "else" } //close first "else" // motor2 if (motorDirection2 == 1) { if (printCycle = true) { Serial.println("motorDirection2 = 1"); } // close "if" analogWrite(motorEnable2, motorSpeed2); digitalWrite(motorClockwise2, HIGH); digitalWrite(motorCounterClockwise2, LOW); } // close first "if" else { if (motorDirection2 == 2) { if (printCycle = true) { Serial.println("motorDirection2 = 2"); } // close "if" analogWrite(motorEnable2, motorSpeed2); digitalWrite(motorClockwise2, LOW); digitalWrite(motorCounterClockwise2, HIGH); } // close second "if" else { if (motorDirection2 == 0) { if (printCycle = true) { Serial.println("motor2 STOPPING"); } // close "if" analogWrite(motorEnable2, LOW); digitalWrite(motorClockwise2, LOW); digitalWrite(motorCounterClockwise2, LOW); } // close "if (stopButtonState)..." } //close second "else" } //close first "else"} // close function "movement"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//....................scanning function.......................//============================================================void scanning() { int i = 0; int lowestValue = 4096; // for remembering low (every group should be lower) int lowestValueI = 0; // for remembering which direction was low Serial.println("new scan: "); servoPosition1 = 0; delay(200); servo1.write(servoPosition1); delay( servoDelay * 3 ); // wait for servo to move // this if goes until the end of the function...
// scanning/panning here for (i = 0; i < 19 ; i++) { if (movementOn == true) { servo1.write(servoPosition1); delay(servoDelay); // wait for servo to move delay(sensorDelay); // wait for voltage stabilization before sensor reading distanceReads1[i] = analogRead(distanceSensor1); delay(sensorDelay); // delay for analog read stability Serial.print("( distanceReads1[] @ "); Serial.print(i * 10, DEC); Serial.print(" degrees = "); Serial.print(distanceReads1[i]); Serial.println(" )"); buttonRead1 = digitalRead(button1); buttonRead2 = digitalRead(button2); buttons(); servoPosition1 += scanIncrement; } // close "if(movementOn...)" } // close "for(servoPosition1...)"
// scoring for (i = 0; i < 17; i++) { distanceScores1[i] = ( distanceReads1[i] + distanceReads1[i + 1] + distanceReads1[i + 2] ); } // close "for(i=0...)"
// analyze the scores for (i = 0; i < 17; i++) { int value = distanceScores1[i]; if (lowestValue > value) { lowestValue = value; // record [possible] new lowest value lowestValueI = i; // record [possible] new lowest direction } // close "if(lowestValue...)" } // close "for(i...)"
// report the chosen score Serial.print("lowestValueI (aka Chosen Direction) = "); Serial.println((lowestValueI + 1) * 10);
if (movementOn == true) { servo1.write((lowestValueI + 1 ) * 10 ); delay( servoDelay * 6 ); motorDirection2 = 2; motors(); delay(delayMovement); servo1.write(90); delay(delayMovement); motorDirection2 = 0; motors(); } // close "if(movementOn...)"} // close function "scanning"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//=======================LIBRARIES============================//============================================================//============================================================#include <Servo.h>//============================================================
//=======================VARIABLES============================//============================================================//============================================================// unused digital pins: 0, 1.// unused analog pins: A3, A4.// analog pin A5 used in setup for servo control output.
// inputsconst int button1 = 7; // on-board, button closest to digital pinsconst int button2 = 4; // on-board closest to analog pinsconst int button3 = 2; // on-board closest to analog pinsconst int button4 = 5; // on-board closest to analog pinsint buttonRead1 = 111;int buttonRead2 = 111;int buttonRead3 = 111;int buttonRead4 = 111;const int pot1 = A0; // on-board potint potRead1 = 111;const int distanceSensor1 = A1; // Sharp "GP2Y0A21YK0F"int sensorDelay = 100; // millis to allow voltage stabilization after dist readint distanceReads1[19] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 111};const int lightSensor1 = A2;int lightRead1 = 111;
// indicatorsconst int light1 = 10; // LED by USB jack; PWM-equipped pin allows brightness controlconst int light2 = 3; // LED by D0; PWM-equipped pin allows brightness controlbool lightOn1 = false;bool lightOn2 = false;bool lightState1 = LOW; // for blinking light1
// actuators - servo1Servo servo1; // create servo object to control a servoint servoPosition1 = 90; // position of servo motorint scanIncrement = 10; // degrees to change between scan samplesint servoDelay = 200; // millis to allow 10-degree servo travel
// actuators - motor1const int motorEnable1 = 11; // PWM-equipped (speed control)const int motorClockwise1 = 12;const int motorCounterClockwise1 = 13;int motorSpeed1 = 100; // percentage speed for matching 1&2int motorDirection1 = 111;
// actuators - motor2const int motorEnable2 = 9; // PWM-equipped (speed control)const int motorClockwise2 = 6;const int motorCounterClockwise2 = 8;int motorSpeed2 = 100; // percentage speed for matching 1&2int motorDirection2 = 111;
// administrative variablesint distanceScores1[17] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};bool movementOn = false; // turns on physical operations (movement&scanning)bool printCycle = false; // if things print to serial monitorint delayPrint = 1000; // delay to print to serial monitorint delayBlink = 111; // for blinking lightint delayMovement = 700; // interval to driveunsigned long currentMillis = 0;unsigned long previousMillis = 0;unsigned long currentMillisPrint = 0;unsigned long previousMillisPrint = 0;unsigned long currentMillisBlink = 0;unsigned long previousMillisBlink = 0;unsigned long currentMillisMovement = 0;unsigned long previousMillisMovement = 0;//============================================================
//=========================SETUP==============================//============================================================//============================================================void setup() { Serial.begin(9600); // Input pins: pinMode(button1, INPUT_PULLUP); pinMode(button2, INPUT_PULLUP); pinMode(button3, INPUT_PULLUP); pinMode(button4, INPUT_PULLUP); pinMode(pot1, INPUT); pinMode(distanceSensor1, INPUT); pinMode(lightSensor1, INPUT); // Output pins: pinMode(light1, OUTPUT); pinMode(light2, OUTPUT); // H-Bridge motor1 pins: pinMode(motorEnable1, OUTPUT); digitalWrite(motorEnable1, LOW); // turn off just in case pinMode(motorClockwise1, OUTPUT); digitalWrite(motorClockwise1, LOW); // turn off just in case pinMode(motorCounterClockwise1, OUTPUT); digitalWrite(motorCounterClockwise1, LOW); // turn off just in case // H-Bridge motor2 pins: pinMode(motorEnable2, OUTPUT); digitalWrite(motorEnable2, LOW); // turn off just in case pinMode(motorClockwise2, OUTPUT); digitalWrite(motorClockwise2, LOW); // turn off just in case pinMode(motorCounterClockwise2, OUTPUT); digitalWrite(motorCounterClockwise2, LOW); // turn off just in case // servo motor servo1.attach(A5); // attach servo on pin A5 to servo1 object. // conversions motorSpeed1 = map (motorSpeed1, 0, 100, 0, 255); motorSpeed2 = map (motorSpeed2, 0, 100, 0, 255); // set time variables currentMillis = millis(); previousMillis = millis(); currentMillisPrint = millis(); previousMillisPrint = millis(); currentMillisBlink = millis(); previousMillisBlink = millis(); currentMillisMovement = millis(); previousMillisMovement = millis();} // close setup.//============================================================
//=========================LOOP===============================//============================================================//============================================================void loop() { readInputs(); // read button and sensor inputs and writes to serial. safety(); // stop movement if bumper switches engaged. buttons(); // act on the button pushes. lights(); // turn on lights according to buttons() and ambient light (lightRead1)
if (movementOn == true) { scanning(); // sweep the servo and record values, drive in the most open direction for a short period. } // close "if(operating...)"} // close loop.//============================================================
//=======================FUNCTIONS============================//============================================================//============================================================//...................readInputs function......................//============================================================void readInputs() { printCycle = false; // read inputs buttonRead1 = digitalRead(button1); buttonRead2 = digitalRead(button2); buttonRead3 = digitalRead(button3); buttonRead4 = digitalRead(button4); potRead1 = analogRead(pot1); delay(2); // delay for analog read stability distanceReads1[0] = analogRead(distanceSensor1); delay(sensorDelay); // delay for analog read stability lightRead1 = analogRead(lightSensor1); delay(2); // delay for analog read stability // write results currentMillisPrint = millis(); if (currentMillisPrint > (previousMillisPrint + delayPrint)) { printCycle = false; Serial.println(""); Serial.println("new cycle: "); Serial.print("( buttonRead1 = "); Serial.print(buttonRead1); Serial.print(" )( buttonRead2 = "); Serial.print(buttonRead2); Serial.print(" )( buttonRead3 = "); Serial.print(buttonRead3); Serial.print(" )( buttonRead4 = "); Serial.print(buttonRead4); Serial.println(" )"); Serial.print("( potRead1 = "); Serial.print(potRead1); Serial.print(" )( distanceReads1[0] = "); Serial.print(distanceReads1[0]); Serial.print(" )( lightRead1 = "); Serial.print(lightRead1); Serial.print(" )( motorSpeed1 = "); Serial.print(map (motorSpeed1, 0, 255, 0, 100)); Serial.print(" )( motorSpeed2 = "); Serial.print(map (motorSpeed2, 0, 255, 0, 100)); Serial.println(" )"); previousMillisPrint = currentMillisPrint; } // close "if(currentMillisPrint...)" else { printCycle = false; } // close "else"} // close function "readInputs"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//....................safety function.........................//============================================================void safety() { if (buttonRead3 == 0 || buttonRead4 == 0) { motorDirection1 = 0; motorDirection2 = 0; Serial.println("safety bumper activated"); } // close "if(buttonRead3...)"} // close function "safety()"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//....................buttons function........................//============================================================void buttons() { if (buttonRead1 == 0 && buttonRead2 == 0) { lightOn1 = !lightOn1; lightOn2 = !lightOn2; Serial.print("toggle lightOn1 = "); Serial.println(lightOn1); Serial.print("toggle lightOn2 = "); Serial.println(lightOn2); } // close first "if" else { if (buttonRead1 == 0 && buttonRead2 == 1) { movementOn = false; Serial.print("movementOn = "); Serial.println(movementOn); } // close second "if" else { if (buttonRead1 == 1 && buttonRead2 == 0) { movementOn = true; Serial.print("movementOn = "); Serial.println(movementOn); } // close "if (buttonRead1...)" } // close second "else" } // close first "else"} // close function "buttons"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//.....................lights function........................//============================================================void lights() { // light1 if (lightOn1 == true) { currentMillisBlink = millis(); if (currentMillisBlink - previousMillisBlink > potRead1) { previousMillisBlink = currentMillisBlink; // if the LED is off turn it on and vice-versa: if (lightState1 == LOW) { lightState1 = HIGH; } // close "if(ledState...)" else { lightState1 = LOW; } // close "else" // set the LED with the ledState of the variable: digitalWrite(light1, lightState1 ); } // close "if(currentMillisBlink...)" else { digitalWrite(light1, HIGH); } // close "else" } // close "if(lightOn1...)" // light2 if (lightOn2 == true) { analogWrite(light2, (255 - (lightRead1 / 4)) ); // use "lightRead1" to turn on brighter in the dark } // close first "if" else { digitalWrite(light2, LOW); } // close "else"} // close function "lights"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//....................motors function.......................//============================================================void motors() { // motor1 if (motorDirection1 == 1) { if (printCycle = true) { Serial.println("motorDirection1 = 1"); } // close "if" analogWrite(motorEnable1, motorSpeed1); digitalWrite(motorClockwise1, HIGH); digitalWrite(motorCounterClockwise1, LOW); } // close first "if" else { if (motorDirection1 == 2) { if (printCycle = true) { Serial.println("motorDirection1 = 2"); } // close "if" analogWrite(motorEnable1, motorSpeed1); digitalWrite(motorClockwise1, LOW); digitalWrite(motorCounterClockwise1, HIGH); } // close second "if" else { if (motorDirection1 == 0) { if (printCycle = true) { Serial.println("motor1 STOPPING"); } // close "if" analogWrite(motorEnable1, LOW); digitalWrite(motorClockwise1, LOW); digitalWrite(motorCounterClockwise1, LOW); } // close "if (stopButtonState)..." } //close second "else" } //close first "else" // motor2 if (motorDirection2 == 1) { if (printCycle = true) { Serial.println("motorDirection2 = 1"); } // close "if" analogWrite(motorEnable2, motorSpeed2); digitalWrite(motorClockwise2, HIGH); digitalWrite(motorCounterClockwise2, LOW); } // close first "if" else { if (motorDirection2 == 2) { if (printCycle = true) { Serial.println("motorDirection2 = 2"); } // close "if" analogWrite(motorEnable2, motorSpeed2); digitalWrite(motorClockwise2, LOW); digitalWrite(motorCounterClockwise2, HIGH); } // close second "if" else { if (motorDirection2 == 0) { if (printCycle = true) { Serial.println("motor2 STOPPING"); } // close "if" analogWrite(motorEnable2, LOW); digitalWrite(motorClockwise2, LOW); digitalWrite(motorCounterClockwise2, LOW); } // close "if (stopButtonState)..." } //close second "else" } //close first "else"} // close function "movement"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//....................scanning function.......................//============================================================void scanning() { int i = 0; int lowestValue = 4096; // for remembering low (every group should be lower) int lowestValueI = 0; // for remembering which direction was low Serial.println("new scan: "); servoPosition1 = 0; delay(200); servo1.write(servoPosition1); delay( servoDelay * 3 ); // wait for servo to move // this if goes until the end of the function...
// scanning/panning here for (i = 0; i < 19 ; i++) { if (movementOn == true) { servo1.write(servoPosition1); delay(servoDelay); // wait for servo to move delay(sensorDelay); // wait for voltage stabilization before sensor reading distanceReads1[i] = analogRead(distanceSensor1); delay(sensorDelay); // delay for analog read stability Serial.print("( distanceReads1[] @ "); Serial.print(i * 10, DEC); Serial.print(" degrees = "); Serial.print(distanceReads1[i]); Serial.println(" )"); buttonRead1 = digitalRead(button1); buttonRead2 = digitalRead(button2); buttons(); servoPosition1 += scanIncrement; } // close "if(movementOn...)" } // close "for(servoPosition1...)"
// scoring for (i = 0; i < 17; i++) { distanceScores1[i] = ( distanceReads1[i] + distanceReads1[i + 1] + distanceReads1[i + 2] ); } // close "for(i=0...)"
// analyze the scores for (i = 0; i < 17; i++) { int value = distanceScores1[i]; if (lowestValue > value) { lowestValue = value; // record [possible] new lowest value lowestValueI = i; // record [possible] new lowest direction } // close "if(lowestValue...)" } // close "for(i...)"
// report the chosen score Serial.print("lowestValueI (aka Chosen Direction) = "); Serial.println((lowestValueI + 1) * 10);
if (movementOn == true) { servo1.write((lowestValueI + 1 ) * 10 ); delay( servoDelay * 6 ); motorDirection2 = 2; motors(); delay(delayMovement); servo1.write(90); delay(delayMovement); motorDirection2 = 0; motors(); } // close "if(movementOn...)"} // close function "scanning"//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
No comments:
Post a Comment