Arduino and LEGO® Power Functions

1/*
2* Arduino and LEGO Power Functions
3* dark_storm@groundzero.com.pt
4*/
5#include  <Servo.h>
6#include <legopowerfunctions.h>
7Servo servoMain; // Define our Servo
8int  curSpeed = 0;
9// IR led on port 13
10LEGOPowerFunctions lego(13);
11int irPin  = 12;
12int count = 0;
13int buttonState=0;
14int fwdRev=0;
15int stationPin  = 2;
16int buzzerPin = 11;
17//irPin - conta as voltas
18//count - variavel que  conta as voltas
19//stationpin - para o comboio na estacao
20//buzzerpin - apito  de saida de estacao
21void setup()
22{
23 Serial.begin(9600);
24 pinMode(irPin,  INPUT);
25 pinMode(stationPin,INPUT);
26 servoMain.attach(10); // servo on digital  pin 10
27 pinMode(buzzerPin,OUTPUT);
28}
29void loop()
30{ 
31 if ( digitalRead(irPin)  == 0 ) 
32 {
33  count++;
34  delay(3000);
35 }
36 if (count <= 1)
37 {
38  lego.SingleOutput( PWM, PWM_FWD5, RED, CH1);
39  delay(100);
40  if (digitalRead(stationPin)  == 0)
41  {
42   lego.SingleOutput( PWM, PWM_FLT, RED, CH1);
43   servoMain.write(0);  // Turn Servo Left to 45 degrees
44   delay(2000);
45   tone(11,2000,1000);
46   delay(500);
47  }
48 }
49 else
50 {
51  servoMain.write(90); // Turn Servo  Left to 45 degrees
52  count = 0; 
53 }
54}