#include <Servo.h>
Servo myServo ;
int angle ;
String message; //string that stores the incoming message
int minimumRange = 35; // Minimum range needed
long duration, distance; // Duration used to calculate distance
const int switchPinOn = 2 ;
const int switchPinOff = 4 ;
const int motorPin = 11 ;
const int greenLedPin = 6 ;
const int hIn1Pin = 10 ;
const int hIn2Pin = 12 ;
const int echoPin = 7 ;
const int trigPin = 8 ;
String msg1, msg2 ;
float mov, steer ;
int switchState = 0 ;
int prog = 0 ;
int randomTurn ;
void setup() {
myServo.attach(9);
Serial.begin(9600);
pinMode(hIn1Pin, OUTPUT) ;
pinMode(hIn2Pin, OUTPUT) ;
digitalWrite(hIn1Pin, LOW) ;
digitalWrite(hIn2Pin, LOW) ;
pinMode(motorPin, OUTPUT);
pinMode(switchPinOn, INPUT) ;
pinMode(switchPinOff, INPUT);
pinMode(greenLedPin, OUTPUT);
analogWrite(motorPin, 0);
angle = 75 ;
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
void loop() {
if (digitalRead(switchPinOff) == HIGH) {
switchState = LOW ;
}
if (switchState == LOW) {
analogWrite(motorPin, 0);
digitalWrite(hIn2Pin, LOW) ;
digitalWrite(hIn1Pin, LOW) ;
if (digitalRead(switchPinOn) == HIGH) {
switchState = HIGH ;
}
}
else {
analogWrite(motorPin, 255);
if (digitalRead(switchPinOff) == HIGH) {
switchState = LOW ;
}
/* measure() ;
if (distance <= 25) {
digitalWrite(hIn2Pin, LOW) ;
digitalWrite(hIn1Pin, HIGH) ;
angle = 75 ;
delay(300);
digitalWrite(hIn1Pin, LOW) ;
} */
while (Serial.available())
{ //while there is data available on the serial monitor
message += char(Serial.read()); //store string from serial command
}
if (!Serial.available())
{
if (message != "")
{ //if data is available
Serial.println( message); //show the data
msg1 = message.substring(0, 1);
msg2 = message.substring(1);
mov = 5 - msg2.toInt() ;
if (msg1.toInt() < 5) {
steer = 20 + msg1.toInt() * 11 ;
}
else {
steer = 75 + (msg1.toInt() - 5) * 19 ;
}
Serial.print( mov);
Serial.print( " : ");
Serial.print(steer);
Serial.println("******");
message = ""; //clear the data
}
}
myServo.write(steer);
if (mov >= -1 && mov <= 1) {
digitalWrite(hIn2Pin, LOW) ;
digitalWrite(hIn1Pin, LOW) ;
}
else if (mov < -1) {
digitalWrite(hIn2Pin, LOW) ;
digitalWrite(hIn1Pin, HIGH) ;
analogWrite(motorPin, 155 - 4 * mov); // mov is negative
}
else {
digitalWrite(hIn2Pin, HIGH) ;
digitalWrite(hIn1Pin, LOW) ;
analogWrite(motorPin, 155 + 4 * mov);
}
digitalWrite(greenLedPin, HIGH);
}
delay(40);
}
void checkOff() {
if (digitalRead(switchPinOff) == HIGH) {
switchState = LOW ;
prog = 0;
}
}
void measure() {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(5);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration / 58.2;
}