#include <AFMotor.h>
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
void setup()
{
Serial.begin(9600);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
}
void loop(){
int Right = digitalRead(A0);
int Left = digitalRead(A1);
Serial.print("RIGHT");Serial.println(Right);
Serial.print("LEFT");Serial.println(Left);
if((Right==0) && (Left==0)){forward();} //00 uragshaa
else if((Right==0) && (Left==1)){left();} //01 zuun
else if((Right==1) && (Left==0)){right();} //10 baruun
else if((Right==1) && (Left==1)){forward();} //11 uragshaa
}
void forward()
{
motor1.setSpeed(255);
motor1.run(FORWARD);
motor2.setSpeed(255);
motor2.run(FORWARD);
}
void back()
{
motor1.setSpeed(255);
motor1.run(BACKWARD);
motor2.setSpeed(255);
motor2.run(BACKWARD);
}
void left()
{
motor1.setSpeed(255);
motor1.run(BACKWARD);
motor2.setSpeed(255);
motor2.run(FORWARD);
}
void right()
{
motor1.setSpeed(255);
motor1.run(FORWARD);
motor2.setSpeed(255);
motor2.run(BACKWARD);
}
void Stop()
{
motor1.setSpeed(0);
motor1.run(RELEASE);
motor2.setSpeed(0);
motor2.run(RELEASE);
}
No comments:
Post a Comment