Hello all,
The project I'm currently working on uses servo motors to move an arm. I am controlling the servos using serial commands, and currently have the servos responding. The problem is that the servos are moving too quickly, causing higher-level problems within my project. I need to add code that adjusts the position of the servo to the desired position (serial command) using a relatively slow, sweeping, motion. My code, currently without the sweep, is below. case 2 -> case 1 is the section of code that relates to this post (and has been marked using ************s)...the rest can be ignored.
Problems that I have run into using both for and while loops are 1) servos respond to only the first command and 2) servos sweep back and forth between two fixed positions (like the sweep example).
Any help would be greatly appreciated.
#include <Servo.h> //reference Arduino servo library
unsigned long serialdata; //create variables and variable arrays
int inbyte;
int servoPose;
int servoPoses[80] = {};
int attachedServos[80] = {};
int servoPin;
int pinNumber;
int sensorVal;
int analogRate;
int digitalState;
Servo megaservos[] = {};
void setup()
{
Serial.begin(9600); //activate serial communication at a baud rate of 9600
}
void loop() //the loop is switch-case structured, using incoming serial data in the form of "a/b/c/d/" to determine which section of the code is used
{
//Pumps or Servos?
getSerial(); //parse incoming serial command for "a" value
switch(serialdata)
{
case 1: //for "a" = 1
{
//Activate Vacuum, Positive Pressure, and Solenoid Valve
getSerial(); //parse incoming serial command for "b" value. This switch is unnecessary, but is kept in order to keep the "four value input" in tact.
switch(serialdata)
{
case 1:
{
//Turn on Vacuum
getSerial(); //parse incoming serial command for "c" value - "c" has three possible values, each corresponding to the digital pins in which the 5V supplies are provided to the electrical switches
pinNumber = serialdata; //sets variable pinNumber equal to "c" value
getSerial(); // parse incoming serial command for "d" value
digitalState = serialdata; //sets variable digitalState equal to "d" value
pinMode(pinNumber, OUTPUT); //sets pin associated with variable pinNumber ("c") as an output pin.
if (digitalState == 0)
{
digitalWrite(pinNumber, LOW); //if d is equal to 0 then set pin associated with variable pinNumber ("c") LOW, i.e. turn off pump
}
if (digitalState == 1)
{
digitalWrite(pinNumber, HIGH); //if d is equal to 1 then set pin associated with variable pinNumber ("c") HIGH, i.e. turn on pump
}
pinNumber = 0;
break;
}
}
break;
}
case 2: ******************************THIS IS WHERE I NEED HELP**************************************
//Control Arm
getSerial(); //parse incoming serial command for "b" value
switch(serialdata)
{
case 1: //for "b" = 1
{
//servo write
getSerial(); //parse incoming serial command for "c" value
servoPin = serialdata; //sets variable servoPin equal to "c" value - this value is the pin in which the signal line for the servo is connected
getSerial(); //parse incoming serial command for "d" value
servoPose = serialdata; //sets variable servoPose equal to "d" value
if (attachedServos[servoPin] == 1)
{
megaservos[servoPin].write(servoPose); //move servos attached to pin servoPin ("c") to servoPose ("d") degrees
}
if (attachedServos[servoPin] == 0)
{
Servo s1;
megaservos[servoPin] = s1;
megaservos[servoPin].attach(servoPin);
megaservos[servoPin].write(servoPose);
attachedServos[servoPin] = 1;
}
servoPoses[servoPin] = servoPose;
break;
}
*************************************END OF HELP*****************************************
case 2: //for "b" = 2
{
//servo detachment
getSerial(); //parse incoming serial command for "c" value
servoPin = serialdata; //sets variable servoPin equal to "c" value - this value is the pin in which the signal line for the servo is connected
if (attachedServos[servoPin] == 1)
{
megaservos[servoPin].detach(); //detaches the servomotors fromt the Arduino servo object. This will idle the servos and allow them to rotate freely.
attachedServos[servoPin] = 0;
}
break;
}
break;
}
}
}
long getSerial() //this code creates the back-slash separator used in the parsing of serial commands of the form "a/b/c/d/".
{
serialdata = 0;
while (inbyte != '/')
{
inbyte = Serial.read();
if (inbyte > 0 && inbyte != '/')
{
serialdata = serialdata * 10 + inbyte - '0';
}
}
inbyte = 0;
return serialdata;
}