#include <Servo.h>
Servo servo1;
Servo servo2;
Servo servo3;
const int potpin1 = A0;
const int potpin2 = A1;
const int potpin3 = A2;
const int button1 = 2;
const int button2 = 10;
int val1;
int val2;
int val3;
int button1Pressed = 0;
boolean button2Pressed = false;
int pot1Angle;
int pot2Angle;
int pot3Angle;
int servo1PosSave[10];
int servo2PosSave[10];
int servo3PosSave[10];
int saves;
void setup() {
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
Serial.begin(9600);
servo1.attach(7);
servo2.attach(8);
servo3.attach(9);
pinMode(button1, INPUT);
pinMode(button2, INPUT);
saves = -1;
}
void loop() {
digitalWrite(4,HIGH);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
val1 = analogRead(potpin1);
val2 = analogRead(potpin2);
val3 = analogRead(potpin3);
pot1Angle = map (val1, 0, 1023, 0, 179);
pot2Angle = map (val2, 0, 1023, 30, 90);
pot3Angle = map (val3, 0, 1023, 110, 50);
servo1.write(pot1Angle);
servo2.write(pot2Angle);
servo3.write(pot3Angle);
if(digitalRead(button1) == HIGH){
button1Pressed++;
switch(button1Pressed){
case 1:
servo1PosSave[0] = pot1Angle;
servo2PosSave[0] = pot2Angle;
servo3PosSave[0] = pot3Angle;
Serial.println("Position #1 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 0;
break;
case 2:
servo1PosSave[1] = pot1Angle;
servo2PosSave[1] = pot2Angle;
servo3PosSave[1] = pot3Angle;
Serial.println("Position #2 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 1;
break;
case 3:
servo1PosSave[2] = pot1Angle;
servo2PosSave[2] = pot2Angle;
servo3PosSave[2] = pot3Angle;
Serial.println("Position #3 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 2;
break;
case 4:
servo1PosSave[3] = pot1Angle;
servo2PosSave[3] = pot2Angle;
servo3PosSave[3] = pot3Angle;
Serial.println("Position #4 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 3;
break;
case 5:
servo1PosSave[4] = pot1Angle;
servo2PosSave[4] = pot2Angle;
servo3PosSave[4] = pot3Angle;
Serial.println("Position #5 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 4;
break;
case 6:
servo1PosSave[5] = pot1Angle;
servo2PosSave[5] = pot2Angle;
servo3PosSave[5] = pot3Angle;
Serial.println("Position #6 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 5;
break;
case 7:
servo1PosSave[6] = pot1Angle;
servo2PosSave[6] = pot2Angle;
servo3PosSave[6] = pot3Angle;
Serial.println("Position #7 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 6;
break;
case 8:
servo1PosSave[7] = pot1Angle;
servo2PosSave[7] = pot2Angle;
servo3PosSave[7] = pot3Angle;
Serial.println("Position #8 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 7;
break;
case 9:
servo1PosSave[8] = pot1Angle;
servo2PosSave[8] = pot2Angle;
servo3PosSave[8] = pot3Angle;
Serial.println("Position #9 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 8;
break;
case 10:
servo1PosSave[9] = pot1Angle;
servo2PosSave[9] = pot2Angle;
servo3PosSave[9] = pot3Angle;
Serial.println("Position #10 Saved");
{digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);}
delay(500);
saves = 9;
break;
default:
for(int i=0;i<4; i++){
digitalWrite(4,HIGH);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
delay(200);
digitalWrite(4,LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
delay(200);
}
}}
if(digitalRead(button2) == HIGH){
button2Pressed = true;
}
if(button2Pressed){
digitalWrite(4,LOW);
digitalWrite(5, HIGH);
digitalWrite(6, HIGH);
for (int i =0; i<=saves; i++){
servo1.write(servo1PosSave[i]);
servo2.write(servo2PosSave[i]);
servo3.write(servo3PosSave[i]);
delay(500);
}
}
delay(500);
}
Comments