Position Control using
Servomechanism
int
gnd=5,vc=4;
int
Mot2=8;
int
Mot1=9;
int
Enb=10;
int
Inp,CurrPos,x,y;
int Err=0;
int
Pot1=A0;
int
Pot2=A1;
void
setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(vc,OUTPUT);
pinMode(gnd,OUTPUT);
pinMode(Mot1,OUTPUT);
pinMode(Mot2,OUTPUT);
pinMode(Enb,OUTPUT);
digitalWrite(vc,HIGH);
digitalWrite(gnd,LOW);
digitalWrite(Enb,HIGH);
}
void
loop(){
readInp();
disInp();
takeAct();
reverse();
}
void
readInp(){
x=analogRead(A0);
y=analogRead(A1);
Inp=map(x,0,1023,0,330);
CurrPos=map(y,0,1023,0,330);
Err=Inp-CurrPos;
}
void disInp(){
Serial.print("\nFeedback
Angle:\t");
Serial.print(CurrPos);
Serial.print("\nInput Angle:\t");
Serial.print(Inp);
Serial.print("\nError:\t");
Serial.print(Err);
}
void takeAct(){
if(Err>11)
forward();
if(Err<-11)
reverse();
if(Err>=-10&&Err<=10)
stop();
}
void forward(){
digitalWrite(Mot1,HIGH);
digitalWrite(Mot2,LOW);
delay(1000);
}
void reverse(){
digitalWrite(Mot1,LOW);
digitalWrite(Mot2,HIGH);
delay(1000);
}
void stop(){
digitalWrite(Mot1,LOW);
digitalWrite(Mot2,LOW);
delay(500.0);
}