Devastador el tanque con cámara (2 / 4 paso)

Paso 2: programa

 #include <Metro.h>#include "GoBLE.h" int speedPin_M1 = 5; int speedPin_M2 = 6; int directionPin_M1 = 4; int directionPin_M2 = 7; Servo base; Servo top; int basePosition = 90; int topPosition = 115; int joystickX, joystickY; int buttonState[5]; void setup() { Goble.begin(); Serial.begin(115200); top.attach(9); base.attach(8); top.write(topPosition); base.write(basePosition); } void loop() { if (Goble.available()) { joystickX = Goble.readJoystickX(); joystickY = Goble.readJoystickY(); buttonState[SWITCH_UP] = Goble.readSwitchUp(); buttonState[SWITCH_DOWN] = Goble.readSwitchDown(); buttonState[SWITCH_LEFT] = Goble.readSwitchLeft(); buttonState[SWITCH_RIGHT] = Goble.readSwitchRight(); // Serial.print("Joystick Value: "); // Serial.print(joystickX); // Serial.print(" "); // Serial.println(joystickY); if (joystickY > 196) { Serial.println("turn right"); carTurnRight(250, 250); } else if (joystickY < 64) { Serial.println("turn left"); carTurnLeft(250, 250); } else if (joystickX > 196) { Serial.println("move forward"); carAdvance(500, 500); } else if (joystickX < 64) { Serial.println("move backward"); carBack(500, 500); } else { carStop(); } if (buttonState[SWITCH_LEFT] == PRESSED && buttonState[SWITCH_RIGHT] == RELEASED) { Serial.println("servo left"); Serial.println(basePosition); basePosition += 5; if (basePosition >= 180) { basePosition = 180; } } else if (buttonState[SWITCH_RIGHT] == PRESSED && buttonState[SWITCH_LEFT] == RELEASED) { Serial.println("servo right"); Serial.println(basePosition); basePosition -= 5; if (basePosition <= 0) { basePosition = 0; } } else if (buttonState[SWITCH_UP] == PRESSED && buttonState[SWITCH_DOWN] == RELEASED) { Serial.println("servo forward"); Serial.println(topPosition); topPosition -= 5; if (topPosition <= 70) { topPosition = 75; } } else if (buttonState[SWITCH_DOWN] == PRESSED && buttonState[SWITCH_UP] == RELEASED) { Serial.println("servo back"); Serial.println(topPosition); topPosition += 5; if (topPosition >= 160) { topPosition = 155; } } top.write(topPosition); base.write(basePosition); } } void carStop() { // Motor Stop digitalWrite(speedPin_M2, 0); digitalWrite(directionPin_M1, LOW); digitalWrite(speedPin_M1, 0); digitalWrite(directionPin_M2, LOW); } void carTurnLeft(int leftSpeed, int rightSpeed) { //Turn Left analogWrite (speedPin_M2, leftSpeed); //PWM Speed Control digitalWrite(directionPin_M1, HIGH); analogWrite (speedPin_M1, rightSpeed); digitalWrite(directionPin_M2, LOW); } void carTurnRight(int leftSpeed, int rightSpeed) { //Turn Right analogWrite (speedPin_M2, leftSpeed); digitalWrite(directionPin_M1, LOW); analogWrite (speedPin_M1, rightSpeed); digitalWrite(directionPin_M2, HIGH); } void carBack(int leftSpeed, int rightSpeed) { //Move backward analogWrite (speedPin_M2, leftSpeed); digitalWrite(directionPin_M1, LOW); analogWrite (speedPin_M1, rightSpeed); digitalWrite(directionPin_M2, LOW); } void carAdvance(int leftSpeed, int rightSpeed) { //Move forward analogWrite (speedPin_M2, leftSpeed); digitalWrite(directionPin_M1, HIGH); analogWrite (speedPin_M1, rightSpeed); digitalWrite(directionPin_M2, HIGH); }