|
|
@ -51,6 +51,9 @@ |
|
|
|
*/ |
|
|
|
#define SERVOPIN 6 |
|
|
|
|
|
|
|
/*Definition for turn time*/ |
|
|
|
#define TURN_TIME 500 |
|
|
|
|
|
|
|
/*
|
|
|
|
* ------------------------------------------------------- |
|
|
|
* Define Servo Data Struct |
|
|
@ -72,6 +75,7 @@ int pos, dir; |
|
|
|
/*
|
|
|
|
* Turn Servo using custom Speed |
|
|
|
*/ |
|
|
|
/*Function that takes as arguments from->first position and deg->next position */ |
|
|
|
void turnServo(int from, int deg){ |
|
|
|
int d; |
|
|
|
if (from > deg){ |
|
|
@ -251,29 +255,64 @@ void moveCar(){ |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void park() { |
|
|
|
turnRight(255); |
|
|
|
forward(); |
|
|
|
delay(TURN_TIME); |
|
|
|
|
|
|
|
motor_stop(); |
|
|
|
forward(); |
|
|
|
|
|
|
|
delay(400); |
|
|
|
} |
|
|
|
String inString; |
|
|
|
void Serial_Input() |
|
|
|
{ |
|
|
|
|
|
|
|
while (Serial.available() > 0) { |
|
|
|
int inChar = Serial.read(); |
|
|
|
if (isDigit(inChar)) { |
|
|
|
// convert the incoming byte to a char and add it to the string:
|
|
|
|
inString += (char)inChar; |
|
|
|
} |
|
|
|
// if you get a newline, print the string, then the string's value:
|
|
|
|
if (inChar == '\n') { |
|
|
|
Serial.print("Value:"); |
|
|
|
Serial.println(inString.toInt()); |
|
|
|
Serial.print("String: "); |
|
|
|
Serial.println(inString); |
|
|
|
// clear the string for new input:
|
|
|
|
inString = ""; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void loop() { |
|
|
|
// Calculating the distance
|
|
|
|
distance= getDistance(); |
|
|
|
//distance= getDistance();
|
|
|
|
|
|
|
|
// Prints the distance on the Serial Monitor
|
|
|
|
//Serial.print("Distance: ");
|
|
|
|
//Serial.println(distance);
|
|
|
|
|
|
|
|
if (distance <15) { |
|
|
|
// Change direction to motors
|
|
|
|
turn(225); |
|
|
|
} else if (distance <13){ |
|
|
|
turn(200); |
|
|
|
} else { |
|
|
|
// Change direction to motors
|
|
|
|
turn(MAX_WHEEL_SPEED); |
|
|
|
} |
|
|
|
if (distance < 6){ |
|
|
|
motor_stop(); |
|
|
|
// Set Ultrasonic to next step
|
|
|
|
next_step(); |
|
|
|
// Move car
|
|
|
|
moveCar(); |
|
|
|
} |
|
|
|
delay(100); |
|
|
|
// Read serial input:
|
|
|
|
Serial_Input(); |
|
|
|
//park();
|
|
|
|
//delay(1000);
|
|
|
|
// if (distance <15) {
|
|
|
|
// // Change direction to motors
|
|
|
|
// turn(225);
|
|
|
|
// } else if (distance <13){
|
|
|
|
// turn(200);
|
|
|
|
// } else {
|
|
|
|
// // Change direction to motors
|
|
|
|
// turn(MAX_WHEEL_SPEED);
|
|
|
|
// }
|
|
|
|
// if (distance < 6){
|
|
|
|
// motor_stop();
|
|
|
|
// // Set Ultrasonic to next step
|
|
|
|
// next_step();
|
|
|
|
// // Move car
|
|
|
|
// moveCar();
|
|
|
|
// }
|
|
|
|
// delay(100);
|
|
|
|
} |
|
|
|