kód :
Kód: Vybrat vše
#include <Servo.h>
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
//digital
const int SwitchPin1 = 10;
const int SwitchPin2 = 11;
//analog
const int XPin1 = A0;
const int YPin1 = A1;
const int XPin2 = A2;
const int YPin2 = A3;
//digital
const int button1 = 12; //Buttons
const int button2 = 13;
int button1Presses = 0;
boolean button2Pressed = false;
int x1Value = analogRead(XPin1); //joystick value read
int y1Value = analogRead(YPin1);
int x2Value = analogRead(XPin2);
int y2Value = analogRead(YPin2);
int switchValue = digitalRead(SwitchPin1);
int servo1PosSaves[] = {1,1,1,1,1}; //position saves
int servo2PosSaves[] = {1,1,1,1,1};
int servo3PosSaves[] = {1,1,1,1,1};
int servo4PosSaves[] = {1,1,1,1,1};
void setup() {
servo1.attach(3);
servo1.attach(5);
servo1.attach(6);
servo1.attach(7);
pinMode(button1, INPUT);
pinMode(button2, INPUT);
Serial.begin(9600);
digitalWrite(SwitchPin1, HIGH);
digitalWrite(SwitchPin2, HIGH);
}
void loop() {
int xValue1 = analogRead(XPin1);
int xValue1Angle = map(xValue1, 0, 512, 0, 528);
int yValue1 = analogRead(YPin1);
int yValue1Angle = map(yValue1, 0, 1022, 0, 528);
int xValue2 = analogRead(XPin2);
int xValue2Angle = map(xValue2, 0, 512, 0, 528);
int yValue2 = analogRead(YPin2);
int yValue2Angle = map(yValue2, 0, 1022, 0, 528);
int switchValue = digitalRead(SwitchPin1);
servo1.write(xValue1Angle);
servo2.write(yValue1Angle);
servo3.write(xValue2Angle);
servo4.write(yValue2Angle);
if(digitalRead(button1) == HIGH){
button1Presses++;
switch(button1Presses){
case 1:
servo1PosSaves[0] = xValue1Angle;
servo2PosSaves[0] = yValue1Angle;
servo3PosSaves[0] = xValue2Angle;
servo4PosSaves[0] = yValue2Angle;
Serial.println("Pos 1 Saved");
break;
case 2:
servo1PosSaves[1] = xValue1Angle;
servo2PosSaves[1] = yValue1Angle;
servo3PosSaves[1] = xValue2Angle;
servo4PosSaves[1] = yValue2Angle;
Serial.println("Pos 2 Saved");
break;
case 3:
servo1PosSaves[2] = xValue1Angle;
servo2PosSaves[2] = yValue1Angle;
servo3PosSaves[2] = xValue2Angle;
servo4PosSaves[2] = yValue2Angle;
Serial.println("Pos 3 Saved");
break;
case 4:
servo1PosSaves[3] = xValue1Angle;
servo2PosSaves[3] = yValue1Angle;
servo3PosSaves[3] = xValue2Angle;
servo4PosSaves[3] = yValue2Angle;
Serial.println("Pos 4 Saved");
break;
case 5:
servo1PosSaves[4] = xValue1Angle;
servo2PosSaves[4] = yValue1Angle;
servo3PosSaves[4] = xValue2Angle;
servo4PosSaves[4] = yValue2Angle;
Serial.println("Pos 5 Saved");
break;
}
}
if(digitalRead(button2) == HIGH){
button2Pressed = true;
}
if(button2Pressed){
for(int i = 0; i < 5; i++){
servo1.write(servo1PosSaves[i]);
servo2.write(servo2PosSaves[i]);
servo3.write(servo3PosSaves[i]);
servo4.write(servo4PosSaves[i]);
Serial.println(" Uhly Joysticku: ");
Serial.println(servo1PosSaves[i]);
Serial.println(servo2PosSaves[i]);
Serial.println(servo3PosSaves[i]);
Serial.println(servo4PosSaves[i]);
delay(1050);
}
}
delay(300);
}