#include <Servo.h> //Servo library
Servo servo_test0; //initialize a servo object for the connected servo
Servo servo_test1;
int ServoNow0=160;
int ServoNow1=180;
int BestAngle=180;
int ServoNew;
int OnOff=0;
void setup() {
servo_test0.attach(2); // attach the signal pin of servo to pin9 of arduino
servo_test1.attach(3);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, INPUT);
pinMode(8, OUTPUT);
pinMode(9, OUTPUT);
Serial.begin(9600);
//servo_test0.write(160);
SetServo1(180);
delay(100);
SetServo0(160);
//servo_test1.write(160);
delay(100);
}
void loop() {
// put your main code here, to run repeatedly:
//SetServo0(10);
bailout:
if (digitalRead(6)==0){
Serial.println(digitalRead(6));
digitalWrite(8,1);
digitalWrite(9,0);
if (OnOff==1){
//servo_test0.write(0);
//SetServo0(0);
//delay(200);
//servo_test1.write(180);
SetServo1(180);
delay(2000);
OnOff=0;
}
goto bailout;
}else {
Serial.println(digitalRead(6));
digitalWrite(8,0);
digitalWrite(9,1);
OnOff=1;
int MaxValA=0;
int MaxValB=0;
int MaxAngleA=0;
int MaxAngleB=0;
//bailout:
delay(100);
int sensorValueAA = 1200 - analogRead(A0);
delay(100);
int sensorValueBB = 1200 - analogRead(A1);
//Serial.println(sensorValueAA);
//Serial.println(sensorValueBB);
//Serial.println("----");
if(sensorValueAA<420 and sensorValueBB<420){///////////////////////////////////
digitalWrite(8,1);
digitalWrite(9,0);
if (OnOff==1){
//servo_test0.write(0);
SetServo0(0);
delay(200);
//servo_test1.write(180);
SetServo1(180);
delay(200);
OnOff=0;
}
delay(2000);
goto bailout;
}else{
digitalWrite(8,0);
digitalWrite(9,1);
OnOff=1;
} ///////////////////////////////////////////////////////
for (int i = 0; i <= 180; i ++) {
servo_test0.write(i);
SetServo0(i);
delay(20);
int sensorValueA = 1200 - analogRead(A0);
if (sensorValueA>MaxValA){
MaxValA=sensorValueA;
MaxAngleA=i;
}
//Serial.println(sensorValueA);
int sensorValueB = 1200 - analogRead(A1);
if (sensorValueB>MaxValB){
MaxValB=sensorValueB;
MaxAngleB=i;
}
//MaxValB<MaxValA;
}
delay(500);
//Serial.println(MaxValA);
//Serial.println(MaxValB);
if (MaxValB<MaxValA){
//digitalWrite(4,1);
//servo_test0.write(MaxAngleA);
SetServo0(MaxAngleA);
Serial.println("---- A max");
Serial.println(MaxAngleA);
BestAngle=MaxAngleA -30;
for (int j = 0; j <= 20; j ++) {
digitalWrite(4,1);
delay(200);
digitalWrite(4,0);
delay(200);
}
digitalWrite(4,1);
}else {
//digitalWrite(5,1);
//servo_test0.write(MaxAngleB);
SetServo0(MaxAngleB);
Serial.println("---- B max");
Serial.println(MaxAngleB);
BestAngle=MaxAngleB -10;
for (int j = 0; j <= 20; j ++) {
digitalWrite(5,1);
delay(200);
digitalWrite(5,0);
delay(200);
}
digitalWrite(5,1);
}
SetServo1(BestAngle);
delay(10000);
digitalWrite(4,0);
digitalWrite(5,0);
delay(200);
}
}
void SetServo0(int ServoNew) {
if (ServoNew>ServoNow0){
for (int j=ServoNow0;j<=ServoNew;j++){
servo_test0.write(j);
delay(20);
}
}else{
for (int k=ServoNow0;k>=ServoNew;k--){
servo_test0.write(k);
delay(20);
}
}
ServoNow0=ServoNew;
servo_test0.write(ServoNew);
}
void SetServo1(int ServoNew) {
if (ServoNew>ServoNow1){
for (int j=ServoNow1;j<=ServoNew;j++){
servo_test1.write(j);
delay(50);
}
}else{
for (int k=ServoNow1;k>=ServoNew;k--){
servo_test1.write(k);
delay(50);
}
}
ServoNow1=ServoNew;
servo_test1.write(ServoNew);
}
Comments
Please log in or sign up to comment.