Goal: I am trying to make a serial feed that will allow me to have control of four servos. I want to type in four numbers, the first number will be the servo number and the last three are the angle for the servo.
Ex: 2180 or 1090 will make servo two turn to 180 degrees or make servo one go to 90 degrees, respectively.
problems: my program seems to keep reseting itself. every so many seconds also the servos are only twitching rather than turning. I have never used the function map() to change values and am unsure if i did it right. still working on it but please help
code:
#include<stdlib.h>
int servoPin2 = 2;
int servoPin3 = 3;
int servoPin4 = 4;
int servoPin5 = 5;
int minPulse = 600;
int maxPulse = 2400;
int refreshTime = 20;
int pulseWidth2;
int pulseWidth3;
int pulseWidth4;
int pulseWidth5;
long lastPulse = 0;
char array[4];
byte count = 0;
byte four = 4;
int servo;
void setup ()
{
pinMode(servoPin2,OUTPUT);
pinMode(servoPin3,OUTPUT);
pinMode(servoPin4,OUTPUT);
pinMode(servoPin5,OUTPUT);
Serial.begin(9600);
Serial.println ("type in four numbers");
Serial.println ("first digit is the servo number");
Serial.println ("last three are anlges");
Serial.println ("start");
}
void loop ()
{
count = 0;
if (Serial.available()) //check to see if i need >0
{
do{
array[count++] = Serial.read();
delay (20);
}while(Serial.available() && count <= four); //check to see if this is correct less than sign
array[count] ='\0';
Serial.print ("you sent");
Serial.println (array);
char anglay[4] = {array[1],array[2],array[3],'\0'};
servo = atoi(anglay);
Serial.print ("the servo ");
Serial.println (array[0]);
Serial.print ("angle measuse of ");
Serial.println (anglay);
if (array[0] =='1') {pulseWidth2 = map(servo,minPulse,maxPulse,0,179);}
if (array[0] =='2') {pulseWidth3 = map(servo,minPulse,maxPulse,0,179);}
if (array[0] =='3') {pulseWidth4 = map(servo,minPulse,maxPulse,0,179);}
if (array[0] =='4') {pulseWidth5 = map(servo,minPulse,maxPulse,0,179);}
}
if (millis()-lastPulse >= refreshTime)
{
digitalWrite(servoPin2,HIGH);
delayMicroseconds(pulseWidth2); // check if it needs to be micro or milli
digitalWrite (servoPin2,LOW);
digitalWrite(servoPin3,HIGH);
delayMicroseconds(pulseWidth3); // check if it needs to be micro or milli
digitalWrite (servoPin3,LOW);
digitalWrite(servoPin4,HIGH);
delayMicroseconds(pulseWidth4); // check if it needs to be micro or milli
digitalWrite (servoPin4,LOW);
digitalWrite(servoPin5,HIGH);
delayMicroseconds(pulseWidth5); // check if it needs to be micro or milli
digitalWrite (servoPin5,LOW);
lastPulse = millis();
}
}