Problem with servos/keytoggle

Development Boards Arduino
1

Hi all!

I try to realize a webcam project. im going to control a selfmade webcam pan/tilt mount with my arduino.

I used the Skyduino sketch http://arduino.cc/blog/2010/02/17/skydu … e/?lang=es am it works really fine.

Now I want to switch on a bulb when “C” is pressed, so that the pan/tilt mount moves to the start-position and i have light to see whats going on in front of the cam. I tried to combine this with the skyduino sketch, but it doesn’t work :frowning:

/*
======================================
  SkyDuino Sketch - Hari Wiguna 2010
======================================

*/

#include <Servo.h> 
#define RELAY_PIN 3

Servo xServo;
Servo yServo;

int stepSize = 10;

const int xInit = 90;
const int yInit = 90;
const int xMin = 0;
const int xMax = 180;
const int yMin = 0;
const int yMax = 180;

int x = xInit;
int y = yInit;
int xNew = x;
int yNew = y;

void setup()
{
  Serial.begin(9600);
  //establishContact();  // send a byte to establish contact until receiver responds 

  xServo.attach(5);  // attaches the servo on pin 9 to the servo object 
  yServo.attach(6);  // attaches the servo on pin 9 to the servo object 

  delay(1000);
  MoveServo(xServo, xServo.read(), x);
  MoveServo(yServo, yServo.read(), y);
  delay(100);
  pinMode(11, OUTPUT);
  pinMode(RELAY_PIN, OUTPUT);
}

void loop()
{
 static int relayVal = 0;
  int cmd;

  while (Serial.available() > 0)
  {
    cmd = Serial.read();

    switch (cmd)
    {
    case 'C':
      {
        relayVal ^= 1; // xor current value with 1 (causes value to toggle)
        if (relayVal)
          Serial.println("Relay on");
        else
          Serial.println("Relay off");
        break;
      }
   }

    if (relayVal)
      digitalWrite(RELAY_PIN, HIGH);
    else
      digitalWrite(RELAY_PIN, LOW);
   }
 
 
  // if we get a valid byte, read analog ins:
  if (Serial.available() > 0) {
    char msg = Serial.read();

    if (msg == 'C') {xNew=xInit; yNew=yInit;}

    if (msg == 'R') xNew = x - stepSize;
    if (msg == 'L') xNew = x + stepSize;

    if (msg == 'D') yNew = y - stepSize;
    if (msg == 'U') yNew = y + stepSize;

    xNew = constrain(xNew, xMin, xMax);
    yNew = constrain(yNew, yMin, yMax);
  }

  MoveServo(xServo, x, xNew);
  MoveServo(yServo, y, yNew);
  x = xNew;
  y = yNew;
}

void MoveServo(Servo servo, int moveFrom, int moveTo)
{
  if (moveFrom <= moveTo)
  {
    for (int c=moveFrom; c<=moveTo; c++)
    {
      servo.write(c);
      delay(50);
    }
  }

  if (moveFrom > moveTo)
  {
    for (int c=moveFrom; c>=moveTo; c--)
    {
      servo.write(c);
      delay(50);
    }
  }
}