int motor1a=6; //control pins for Motor1

int motor1b=7;

int motor2a=9; //control pins for Motor2

int motor2b=8;

 

int enableMotor1=10; //PWM Motor 1 Speed pin

int enableMotor2=11;

 

int pingPin = 12;

long duration, cm;

 

//float IR1;

//float IR2;

 

 

 

void forward(int speed) //function to move forward at a defined speed (0-255)

{

digitalWrite(motor1a, HIGH); //set motor 1 direction

digitalWrite(motor1b, LOW);

digitalWrite(motor2a, HIGH); //set motor 2 direction

digitalWrite(motor2b, LOW);

analogWrite(enableMotor1, speed); //set motor 1 speed (0-255)

analogWrite(enableMotor2, speed);

}

 

void reverse(int speed)

{

digitalWrite(motor1a, LOW); //set motor 1 direction

digitalWrite(motor1b, HIGH);

digitalWrite(motor2a, LOW); //set motor 2 direction

digitalWrite(motor2b, HIGH);

analogWrite(enableMotor1, speed);

analogWrite(enableMotor2, speed);

}

 

void left(int speed)

{

digitalWrite(motor1a, HIGH); //set motor 1 direction

digitalWrite(motor1b, HIGH);

digitalWrite(motor2a, HIGH); //set motor 2 direction

digitalWrite(motor2b, LOW);

analogWrite(enableMotor1, speed);

analogWrite(enableMotor2, speed);

}

 

void right(int speed)

{

digitalWrite(motor1a, HIGH); //set motor 1 direction

digitalWrite(motor1b, LOW);

digitalWrite(motor2a, HIGH); //set motor 2 direction

digitalWrite(motor2b, HIGH);

analogWrite(enableMotor1, speed);

analogWrite(enableMotor2, speed);

}

void tankturnleft(int speed)

{

digitalWrite(motor1a, LOW); //set motor 1 direction

digitalWrite(motor1b, HIGH);

digitalWrite(motor2a, HIGH); //set motor 2 direction

digitalWrite(motor2b, LOW);

analogWrite(enableMotor1, speed); //set motor 1 speed (0-255)

analogWrite(enableMotor2, speed);

}

 

void tankturnright(int speed)

{

digitalWrite(motor1a, HIGH); //set motor 1 direction

digitalWrite(motor1b, LOW);

digitalWrite(motor2a, LOW); //set motor 2 direction

digitalWrite(motor2b, HIGH);

analogWrite(enableMotor1, speed); //set motor 1 speed (0-255)

analogWrite(enableMotor2, speed);

}

 

void stop()

{

digitalWrite(motor1a, HIGH); //set motor 1 direction

digitalWrite(motor1b, HIGH);

digitalWrite(motor2a, HIGH); //set motor 2 direction

digitalWrite(motor2b, HIGH);

}

void ping()

{

pinMode(pingPin, OUTPUT);

digitalWrite(pingPin, LOW);

delayMicroseconds(2);

digitalWrite(pingPin, HIGH);

delayMicroseconds(5);

digitalWrite(pingPin, LOW);

 

pinMode(pingPin, INPUT);

duration = pulseIn(pingPin, HIGH);

 

cm = duration / 29 / 2;

}

 

void angleL(int degree)

{

tankturnright(120);

delay(degree*(5217/360));

stop();

delay(1000);

}

 

void angleR(int degree)

{

tankturnleft(120);

delay(degree*(5217/360));

stop();

delay(1000);

}

 

void triangle()

{

forward(120);

delay(1500);

stop();

delay(1500);

angleR(146);

forward(120);

delay(1500);

stop();

delay(1500);

angleR(146);

forward(120);

delay(1500);

}

void square()
{

angleL(125);

forward(120);

delay(750);

stop();

delay(1500);

angleR(105);

forward(120);

delay(750);

stop();

delay(1500);

angleR(115);

stop();

delay(1500);

forward(120);

delay(750);

}

void setup()

{

pinMode(motor1a, OUTPUT);

pinMode(motor1b, OUTPUT);

pinMode(motor2a, OUTPUT);

pinMode(motor2b, OUTPUT);

pinMode(enableMotor1, OUTPUT);

pinMode(enableMotor2, OUTPUT);

//Serial.begin(9600);

//pinMode(A0,INPUT);

//pinMode(A1, INPUT);

// IR1 = analogRead(A0);

}

 

 

 

void loop()

{

angleL(135);

triangle();

square();

stop();

delay(5000);
}

Advertisements