I'm trying to control two Tower Pro MG996R servo motors to so that I can send an X,Y coordinate to point a laser at that location. I had the code and hardware working in conjunction with Processing. Something has gone wrong and now the servo motors go haywire and try to break themselves. I've tried every debugging method I can and have not come any closer to a solution. I've attached the Arduino code below if anyone can spot any issues with it. I'm running it with a 7V 1A supply common ground using an Arduino nano.
#include <Servo.h>
Servo servoX; // create servo object for X-axis
Servo servoY; // create servo object for Y-axis
int posx; // initialize variable for X-axis servo position
int posy ; // initialize variable for Y-axis servo position
int ledPin = 4; //define the LED pin the LED can be replaced with the laser, same layout in bread board too
int servoXPin = 6;
int servoYPin = 5;
int ledval; //stores LED command from serial port
void setup() {
pinMode(ledPin, OUTPUT); //initialize LED output
Serial.begin(9600);
servoX.attach(servoXPin);
servoY.attach(servoYPin); // attaches the servo on pin 6 & 5 to the servo object
posx = 0; // initialize variable for X-axis servo position
posy = 0; // initialize variable for Y-axis servo position]
//Reset their location
servoX.write(0);
servoY.write(0);
}
void loop() {
if (Serial.available() > 2) { //if some data is available of in the serial port
// Reads in x-direaction position value from serial
posx = Serial.parseInt();
// The motor can only go from 0 to 180 and at these boundaries it continues to try move so to prevent it map the values to 3 and 179
// map(value_you_want_to_map, Lower_value_to_be_mapped, Upper_value_to_be_mapped, desired_lower_value, desired_upper_value)
posx = map(posx, 0, 255, 3, 179);
// write position to servo
servoX.write(posx);
// Reads in Y-direaction position value from serial
posy = Serial.parseInt();
// as the 0 on the y motor arm is too high up still we are using the reverse side of it, hence 90 to 160
// at 90 the arm is pointing towards the sky and and at 160 the arm is pointing down to the ground
posy = map(posy, 0, 255, 90, 160);
// write position to servo
servoY.write(posy);
// reading in LED value and turnign it on or off depending on input
// this is where you can control the laser turning on and off
ledval = Serial.parseInt();
if (ledval == 1) {
digitalWrite(ledPin, HIGH); //turn ON the LED
}
else {
digitalWrite(ledPin, LOW);
}
//Uncomment the following lines if you are using the serial monitor for testing to see if the values are being recieved and mapped correctly
// Serial.println(posx);
// Serial.println(posy);
// Serial.println(ledval);
// delay to get the motors time to get into position, lower this if you want it changing positions faster
delay(2000);
}
// turnign the LED off again after 2 seconds, REMOVE THIS IF YOU WANT TO KEPP THE LASER ON ALWAYS
digitalWrite(ledPin, LOW);
}