// Turn the Grabber in to an specific angle between -110 to 110 degree:
// Status: Untested
void grabBasePosition(int angle) {
if(angle > NUM_POTENTIOMETER_2_MAX_ANGLE || angle < NUM_POTENTIOMETER_2_MIN_ANGLE){ //Check if the input angle is posible
return;
}
int diff =angle - checkGrabAngle(); //calculate the difference
if(diff<5&&diff>-5){
Serial.println("Grab is not going to turn.");
Serial.print("Diff:");Serial.println(diff);
}
else if(diff<0){
m2.backward(200); //Here is Backward!!!!
}
else if(diff>0){
m2.forward(200);
}
while(diff>5||diff<-5){
diff =angle - checkGrabAngle();
}
m2.brake();
}
// Turn Secondary arm in to an specific angle between -110 to 110 degree:
// Status: Untested
void secondaryArmPosition(int angle){
if(angle > NUM_POTENTIOMETER_1_MAX_ANGLE || angle < NUM_POTENTIOMETER_1_MIN_ANGLE){ //Check if the input angle is posible
return;
}
int diff =angle - checkSecondaryArmAngle(); //calculate the difference
if(diff<5&&diff>-5){
Serial.println("Secondary Arm is not going to turn.");
Serial.print("Diff:");Serial.println(diff);
}
else if(diff>0){
m1.forward(255);
}
else if(diff<0){
m1.backward(255);
}
while(diff>5||diff<-5){
diff =angle - checkSecondaryArmAngle();
}
m1.brake();
}
