94 lines
1.7 KiB
C++
94 lines
1.7 KiB
C++
#include <Servo.h>
|
|
|
|
Servo ServoBase;
|
|
Servo ServoArmOne;
|
|
Servo ServoArmTwo;
|
|
const int SW_pin = 10;
|
|
const int X_pin = A1;
|
|
const int Y_pin = A0;
|
|
const int podpin = A3;
|
|
const int leftButton = 7;
|
|
const int rightButton = 8;
|
|
|
|
void setup () {
|
|
ServoBase.attach(13);
|
|
ServoArmOne.attach(12);
|
|
ServoArmTwo.attach(9);
|
|
Serial.begin(9600);
|
|
pinMode(SW_pin, INPUT);
|
|
digitalWrite(SW_pin, HIGH);
|
|
pinMode(leftButton, INPUT_PULLUP);
|
|
pinMode(rightButton, INPUT_PULLUP);
|
|
|
|
|
|
}
|
|
|
|
void loop() {
|
|
|
|
bool SW = digitalRead(SW_pin);
|
|
if (!SW) {
|
|
ServoBase.write(90);
|
|
ServoArmOne.write(90);
|
|
}
|
|
|
|
|
|
|
|
//base movement
|
|
int Y_State = analogRead(Y_pin);
|
|
int currentYState = ServoBase.read();
|
|
|
|
|
|
|
|
if (Y_State > 524) {
|
|
int newYState = currentYState + 1;
|
|
ServoBase.write(newYState);
|
|
delay(15);
|
|
}else if (Y_State < 502) {
|
|
int newYState = currentYState - 1;
|
|
ServoBase.write(newYState);
|
|
delay(15);
|
|
}
|
|
|
|
//arm movement
|
|
|
|
int X_State = analogRead(X_pin);
|
|
int currentXState = ServoArmOne.read();
|
|
|
|
|
|
if (X_State > 524) {
|
|
int newXState = currentXState + 1;
|
|
ServoArmOne.write(newXState);
|
|
delay(15);
|
|
|
|
}else if (X_State < 502) {
|
|
int newXState = currentXState - 1;
|
|
ServoArmOne.write(newXState);
|
|
delay(15);
|
|
}
|
|
|
|
|
|
//armTwo movement
|
|
|
|
int currentStateLeftButton = digitalRead(leftButton);
|
|
int currentStateRightButton = digitalRead(rightButton);
|
|
int currentStateArmTwo = ServoArmTwo.read();
|
|
Serial.print("button: ");
|
|
Serial.print(leftButton);
|
|
Serial.print(rightButton);
|
|
Serial.print("\n");
|
|
|
|
while (leftButton == HIGH) {
|
|
|
|
int newStateArmTwo = currentStateArmTwo + 1;
|
|
ServoArmTwo.write(newStateArmTwo);
|
|
delay(15);
|
|
}
|
|
|
|
while (rightButton == HIGH) {
|
|
int newStateArmTwo = currentStateArmTwo - 1;
|
|
ServoArmTwo.write(newStateArmTwo);
|
|
delay(15);
|
|
}
|
|
|
|
}
|