//Sage Pratt Project 3
float turny=0;
int db=6;
int K=2;
float x=0;
float y=0;
float z=0;
float rad=0;
float theta=0;
float abstheta=0;
void setup() {
pinMode(3,OUTPUT);
pinMode(8,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
}
void loop() {
x = analogRead(0);
y = analogRead(1);
z = analogRead(2);
//Finding angle
rad = atan2(((x-327)/64),((y-335)/64));
theta = rad*(360/(2*3.1416))-90;;
abstheta = abs(theta);
digitalWrite(10,HIGH);//STBY (HIGH)
//The following determines direction
if (theta > 0 | theta < -180){
digitalWrite(9,LOW);
digitalWrite(8,HIGH);
}
else{
digitalWrite(9,HIGH);
digitalWrite(8,LOW);
}
if (abstheta<2){
turny=0;
}
else if (abstheta<(db/K)){
turny=db;
}
else if (abstheta>(255/K)){
turny=255;
}
else {
turny=K*abstheta;
}
analogWrite(3,turny);
}

Arduino example

  • 1.
    //Sage Pratt Project3 float turny=0; int db=6; int K=2; float x=0; float y=0; float z=0; float rad=0; float theta=0; float abstheta=0; void setup() { pinMode(3,OUTPUT); pinMode(8,OUTPUT); pinMode(9,OUTPUT); pinMode(10,OUTPUT); } void loop() { x = analogRead(0); y = analogRead(1); z = analogRead(2); //Finding angle rad = atan2(((x-327)/64),((y-335)/64)); theta = rad*(360/(2*3.1416))-90;; abstheta = abs(theta); digitalWrite(10,HIGH);//STBY (HIGH) //The following determines direction if (theta > 0 | theta < -180){ digitalWrite(9,LOW); digitalWrite(8,HIGH); } else{ digitalWrite(9,HIGH); digitalWrite(8,LOW); } if (abstheta<2){ turny=0; } else if (abstheta<(db/K)){ turny=db; } else if (abstheta>(255/K)){ turny=255; } else { turny=K*abstheta; } analogWrite(3,turny); }