Prvni commit.

2026-02-01 00:00:07 +01:00
commit 4e5c55e846
213 changed files with 17991 additions and 0 deletions
@@ -0,0 +1,26 @@
+#include <Servo.h>
+Servo myservo1; // create servo object to control a servo
+Servo myservo2;
+Servo myservo3;
+Servo myservo4;
+int pos1=90, pos2=90, pos3=90, pos4=90; //define the variable of 4 servo angle and assign the initial value for installing
+void setup()
+{
+  Serial.begin(9600);
+  myservo1.attach(3); // set the control pin of servo 1 to 3 digital I/0
+  myservo2.attach(5); // set the control pin of servo 2 to 5 digital I/0
+  myservo3.attach(6); // set the control pin of servo 3 to 6 digital I/0
+  myservo4.attach(9); // set the control pin of servo 4 to 9 digital I/0
+  delay(1000);
+}
+
+void loop() 
+{
+  myservo1.write(pos1);  // Control servo motor rotation to specified angle
+  myservo2.write(pos2);
+  myservo3.write(pos3);
+  myservo4.write(pos4);
+  delay(1000);
+ 
+  
+}
@@ -0,0 +1,110 @@
+#include <Servo.h>
+Servo myservo1;  // create servo object to control a servo
+Servo myservo2;
+Servo myservo3;
+Servo myservo4;
+int pos1=90, pos2=90, pos3=90, pos4=90; 
+
+void setup()
+{
+  myservo1.attach(3);    // set the control pin of servo 1 to 3 digital I/0
+  myservo2.attach(5);    // set the control pin of servo 1 to 3 digital I/0
+  myservo3.attach(6);    // set the control pin of servo 1 to 3 digital I/0
+  myservo4.attach(9);    // set the control pin of servo 1 to 3 digital I/0
+  
+  myservo1.write(pos1);
+  myservo2.write(pos2);
+  myservo3.write(pos3);
+  myservo4.write(pos4);
+  delay(1500);
+}
+
+void loop() 
+{
+
+  // close the claw 
+  for(pos4;pos4>45;pos4--)
+  {
+    myservo4.write(pos4);
+  }
+  delay(1000);
+  
+ // open the claw
+ for(pos4;pos4<120;pos4++)
+ {
+   myservo4.write(pos4);
+ }
+  delay(1000);
+
+
+// turn right
+  for(pos1;pos1>30;pos1--)
+  {
+    myservo1.write(pos1);
+    delay(5);      // delay 5ms（used to adjust the servo speed）
+  }
+  delay(1000);
+
+
+
+   // turn to left 
+  for(pos1;pos1<150;pos1++)
+  {
+    myservo1.write(pos1);
+    delay(5);
+  }
+  delay(1000);
+
+  //  stretch out the arm
+  for(pos2;pos2<130;pos2++)
+  {
+    myservo2.write(pos2);
+    delay(5);
+  }
+  delay(1000);
+
+
+  // retracte the arm
+  for(pos2;pos2>80;pos2--)
+  {
+    myservo2.write(pos2);
+    delay(5);
+  }
+  delay(1000);
+
+
+
+
+  // raise the arm 
+  for(pos3;pos3<100;pos3++)
+  {
+    myservo3.write(pos3);
+    delay(5);
+  }
+  delay(1500);
+
+ // Lower the arm 
+  for(pos3;pos3>40;pos3--)
+  {
+    myservo3.write(pos3);
+    delay(5);
+  }
+  delay(1000);
+
+}
+  
+  
+ 
+  
+  
+
+
+
+  
+  
+
+
+ 
+
+ 
+ 
@@ -0,0 +1,19 @@
+#include <Servo.h>
+Servo myservo;  // create servo object to control a servo
+
+void setup()
+{
+  Serial.begin(9600); 
+  delay(1000);
+}
+
+void loop() 
+{
+  myservo.attach(3);  // modify each pin to adjust 
+  //myservo.write(0);  // the servo will automatically rotate to 0°
+  //delay(1000);
+  myservo.write(90);  //  the servo will automatically rotate to 90°
+  //myservo.write(180);  //  the servo will automatically rotate to 180°
+  delay(1000);
+  
+}
@@ -0,0 +1,46 @@
+const int right_X = A2; // define the right X pin to A2 
+const int right_Y = A5; // define the right Y pin to A5 
+const int right_key = 7; //define the right key pin to 7（that is the value Z）
+
+const int left_X = A3; //define the left X pin to A3
+const int left_Y = A4;  // define the left Y pin to A4
+const int left_key = 8; //define the left key pin to 8（that is the value Z） 
+
+void setup() 
+{
+  pinMode(right_key, INPUT);   // set the right/left key to INPUT 
+  pinMode(left_key, INPUT);
+  Serial.begin(9600); //  set the baud rate to 9600
+}
+
+void loop() 
+{
+  int x1,y1,z1;  // define the variable, used to save the joystick value it reads
+  int x2,y2,z2;
+  
+  x2 = analogRead(right_X); // read the value of right X 
+  y2 = analogRead(right_Y);  // read the value of right Y 
+  z2 = digitalRead(right_key);  //// read the value of right Z 
+  
+  x1 = analogRead(left_X);  // read the value of left X
+  y1 = analogRead(left_Y);  // read the value of left Y 
+  z1 = digitalRead(left_key);  // read the value of left Z 
+  
+  Serial.println("#####right_joystick########");
+  Serial.print("right_X = "); // on the serial monitor, print out right_X = 
+  Serial.println(x2 ,DEC);  // print out the value of right X and line wrap
+  Serial.print("right_Y = ");
+  Serial.println(y2 ,DEC);
+  //Serial.print("right_key = ");
+  //Serial.println(z2 ,DEC);
+  Serial.println("##########################");
+  Serial.println("#####left_joystick########");
+  Serial.print("left_X = ");
+  Serial.println(x1 ,DEC);
+  Serial.print("left_Y = ");
+  Serial.println(y1 ,DEC);
+  //Serial.print("left_key = ");
+  //Serial.println(z1 ,DEC);
+  Serial.println("##########################");
+  delay(2000);
+}
@@ -0,0 +1,179 @@
+#include <Servo.h> // add the servo libraries
+Servo myservo1; // create servo object to control a servo
+Servo myservo2;
+Servo myservo3;
+Servo myservo4;
+int pos1=90, pos2=90, pos3=90, pos4=90; // define the variable of 4 servo angle,and assign the initial value (that is the boot posture
+//angle value)
+const int right_X = A2; // define the right X pin to A2
+const int right_Y = A5; // define the right Y pin to A5
+const int right_key = 7; // define the right key pin to 7（that is the value of Z）
+const int left_X = A3; // define the left X pin to A3
+const int left_Y = A4; // define the left X pin to A4
+const int left_key = 8; //define the left key pin to 8（that is the value of Z）
+int x1,y1,z1; // define the variable, used to save the joystick value it read.
+int x2,y2,z2;
+
+void setup()
+{
+// boot posture
+myservo1.write(pos1);
+delay(1000);
+myservo2.write(pos2);
+myservo3.write(pos3);
+myservo4.write(pos4);
+delay(1500);
+pinMode(right_key, INPUT); // set the right/left key to INPUT
+pinMode(left_key, INPUT);
+Serial.begin(9600); // set the baud rate to 9600
+}
+void loop()
+{
+myservo1.attach(3); // set the control pin of servo 1 to D3  dizuo-servo1-3
+myservo2.attach(5); // set the control pin of servo 2 to D5  arm-servo2-5
+myservo3.attach(6); //set the control pin of servo 3 to D6   lower arm-servo-6
+myservo4.attach(9); // set the control pin of servo 4 to D9  claw-servo-9
+x2 = analogRead(right_X); //read the right X value
+y2 = analogRead(right_Y); // read the right Y value
+z2 = digitalRead(right_key); //// read the right Z value
+x1 = analogRead(left_X); //read the left X value
+y1 = analogRead(left_Y); //read the left Y value
+z1 = digitalRead(left_key); // read the left Z value
+//delay(5); // lower the speed overall
+
+// claw
+claw();
+
+// rotate
+turn();
+
+// upper arm
+upper_arm();
+
+//lower arm
+lower_arm();
+}
+
+
+
+//***************************************************
+//claw
+void claw()
+{
+//claw
+if(x1<50) // if push the left joystick to the right
+{
+pos4=pos4+3; 
+myservo4.write(pos4); //servo 4 operates the motion, the claw gradually opens. 
+delay(5);
+
+if(pos4>120) //limit the largest angle when open the claw 
+{
+pos4=120;
+}
+}
+
+if(x1>1000) ////if push the right joystick to the left 
+{
+pos4=pos4-3; 
+myservo4.write(pos4); // servo 4 operates the action, claw is gradually closed.
+delay(5);
+if(pos4<45) // 
+{
+pos4=45; //limit the largest angle when close the claw
+}
+
+}
+}
+
+
+
+//******************************************************/
+// turn
+void turn()
+{
+if(x2<50) //if push the right joystick to the let 
+{
+pos1=pos1+3; 
+myservo1.write(pos1); // arm turns left
+delay(5);
+if(pos1>180) //limit the angle when turn right 
+{
+pos1=180;
+}
+
+}
+
+if(x2>1000) // if push the right joystick to the right
+{
+pos1=pos1-3; 
+myservo1.write(pos1); //servo 1 operates the motion, the arm turns right. 
+delay(5);
+if(pos1<1) // limit the angle when turn left
+{
+pos1=1;
+}
+}
+}
+
+
+
+
+//**********************************************************/
+// lower arm
+void lower_arm()
+{
+if(y2>1000) // if push the right joystick downward
+{
+pos2=pos2-2;
+myservo2.write(pos2); // lower arm will draw back
+delay(5);
+if(pos2<25) // limit the retracted angle
+{
+pos2=25;
+}
+}
+
+if(y2<50) // if push the right joystick upward
+{
+pos2=pos2+2;
+myservo2.write(pos2); // lower arm will stretch out
+delay(5);
+if(pos2>180) // limit the stretched angle
+{
+pos2=180;
+}
+}
+}
+
+
+
+
+
+//*************************************************************/
+
+//upper arm
+void upper_arm()
+{
+if(y1<50) // if push the left joystick downward
+{
+pos3=pos3-2;
+myservo3.write(pos3); // upper arm will go down
+delay(5);
+if(pos3<1) //  limit the angle when go down 
+{
+pos3=1;
+}
+}
+if(y1>1000) // if push the left joystick upward
+{
+pos3=pos3+2;
+myservo3.write(pos3); // the upper arm will lift
+delay(5);
+
+if(pos3>135) //limit the lifting angle 
+{
+pos3=135;
+}
+}
+}
@@ -0,0 +1,186 @@
+#include <Servo.h>  // add the servo libraries
+Servo myservo1;  // create servo object to control a servo
+Servo myservo2;
+Servo myservo3;
+Servo myservo4;
+int pos1=90, pos2=90, pos3=90, pos4=90;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
+char val;
+int incomingByte = 0;          // Received data byte
+String inputString = "";         // Used to store received content
+boolean newLineReceived = false; // Previous data end flag
+boolean startBit  = false;  //Acceptance Agreement Start Sign
+int num_reveice=0;
+void setup()
+{
+   // boot posture
+  myservo1.write(pos1);  
+  delay(1000);
+  myservo2.write(pos2);
+  myservo3.write(pos3);
+  myservo4.write(pos4);
+  delay(1500);
+
+  Serial.begin(9600); //  set the baud rate to 9600
+}
+
+void loop() 
+{
+  myservo1.attach(3);  // set the control pin of servo 1 to D3
+  myservo2.attach(5);  // set the control pin of servo 2 to D5
+  myservo3.attach(6);   // set the control pin of servo 3 to D6
+  myservo4.attach(9);   // set the control pin of servo 4 to D9
+
+while (Serial.available())
+  {
+    incomingByte = Serial.read();              //One byte by byte, the next sentence is read into a string array to form a completed packet
+    if (incomingByte == '%')
+    {
+      num_reveice = 0;
+      startBit = true;
+    }
+    if (startBit == true)
+    {
+      num_reveice++;
+      inputString += (char) incomingByte;     
+    }
+    if (startBit == true && incomingByte == '#')
+    {
+      newLineReceived = true;
+      startBit = false;
+    }
+    
+    if(num_reveice >= 20)
+    {
+      num_reveice = 0;
+      startBit = false;
+      newLineReceived = false;
+      inputString = "";
+    }  
+  }
+
+
+
+if(newLineReceived)
+{
+    switch(inputString[1])   
+    {
+      case 'B':  T_left();  break;   // turn left
+      case 'C':  T_right();  break;//turn right 
+      case 'A':  RB();  break;// the lower arm will draw back 
+      case 'D':  RF();  break;// the lower arm will  stretch out
+      case '5':  ZK();  break;//close the claw
+      case '6':  ZB();  break;//close the claw
+      case '4':  LB();  break;//the upper arm will lift up 
+      case '7':  LF();  break;//the upper arm will go down 
+      default:break;
+    }
+
+      inputString = "";   // clear the string
+      newLineReceived = false;
+}   
+   
+
+
+
+
+  
+}
+//**************************************************
+// turn left
+void T_left()
+{
+    pos1=pos1+8;
+    myservo1.write(pos1);
+    delay(5);
+    if(pos1>180)
+    {
+      pos1=180;
+    }
+}
+//turn right 
+void T_right()
+{
+    pos1=pos1-8;
+    myservo1.write(pos1);
+    delay(5);
+    if(pos1<1)
+    {
+      pos1=1;
+    }
+}
+//********************************************
+//close the claw
+
+void ZK()
+{
+      pos4=pos4-8;
+      Serial.println(pos4);
+      myservo4.write(pos4);
+      delay(5);
+      if(pos4<45)
+      {
+        pos4=45;
+      }
+}
+// open the claw
+void ZB()
+{
+    pos4=pos4+8;
+      Serial.println(pos4);
+      myservo4.write(pos4);
+      delay(5);
+      if(pos4>120)
+      {
+        pos4=120;
+      }
+}
+
+//******************************************
+// the lower arm will  stretch out
+void RF()
+{
+    pos2=pos2-8;
+    myservo2.write(pos2);
+    delay(5);
+    if(pos2<25)
+    {
+      pos2=25;
+    }
+}
+// the lower arm will draw back 
+void RB()
+{
+    pos2=pos2+8;
+    myservo2.write(pos2);
+    delay(5);
+    if(pos2>180)
+    {
+      pos2=180;
+    }
+}
+
+//***************************************
+//the upper arm will lift up  
+void LB()
+{
+  pos3=pos3+8;
+    myservo3.write(pos3);
+    delay(5);
+    if(pos3>135)
+    {
+      pos3=135;
+    }
+}
+
+//the upper arm will go down  
+
+void LF()
+{
+  pos3=pos3-8;
+    myservo3.write(pos3);
+    delay(5);
+    if(pos3<0)
+    {
+      pos3=0;
+    }
+}
@@ -0,0 +1,68 @@
+int pos1=90, pos2=90, pos3=90, pos4=90;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
+char val;
+int incomingByte = 0;          // Received data byte
+String inputString = "";         // Used to store received content
+boolean newLineReceived = false; //  Previous data end flag
+boolean startBit  = false;  //Acceptance Agreement Start Sign
+int num_reveice=0;
+void setup()
+{
+  Serial.begin(9600); //  set the baud rate to 9600
+}
+
+void loop() 
+{
+while (Serial.available())
+  {
+    incomingByte = Serial.read();              //One byte by byte, the next sentence is read into a string array to form a completed packet
+    if (incomingByte == '%')
+    {
+      num_reveice = 0;
+      startBit = true;
+    }
+    if (startBit == true)
+    {
+      num_reveice++;
+      inputString += (char) incomingByte;    
+    }
+    if (startBit == true && incomingByte == '#')
+    {
+      newLineReceived = true;
+      startBit = false;
+    }
+    
+    if(num_reveice >= 20)
+    {
+      num_reveice = 0;
+      startBit = false;
+      newLineReceived = false;
+      inputString = "";
+    }  
+  }
+
+if(newLineReceived)
+{
+      Serial.println(inputString);
+      inputString = "";   // clear the string
+      newLineReceived = false;
+}   
+   
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+  
+}
@@ -0,0 +1,279 @@
+
+#include <PS2X_lib.h>
+
+PS2X ps2x; // create PS2 Controller Class
+
+
+//right now, the library does NOT support hot pluggable controllers, meaning 
+//you must always either restart your Arduino after you connect the controller, 
+//or call config_gamepad(pins) again after connecting the controller.
+int error = 0; 
+byte vibrate = 0;
+
+#include <Servo.h>  // add the servo libraries 
+Servo myservo1;  // create servo object to control a servo
+Servo myservo2;
+Servo myservo3;
+Servo myservo4;
+
+int pos1=90, pos2=90, pos3=90, pos4=90;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
+
+void setup(){
+ Serial.begin(57600);
+
+// boot posture
+  myservo1.write(pos1);  
+  delay(1000);
+  myservo2.write(pos2);
+  myservo3.write(pos3);
+  myservo4.write(pos4);
+  delay(1500);
+  
+ error = ps2x.config_gamepad(13,11,10,12);   //setup GamePad(clock, command, attention, data) pins, check for error
+ 
+ if(error == 0){
+   Serial.println("Found Controller, configured successful");
+   Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
+  Serial.println("holding L1 or R1 will print out the analog stick values.");
+  Serial.println("Go to www.billporter.info for updates and to report bugs.");
+ }
+   
+  else if(error == 1)
+   Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
+   
+  else if(error == 2)
+   Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
+   
+   //Serial.print(ps2x.Analog(1), HEX);
+ 
+ 
+ ps2x.enableRumble();              //enable rumble vibration motors
+ ps2x.enablePressures();           //enable reading the pressure values from the buttons. 
+  
+
+  
+}
+
+void loop(){
+   /* You must Read Gamepad to get new values
+   Read GamePad and set vibration values
+   ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
+   if you don't enable the rumble, use ps2x.read_gamepad(); with no values
+   
+   you should call this at least once a second
+   */
+
+  myservo1.attach(3);  // set the control pin of servo 1 to A1
+  myservo2.attach(5);  // set the control pin of servo 2 to A0
+  myservo3.attach(6);   // set the control pin of servo 3 to D6
+  myservo4.attach(9);   // set the control pin of servo 4 to D9
+  
+ if(error != 0)
+  return; 
+  
+  ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed
+  
+  if(ps2x.Button(PSB_START))                   //will be TRUE as long as button is pressed
+       Serial.println("Start is being held");
+  if(ps2x.Button(PSB_SELECT))
+       Serial.println("Select is being held");
+       
+       
+   if(ps2x.Button(PSB_PAD_UP)) {         //will be TRUE as long as button is pressed
+     Serial.print("Up held this hard: ");
+     Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_RIGHT)){
+     Serial.print("Right held this hard: ");
+      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_LEFT)){
+     Serial.print("LEFT held this hard: ");
+      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_DOWN)){
+     Serial.print("DOWN held this hard: ");
+   Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
+    }   
+
+  
+    vibrate = ps2x.Analog(PSAB_BLUE);        //this will set the large motor vibrate speed based on 
+                                            //how hard you press the blue (X) button    
+  
+  if (ps2x.NewButtonState())               //will be TRUE if any button changes state (on to off, or off to on)
+  {
+      if(ps2x.Button(PSB_R3))
+        
+       Serial.println("R3 pressed");
+     
+       
+      if(ps2x.Button(PSB_L3))
+       Serial.println("L3 pressed");
+      
+      if(ps2x.Button(PSB_L2))
+       Serial.println("L2 pressed");
+      if(ps2x.Button(PSB_R2))
+       Serial.println("R2 pressed");
+      if(ps2x.Button(PSB_GREEN))
+       Serial.println("Triangle pressed");
+       
+  }   
+       
+  
+  if(ps2x.ButtonPressed(PSB_RED))             //will be TRUE if button was JUST pressed
+       Serial.println("Circle just pressed");
+       
+  if(ps2x.ButtonReleased(PSB_PINK))             //will be TRUE if button was JUST released
+       Serial.println("Square just released");     
+  
+  if(ps2x.NewButtonState(PSB_BLUE))            //will be TRUE if button was JUST pressed OR released
+       Serial.println("X just changed");    
+
+  // claw
+claw();
+
+// rotate
+turn();
+
+// upper arm
+upper_arm();
+
+//lower arm
+lower_arm();
+    
+  if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) // print stick values if either is TRUE
+  {
+      Serial.print("Stick Values:");
+      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX  
+      Serial.print(",");
+      Serial.print(ps2x.Analog(PSS_LX), DEC); 
+      Serial.print(",");
+      Serial.print(ps2x.Analog(PSS_RY), DEC); 
+      Serial.print(",");
+      Serial.println(ps2x.Analog(PSS_RX), DEC); 
+  }    
+  delay(5);    
+}
+
+
+
+
+
+//***************************************************
+//claw
+void claw()
+{
+//claw
+if(ps2x.Analog(PSS_LX)<10) // if push the left joystick to the right
+{
+pos4=pos4-3; 
+myservo4.write(pos4); //servo 4 operates the motion, the claw gradually opens. 
+delay(5);
+if(pos4<45) // limit the largest angle when open the claw
+
+pos4=45; 
+}
+
+
+if(ps2x.Analog(PSS_LX)>220) ////if push the right joystick to the left 
+{
+pos4=pos4+3; 
+myservo4.write(pos4); // servo 4 operates the action, claw is gradually closed.
+delay(5);
+if(pos4>120) // limit the largest angle when close the claw
+{
+pos4=120;
+}
+}
+}
+
+
+
+//******************************************************/
+// turn
+void turn()
+{
+if(ps2x.Analog (PSS_RX) < 50) //if push the right joystick to the let 
+{
+pos1=pos1+3; 
+myservo1.write(pos1); // arm turns left
+delay(5);
+if(pos1>180) //limit the angle when turn right 
+{
+pos1=180;
+}
+
+}
+
+if(ps2x.Analog (PSS_RX) > 200) // if push the right joystick to the right
+{
+pos1=pos1-3; 
+myservo1.write(pos1); //servo 1 operates the motion, the arm turns right. 
+delay(5);
+if(pos1<1) // limit the angle when turn left
+{
+pos1=1;
+}
+}
+}
+
+
+
+
+//**********************************************************/
+// lower arm
+void lower_arm()
+{
+if(ps2x.Analog(PSS_RY)>200) // if push the right joystick downward
+{
+pos2=pos2-2;
+myservo2.write(pos2); // lower arm will draw back
+delay(5);
+if(pos2<25) // limit the retracted angle
+{
+pos2=25;
+}
+}
+
+if(ps2x.Analog(PSS_RY)<50) // if push the right joystick upward
+{
+pos2=pos2+2;
+myservo2.write(pos2); // lower arm will stretch out
+delay(5);
+if(pos2>180) // limit the stretched angle
+{
+pos2=180;
+}
+}
+}
+
+
+
+
+
+//*************************************************************/
+
+//upper arm
+void upper_arm()
+{
+if(ps2x.Analog(PSS_LY)<10) // if push the left joystick downward
+{
+pos3=pos3+2;
+myservo3.write(pos3); // upper arm will go down
+delay(5);
+if(pos3>135) // limit the angle when go down 
+{
+pos3=135;
+}
+}
+if(ps2x.Analog(PSS_LY)>200) // if push the left joystick upward
+{
+pos3=pos3-2;
+myservo3.write(pos3); // the upper arm will lift
+delay(5);
+if(pos3<1) //limit the lifting angle
+{
+pos3=1;
+}
+}
+}
@@ -0,0 +1,401 @@
+
+#include <PS2X_lib.h>
+
+PS2X ps2x; // create PS2 Controller Class
+
+
+//right now, the library does NOT support hot pluggable controllers, meaning 
+//you must always either restart your Arduino after you connect the controller, 
+//or call config_gamepad(pins) again after connecting the controller.
+int error = 0; 
+byte vibrate = 0;
+int s1,s2,s3,s4;
+int jiyi1[20];  // define four array, separately used to save the angle of 4 servos.
+int jiyi2[20];
+int jiyi3[20];
+int jiyi4[20];
+int i=0;
+int j=0;
+#include <Servo.h>  // add the servo libraries 
+Servo myservo1;  // create servo object to control a servo
+Servo myservo2;
+Servo myservo3;
+Servo myservo4;
+
+int pos1=90, pos2=90, pos3=90, pos4=90;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
+
+void setup(){
+ Serial.begin(57600);
+
+// boot posture
+  myservo1.write(pos1);  
+  delay(1000);
+  myservo2.write(pos2);
+  myservo3.write(pos3);
+  myservo4.write(pos4);
+  delay(1500);
+  
+ error = ps2x.config_gamepad(13,11,10,12);   //setup GamePad(clock, command, attention, data) pins, check for error
+ 
+ if(error == 0){
+   Serial.println("Found Controller, configured successful");
+   Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
+  Serial.println("holding L1 or R1 will print out the analog stick values.");
+  Serial.println("Go to www.billporter.info for updates and to report bugs.");
+ }
+   
+  else if(error == 1)
+   Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
+   
+  else if(error == 2)
+   Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
+   
+   //Serial.print(ps2x.Analog(1), HEX);
+ 
+ 
+ ps2x.enableRumble();              //enable rumble vibration motors
+ ps2x.enablePressures();           //enable reading the pressure values from the buttons. 
+  
+
+  
+}
+
+void loop(){
+   /* You must Read Gamepad to get new values
+   Read GamePad and set vibration values
+   ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
+   if you don't enable the rumble, use ps2x.read_gamepad(); with no values
+   
+   you should call this at least once a second
+   */
+
+  myservo1.attach(3);  // set the control pin of servo 1 to A1
+  myservo2.attach(5);  // set the control pin of servo 2 to A0
+  myservo3.attach(6);   // set the control pin of servo 3 to D6
+  myservo4.attach(9);   // set the control pin of servo 4 to D9
+  
+ if(error != 0)
+  return; 
+  
+  ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed
+  
+  if(ps2x.Button(PSB_START))                   //will be TRUE as long as button is pressed
+       Serial.println("Start is being held");
+  if(ps2x.Button(PSB_SELECT))
+       Serial.println("Select is being held");
+       
+       
+   if(ps2x.Button(PSB_PAD_UP)) {         //will be TRUE as long as button is pressed
+     Serial.print("Up held this hard: ");
+     Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_RIGHT)){
+     Serial.print("Right held this hard: ");
+      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_LEFT)){
+     Serial.print("LEFT held this hard: ");
+      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_DOWN)){
+     Serial.print("DOWN held this hard: ");
+   Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
+    }   
+
+  
+    vibrate = ps2x.Analog(PSAB_BLUE);        //this will set the large motor vibrate speed based on 
+                                            //how hard you press the blue (X) button    
+ if (ps2x.NewButtonState())               //will be TRUE if any button changes state (on to off, or off to on)
+  {
+      if(ps2x.Button(PSB_R3))
+      {
+        //Serial.println("R3 pressed");
+        // record
+        s1=myservo1.read();
+        delay(100);
+        Serial.println(s1);
+        s2=myservo2.read();
+        delay(100);
+        Serial.println(s2);
+        s3=myservo3.read();
+        delay(100);
+        Serial.println(s3);
+        s4=myservo4.read();
+        delay(100);
+        Serial.println(s4);
+      
+        jiyi1[i]=s1;  // save the servo value read in the array sequentially
+        jiyi2[i]=s2;
+        jiyi3[i]=s3;
+        jiyi4[i]=s4;
+        i++;
+        j=i;
+       // delay(100);
+       Serial.println(i);
+      }
+       
+      if(ps2x.Button(PSB_L3))
+      {
+        //Serial.println("L3 pressed");
+        i=0;
+   //执行
+       pos1 = myservo1.read();
+       pos2 = myservo2.read();
+       pos3 = myservo3.read();
+       pos4 = myservo4.read();
+
+      for(int k=0;k<j;k++)  //for loop, to execute all the stored actions
+      { 
+        if(pos1<jiyi1[k])  //if the current servo 1 angle is less than the value stored in array 1.
+        {
+          while(pos1<jiyi1[k])  //while loop, make servo turn to the position of value stored in the array.
+          {
+            myservo1.write(pos1);   // servo 1 executes the action
+            delay(5);   // delay 5ms，controlling the rotating speed of servo
+            pos1++;
+            //Serial.println(pos1);
+          }
+        }
+        else   //if the current servo 1 angle is greater than the value stored in array 1.
+        {
+         while(pos1>jiyi1[k])  //while loop, make servo turn to the position of value stored in the array.
+          {  
+            myservo1.write(pos1);   // servo 1 executes the action
+            delay(5);    //delay 5ms，controlling the rotating speed of servo
+            pos1--;
+            //Serial.println(pos1);
+          }
+        
+//**********************************************
+// the same analysis as the previous servo
+        if(pos2<jiyi2[k])
+        {
+          while(pos2<jiyi2[k])
+          {
+            myservo2.write(pos2);
+            delay(5);
+            pos2++;
+            //Serial.println(pos1);
+          }
+        }
+        else
+        {
+         while(pos2>jiyi2[k])
+          {
+            myservo2.write(pos2);
+            delay(5);
+            pos2--;
+            //Serial.println(pos1);
+          }
+        }
+ //*****************************************************
+ //the same analysis 
+        if(pos3<jiyi3[k])
+        {
+          while(pos3<jiyi3[k])
+          {
+            myservo3.write(pos3);
+            delay(5);
+            pos3++;
+            //Serial.println(pos1);
+          }
+        }
+        else
+        {
+         while(pos3>jiyi3[k])
+          {
+            myservo3.write(pos3);
+            delay(5);
+            pos3--;
+            //Serial.println(pos1);
+          }
+        }
+ //*****************************************************   
+ //the same analysis
+        if(pos4<jiyi4[k])
+        {
+          while(pos4<jiyi4[k])
+          {
+            myservo4.write(pos4);
+            delay(5);
+            pos4++;
+            //Serial.println(pos1);
+          }
+        }
+        else
+        {
+         while(pos4>jiyi4[k])
+          {
+            myservo4.write(pos4);
+            delay(5);
+            pos4--;
+            //Serial.println(pos1);
+          }
+        }
+      }
+    }
+      }
+       
+      
+      if(ps2x.Button(PSB_L2))
+       Serial.println("L2 pressed");
+      if(ps2x.Button(PSB_R2))
+       Serial.println("R2 pressed");
+      if(ps2x.Button(PSB_GREEN))
+       Serial.println("Triangle pressed");
+       
+  }   
+       
+  
+  if(ps2x.ButtonPressed(PSB_RED))             //will be TRUE if button was JUST pressed
+       Serial.println("Circle just pressed");
+       
+  if(ps2x.ButtonReleased(PSB_PINK))             //will be TRUE if button was JUST released
+       Serial.println("Square just released");     
+  
+  if(ps2x.NewButtonState(PSB_BLUE))            //will be TRUE if button was JUST pressed OR released
+       Serial.println("X just changed");    
+
+     // claw
+claw();
+
+// rotate
+turn();
+
+// upper arm
+upper_arm();
+
+//lower arm
+lower_arm();
+    
+  if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) // print stick values if either is TRUE
+  {
+      Serial.print("Stick Values:");
+      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX  
+      Serial.print(",");
+      Serial.print(ps2x.Analog(PSS_LX), DEC); 
+      Serial.print(",");
+      Serial.print(ps2x.Analog(PSS_RY), DEC); 
+      Serial.print(",");
+      Serial.println(ps2x.Analog(PSS_RX), DEC); 
+  }    
+  delay(5);    
+}
+//***************************************************
+//claw
+void claw()
+{
+//claw
+if(ps2x.Analog(PSS_LX)<10) // if push the left joystick to the right
+{
+pos4=pos4-3; 
+myservo4.write(pos4); //servo 4 operates the motion, the claw gradually opens. 
+delay(5);
+if(pos4<45) // limit the largest angle when open the claw
+
+pos4=45; 
+}
+
+
+if(ps2x.Analog(PSS_LX)>220) ////if push the right joystick to the left 
+{
+pos4=pos4+3; 
+myservo4.write(pos4); // servo 4 operates the action, claw is gradually closed.
+delay(5);
+if(pos4>120) // limit the largest angle when close the claw
+{
+pos4=120;
+}
+}
+}
+
+
+
+//******************************************************/
+// turn
+void turn()
+{
+if(ps2x.Analog (PSS_RX) < 50) //if push the right joystick to the let 
+{
+pos1=pos1+3; 
+myservo1.write(pos1); // arm turns left
+delay(5);
+if(pos1>180) //limit the angle when turn right 
+{
+pos1=180;
+}
+
+}
+
+if(ps2x.Analog (PSS_RX) > 200) // if push the right joystick to the right
+{
+pos1=pos1-3; 
+myservo1.write(pos1); //servo 1 operates the motion, the arm turns right. 
+delay(5);
+if(pos1<1) // limit the angle when turn left
+{
+pos1=1;
+}
+}
+}
+
+
+
+
+//**********************************************************/
+// lower arm
+void lower_arm()
+{
+if(ps2x.Analog(PSS_RY)>200) // if push the right joystick downward
+{
+pos2=pos2-2;
+myservo2.write(pos2); // lower arm will draw back
+delay(5);
+if(pos2<25) // limit the retracted angle
+{
+pos2=25;
+}
+}
+
+if(ps2x.Analog(PSS_RY)<50) // if push the right joystick upward
+{
+pos2=pos2+2;
+myservo2.write(pos2); // lower arm will stretch out
+delay(5);
+if(pos2>180) // limit the stretched angle
+{
+pos2=180;
+}
+}
+}
+
+
+
+
+
+//*************************************************************/
+
+//upper arm
+void upper_arm()
+{
+if(ps2x.Analog(PSS_LY)<10) // if push the left joystick downward
+{
+pos3=pos3+2;
+myservo3.write(pos3); // upper arm will go down
+delay(5);
+if(pos3>135) // limit the angle when go down 
+{
+pos3=135;
+}
+}
+if(ps2x.Analog(PSS_LY)>200) // if push the left joystick upward
+{
+pos3=pos3-2;
+myservo3.write(pos3); // the upper arm will lift
+delay(5);
+if(pos3<1) //limit the lifting angle
+{
+pos3=1;
+}
+}
+}
@@ -0,0 +1,464 @@
+#include "PS2X_lib.h"
+#include <math.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <avr/io.h>
+#if ARDUINO > 22
+  #include "Arduino.h"
+#else
+  #include "WProgram.h"
+  #include "pins_arduino.h"
+#endif
+
+static byte enter_config[]={0x01,0x43,0x00,0x01,0x00};
+static byte set_mode[]={0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00};
+static byte set_bytes_large[]={0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00};
+static byte exit_config[]={0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A};
+static byte enable_rumble[]={0x01,0x4D,0x00,0x00,0x01};
+static byte type_read[]={0x01,0x45,0x00,0x5A,0x5A,0x5A,0x5A,0x5A,0x5A};
+
+/****************************************************************************************/
+boolean PS2X::NewButtonState() {
+  return ((last_buttons ^ buttons) > 0);
+}
+
+/****************************************************************************************/
+boolean PS2X::NewButtonState(unsigned int button) {
+  return (((last_buttons ^ buttons) & button) > 0);
+}
+
+/****************************************************************************************/
+boolean PS2X::ButtonPressed(unsigned int button) {
+  return(NewButtonState(button) & Button(button));
+}
+
+/****************************************************************************************/
+boolean PS2X::ButtonReleased(unsigned int button) {
+  return((NewButtonState(button)) & ((~last_buttons & button) > 0));
+}
+
+/****************************************************************************************/
+boolean PS2X::Button(uint16_t button) {
+  return ((~buttons & button) > 0);
+}
+
+/****************************************************************************************/
+unsigned int PS2X::ButtonDataByte() {
+   return (~buttons);
+}
+
+/****************************************************************************************/
+byte PS2X::Analog(byte button) {
+   return PS2data[button];
+}
+
+/****************************************************************************************/
+unsigned char PS2X::_gamepad_shiftinout (char byte) {
+   unsigned char tmp = 0;
+   for(unsigned char i=0;i<8;i++) {
+      if(CHK(byte,i)) CMD_SET();
+      else CMD_CLR();
+	  
+      CLK_CLR();
+      delayMicroseconds(CTRL_CLK);
+
+      //if(DAT_CHK()) SET(tmp,i);
+      if(DAT_CHK()) bitSet(tmp,i);
+
+      CLK_SET();
+#if CTRL_CLK_HIGH
+      delayMicroseconds(CTRL_CLK_HIGH);
+#endif
+   }
+   CMD_SET();
+   delayMicroseconds(CTRL_BYTE_DELAY);
+   return tmp;
+}
+
+/****************************************************************************************/
+void PS2X::read_gamepad() {
+   read_gamepad(false, 0x00);
+}
+
+/****************************************************************************************/
+boolean PS2X::read_gamepad(boolean motor1, byte motor2) {
+   double temp = millis() - last_read;
+
+   if (temp > 1500) //waited to long
+      reconfig_gamepad();
+
+   if(temp < read_delay)  //waited too short
+      delay(read_delay - temp);
+
+   if(motor2 != 0x00)
+      motor2 = map(motor2,0,255,0x40,0xFF); //noting below 40 will make it spin
+
+   char dword[9] = {0x01,0x42,0,motor1,motor2,0,0,0,0};
+   byte dword2[12] = {0,0,0,0,0,0,0,0,0,0,0,0};
+
+   // Try a few times to get valid data...
+   for (byte RetryCnt = 0; RetryCnt < 5; RetryCnt++) {
+      CMD_SET();
+      CLK_SET();
+      ATT_CLR(); // low enable joystick
+
+      delayMicroseconds(CTRL_BYTE_DELAY);
+      //Send the command to send button and joystick data;
+      for (int i = 0; i<9; i++) {
+         PS2data[i] = _gamepad_shiftinout(dword[i]);
+      }
+
+      if(PS2data[1] == 0x79) {  //if controller is in full data return mode, get the rest of data
+         for (int i = 0; i<12; i++) {
+            PS2data[i+9] = _gamepad_shiftinout(dword2[i]);
+         }
+      }
+
+      ATT_SET(); // HI disable joystick
+      // Check to see if we received valid data or not.  
+	  // We should be in analog mode for our data to be valid (analog == 0x7_)
+      if ((PS2data[1] & 0xf0) == 0x70)
+         break;
+
+      // If we got to here, we are not in analog mode, try to recover...
+      reconfig_gamepad(); // try to get back into Analog mode.
+      delay(read_delay);
+   }
+
+   // If we get here and still not in analog mode (=0x7_), try increasing the read_delay...
+   if ((PS2data[1] & 0xf0) != 0x70) {
+      if (read_delay < 10)
+         read_delay++;   // see if this helps out...
+   }
+
+#ifdef PS2X_COM_DEBUG
+   Serial.println("OUT:IN");
+   for(int i=0; i<9; i++){
+      Serial.print(dword[i], HEX);
+      Serial.print(":");
+      Serial.print(PS2data[i], HEX);
+      Serial.print(" ");
+   }
+   for (int i = 0; i<12; i++) {
+      Serial.print(dword2[i], HEX);
+      Serial.print(":");
+      Serial.print(PS2data[i+9], HEX);
+      Serial.print(" ");
+   }
+   Serial.println("");
+#endif
+
+   last_buttons = buttons; //store the previous buttons states
+
+#if defined(__AVR__)
+   buttons = *(uint16_t*)(PS2data+3);   //store as one value for multiple functions
+#else
+   buttons =  (uint16_t)(PS2data[4] << 8) + PS2data[3];   //store as one value for multiple functions
+#endif
+   last_read = millis();
+   return ((PS2data[1] & 0xf0) == 0x70);  // 1 = OK = analog mode - 0 = NOK
+}
+
+/****************************************************************************************/
+byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat) {
+   return config_gamepad(clk, cmd, att, dat, false, false);
+}
+
+/****************************************************************************************/
+byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat, bool pressures, bool rumble) {
+
+  byte temp[sizeof(type_read)];
+
+#ifdef __AVR__
+  _clk_mask = digitalPinToBitMask(clk);
+  _clk_oreg = portOutputRegister(digitalPinToPort(clk));
+  _cmd_mask = digitalPinToBitMask(cmd);
+  _cmd_oreg = portOutputRegister(digitalPinToPort(cmd));
+  _att_mask = digitalPinToBitMask(att);
+  _att_oreg = portOutputRegister(digitalPinToPort(att));
+  _dat_mask = digitalPinToBitMask(dat);
+  _dat_ireg = portInputRegister(digitalPinToPort(dat));
+#else
+  uint32_t            lport;                   // Port number for this pin
+  _clk_mask = digitalPinToBitMask(clk);
+  lport = digitalPinToPort(clk);
+  _clk_lport_set = portOutputRegister(lport) + 2;
+  _clk_lport_clr = portOutputRegister(lport) + 1;
+
+  _cmd_mask = digitalPinToBitMask(cmd);
+  lport = digitalPinToPort(cmd);
+  _cmd_lport_set = portOutputRegister(lport) + 2;
+  _cmd_lport_clr = portOutputRegister(lport) + 1;
+
+  _att_mask = digitalPinToBitMask(att);
+  lport = digitalPinToPort(att);
+  _att_lport_set = portOutputRegister(lport) + 2;
+  _att_lport_clr = portOutputRegister(lport) + 1;
+
+  _dat_mask = digitalPinToBitMask(dat);
+  _dat_lport = portInputRegister(digitalPinToPort(dat));
+#endif
+
+  pinMode(clk, OUTPUT); //configure ports
+  pinMode(att, OUTPUT);
+  pinMode(cmd, OUTPUT);
+  pinMode(dat, INPUT);
+
+#if defined(__AVR__)
+  digitalWrite(dat, HIGH); //enable pull-up
+#endif
+
+  CMD_SET(); // SET(*_cmd_oreg,_cmd_mask);
+  CLK_SET();
+
+  //new error checking. First, read gamepad a few times to see if it's talking
+  read_gamepad();
+  read_gamepad();
+
+  //see if it talked - see if mode came back. 
+  //If still anything but 41, 73 or 79, then it's not talking
+  if(PS2data[1] != 0x41 && PS2data[1] != 0x73 && PS2data[1] != 0x79){ 
+#ifdef PS2X_DEBUG
+    Serial.println("Controller mode not matched or no controller found");
+    Serial.print("Expected 0x41, 0x73 or 0x79, but got ");
+    Serial.println(PS2data[1], HEX);
+#endif
+    return 1; //return error code 1
+  }
+
+  //try setting mode, increasing delays if need be.
+  read_delay = 1;
+
+  for(int y = 0; y <= 10; y++) {
+    sendCommandString(enter_config, sizeof(enter_config)); //start config run
+
+    //read type
+    delayMicroseconds(CTRL_BYTE_DELAY);
+
+    CMD_SET();
+    CLK_SET();
+    ATT_CLR(); // low enable joystick
+
+    delayMicroseconds(CTRL_BYTE_DELAY);
+
+    for (int i = 0; i<9; i++) {
+      temp[i] = _gamepad_shiftinout(type_read[i]);
+    }
+
+    ATT_SET(); // HI disable joystick
+
+    controller_type = temp[3];
+
+    sendCommandString(set_mode, sizeof(set_mode));
+    if(rumble){ sendCommandString(enable_rumble, sizeof(enable_rumble)); en_Rumble = true; }
+    if(pressures){ sendCommandString(set_bytes_large, sizeof(set_bytes_large)); en_Pressures = true; }
+    sendCommandString(exit_config, sizeof(exit_config));
+
+    read_gamepad();
+
+    if(pressures){
+      if(PS2data[1] == 0x79)
+        break;
+      if(PS2data[1] == 0x73)
+        return 3;
+    }
+
+    if(PS2data[1] == 0x73)
+      break;
+
+    if(y == 10){
+#ifdef PS2X_DEBUG
+      Serial.println("Controller not accepting commands");
+      Serial.print("mode stil set at");
+      Serial.println(PS2data[1], HEX);
+#endif
+      return 2; //exit function with error
+    }
+    read_delay += 1; //add 1ms to read_delay
+  }
+  return 0; //no error if here
+}
+
+/****************************************************************************************/
+void PS2X::sendCommandString(byte string[], byte len) {
+#ifdef PS2X_COM_DEBUG
+  byte temp[len];
+  ATT_CLR(); // low enable joystick
+  delayMicroseconds(CTRL_BYTE_DELAY);
+
+  for (int y=0; y < len; y++)
+    temp[y] = _gamepad_shiftinout(string[y]);
+
+  ATT_SET(); //high disable joystick
+  delay(read_delay); //wait a few
+
+  Serial.println("OUT:IN Configure");
+  for(int i=0; i<len; i++) {
+    Serial.print(string[i], HEX);
+    Serial.print(":");
+    Serial.print(temp[i], HEX);
+    Serial.print(" ");
+  }
+  Serial.println("");
+#else
+  ATT_CLR(); // low enable joystick
+  for (int y=0; y < len; y++)
+    _gamepad_shiftinout(string[y]);
+  ATT_SET(); //high disable joystick
+  delay(read_delay);                  //wait a few
+#endif
+}
+
+/****************************************************************************************/
+byte PS2X::readType() {
+/*
+  byte temp[sizeof(type_read)];
+
+  sendCommandString(enter_config, sizeof(enter_config));
+
+  delayMicroseconds(CTRL_BYTE_DELAY);
+
+  CMD_SET();
+  CLK_SET();
+  ATT_CLR(); // low enable joystick
+
+  delayMicroseconds(CTRL_BYTE_DELAY);
+
+  for (int i = 0; i<9; i++) {
+    temp[i] = _gamepad_shiftinout(type_read[i]);
+  }
+
+  sendCommandString(exit_config, sizeof(exit_config));
+
+  if(temp[3] == 0x03)
+    return 1;
+  else if(temp[3] == 0x01)
+    return 2;
+
+  return 0;
+*/
+
+  if(controller_type == 0x03)
+    return 1;
+  else if(controller_type == 0x01)
+    return 2;
+  else if(controller_type == 0x0C)  
+    return 3;  //2.4G Wireless Dual Shock PS2 Game Controller
+	
+  return 0;
+}
+
+/****************************************************************************************/
+void PS2X::enableRumble() {
+  sendCommandString(enter_config, sizeof(enter_config));
+  sendCommandString(enable_rumble, sizeof(enable_rumble));
+  sendCommandString(exit_config, sizeof(exit_config));
+  en_Rumble = true;
+}
+
+/****************************************************************************************/
+bool PS2X::enablePressures() {
+  sendCommandString(enter_config, sizeof(enter_config));
+  sendCommandString(set_bytes_large, sizeof(set_bytes_large));
+  sendCommandString(exit_config, sizeof(exit_config));
+
+  read_gamepad();
+  read_gamepad();
+
+  if(PS2data[1] != 0x79)
+    return false;
+
+  en_Pressures = true;
+    return true;
+}
+
+/****************************************************************************************/
+void PS2X::reconfig_gamepad(){
+  sendCommandString(enter_config, sizeof(enter_config));
+  sendCommandString(set_mode, sizeof(set_mode));
+  if (en_Rumble)
+    sendCommandString(enable_rumble, sizeof(enable_rumble));
+  if (en_Pressures)
+    sendCommandString(set_bytes_large, sizeof(set_bytes_large));
+  sendCommandString(exit_config, sizeof(exit_config));
+}
+
+/****************************************************************************************/
+#ifdef __AVR__
+inline void  PS2X::CLK_SET(void) {
+  register uint8_t old_sreg = SREG;
+  cli();
+  *_clk_oreg |= _clk_mask;
+  SREG = old_sreg;
+}
+
+inline void  PS2X::CLK_CLR(void) {
+  register uint8_t old_sreg = SREG;
+  cli();
+  *_clk_oreg &= ~_clk_mask;
+  SREG = old_sreg;
+}
+
+inline void  PS2X::CMD_SET(void) {
+  register uint8_t old_sreg = SREG;
+  cli();
+  *_cmd_oreg |= _cmd_mask; // SET(*_cmd_oreg,_cmd_mask);
+  SREG = old_sreg;
+}
+
+inline void  PS2X::CMD_CLR(void) {
+  register uint8_t old_sreg = SREG;
+  cli();
+  *_cmd_oreg &= ~_cmd_mask; // SET(*_cmd_oreg,_cmd_mask);
+  SREG = old_sreg;
+}
+
+inline void  PS2X::ATT_SET(void) {
+  register uint8_t old_sreg = SREG;
+  cli();
+  *_att_oreg |= _att_mask ;
+  SREG = old_sreg;
+}
+
+inline void PS2X::ATT_CLR(void) {
+  register uint8_t old_sreg = SREG;
+  cli();
+  *_att_oreg &= ~_att_mask;
+  SREG = old_sreg;
+}
+
+inline bool PS2X::DAT_CHK(void) {
+  return (*_dat_ireg & _dat_mask) ? true : false;
+}
+
+#else
+// On pic32, use the set/clr registers to make them atomic...
+inline void  PS2X::CLK_SET(void) {
+  *_clk_lport_set |= _clk_mask;
+}
+
+inline void  PS2X::CLK_CLR(void) {
+  *_clk_lport_clr |= _clk_mask;
+}
+
+inline void  PS2X::CMD_SET(void) {
+  *_cmd_lport_set |= _cmd_mask;
+}
+
+inline void  PS2X::CMD_CLR(void) {
+  *_cmd_lport_clr |= _cmd_mask;
+}
+
+inline void  PS2X::ATT_SET(void) {
+  *_att_lport_set |= _att_mask;
+}
+
+inline void PS2X::ATT_CLR(void) {
+  *_att_lport_clr |= _att_mask;
+}
+
+inline bool PS2X::DAT_CHK(void) {
+  return (*_dat_lport & _dat_mask) ? true : false;
+}
+
+#endif
@@ -0,0 +1,236 @@
+/******************************************************************
+*  Super amazing PS2 controller Arduino Library v1.8
+*		details and example sketch: 
+*			http://www.billporter.info/?p=240
+*
+*    Original code by Shutter on Arduino Forums
+*
+*    Revamped, made into lib by and supporting continued development:
+*              Bill Porter
+*              www.billporter.info
+*
+*	 Contributers:
+*		Eric Wetzel (thewetzel@gmail.com)
+*		Kurt Eckhardt
+*
+*  Lib version history
+*    0.1 made into library, added analog stick support. 
+*    0.2 fixed config_gamepad miss-spelling
+*        added new functions:
+*          NewButtonState();
+*          NewButtonState(unsigned int);
+*          ButtonPressed(unsigned int);
+*          ButtonReleased(unsigned int);
+*        removed 'PS' from begining of ever function
+*    1.0 found and fixed bug that wasn't configuring controller
+*        added ability to define pins
+*        added time checking to reconfigure controller if not polled enough
+*        Analog sticks and pressures all through 'ps2x.Analog()' function
+*        added:
+*          enableRumble();
+*          enablePressures();
+*    1.1  
+*        added some debug stuff for end user. Reports if no controller found
+*        added auto-increasing sentence delay to see if it helps compatibility.
+*    1.2
+*        found bad math by Shutter for original clock. Was running at 50kHz, not the required 500kHz. 
+*        fixed some of the debug reporting. 
+*	1.3 
+*	    Changed clock back to 50kHz. CuriousInventor says it's suppose to be 500kHz, but doesn't seem to work for everybody. 
+*	1.4
+*		Removed redundant functions.
+*		Fixed mode check to include two other possible modes the controller could be in.
+*       Added debug code enabled by compiler directives. See below to enable debug mode.
+*		Added button definitions for shapes as well as colors.
+*	1.41
+*		Some simple bug fixes
+*		Added Keywords.txt file
+*	1.5
+*		Added proper Guitar Hero compatibility
+*		Fixed issue with DEBUG mode, had to send serial at once instead of in bits
+*	1.6
+*		Changed config_gamepad() call to include rumble and pressures options
+*			This was to fix controllers that will only go into config mode once
+*			Old methods should still work for backwards compatibility 
+*    1.7
+*		Integrated Kurt's fixes for the interrupts messing with servo signals
+*		Reorganized directory so examples show up in Arduino IDE menu
+*    1.8
+*		Added Arduino 1.0 compatibility. 
+*    1.9
+*       Kurt - Added detection and recovery from dropping from analog mode, plus
+*       integreated Chipkit (pic32mx...) support
+*
+*
+*
+*This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or(at your option) any later version.
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+<http://www.gnu.org/licenses/>
+*  
+******************************************************************/
+
+// $$$$$$$$$$$$ DEBUG ENABLE SECTION $$$$$$$$$$$$$$$$
+// to debug ps2 controller, uncomment these two lines to print out debug to uart
+//#define PS2X_DEBUG
+//#define PS2X_COM_DEBUG
+
+#ifndef PS2X_lib_h
+  #define PS2X_lib_h
+
+#if ARDUINO > 22
+  #include "Arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+
+#include <math.h>
+#include <stdio.h>
+#include <stdint.h>
+#ifdef __AVR__
+  // AVR
+  #include <avr/io.h>
+  #define CTRL_CLK        4
+  #define CTRL_BYTE_DELAY 3
+#else
+  // Pic32...
+  #include <pins_arduino.h>
+  #define CTRL_CLK        5
+  #define CTRL_CLK_HIGH   5
+  #define CTRL_BYTE_DELAY 4
+#endif 
+
+//These are our button constants
+#define PSB_SELECT      0x0001
+#define PSB_L3          0x0002
+#define PSB_R3          0x0004
+#define PSB_START       0x0008
+#define PSB_PAD_UP      0x0010
+#define PSB_PAD_RIGHT   0x0020
+#define PSB_PAD_DOWN    0x0040
+#define PSB_PAD_LEFT    0x0080
+#define PSB_L2          0x0100
+#define PSB_R2          0x0200
+#define PSB_L1          0x0400
+#define PSB_R1          0x0800
+#define PSB_GREEN       0x1000
+#define PSB_RED         0x2000
+#define PSB_BLUE        0x4000
+#define PSB_PINK        0x8000
+#define PSB_TRIANGLE    0x1000
+#define PSB_CIRCLE      0x2000
+#define PSB_CROSS       0x4000
+#define PSB_SQUARE      0x8000
+
+//Guitar  button constants
+#define UP_STRUM		0x0010
+#define DOWN_STRUM		0x0040
+#define STAR_POWER		0x0100
+#define GREEN_FRET		0x0200
+#define YELLOW_FRET		0x1000
+#define RED_FRET		0x2000
+#define BLUE_FRET		0x4000
+#define ORANGE_FRET		0x8000
+#define WHAMMY_BAR		8
+
+//These are stick values
+#define PSS_RX 5
+#define PSS_RY 6
+#define PSS_LX 7
+#define PSS_LY 8
+
+//These are analog buttons
+#define PSAB_PAD_RIGHT    9
+#define PSAB_PAD_UP      11
+#define PSAB_PAD_DOWN    12
+#define PSAB_PAD_LEFT    10
+#define PSAB_L2          19
+#define PSAB_R2          20
+#define PSAB_L1          17
+#define PSAB_R1          18
+#define PSAB_GREEN       13
+#define PSAB_RED         14
+#define PSAB_BLUE        15
+#define PSAB_PINK        16
+#define PSAB_TRIANGLE    13
+#define PSAB_CIRCLE      14
+#define PSAB_CROSS       15
+#define PSAB_SQUARE      16
+
+#define SET(x,y) (x|=(1<<y))
+#define CLR(x,y) (x&=(~(1<<y)))
+#define CHK(x,y) (x & (1<<y))
+#define TOG(x,y) (x^=(1<<y))
+
+class PS2X {
+  public:
+    boolean Button(uint16_t);                //will be TRUE if button is being pressed
+    unsigned int ButtonDataByte();
+    boolean NewButtonState();
+    boolean NewButtonState(unsigned int);    //will be TRUE if button was JUST pressed OR released
+    boolean ButtonPressed(unsigned int);     //will be TRUE if button was JUST pressed
+    boolean ButtonReleased(unsigned int);    //will be TRUE if button was JUST released
+    void read_gamepad();
+    boolean  read_gamepad(boolean, byte);
+    byte readType();
+    byte config_gamepad(uint8_t, uint8_t, uint8_t, uint8_t);
+    byte config_gamepad(uint8_t, uint8_t, uint8_t, uint8_t, bool, bool);
+    void enableRumble();
+    bool enablePressures();
+    byte Analog(byte);
+    void reconfig_gamepad();
+
+  private:
+    inline void CLK_SET(void);
+    inline void CLK_CLR(void);
+    inline void CMD_SET(void);
+    inline void CMD_CLR(void);
+    inline void ATT_SET(void);
+    inline void ATT_CLR(void);
+    inline bool DAT_CHK(void);
+    
+    unsigned char _gamepad_shiftinout (char);
+    unsigned char PS2data[21];
+    void sendCommandString(byte*, byte);
+    unsigned char i;
+    unsigned int last_buttons;
+    unsigned int buttons;
+	
+    #ifdef __AVR__
+      uint8_t maskToBitNum(uint8_t);
+      uint8_t _clk_mask; 
+      volatile uint8_t *_clk_oreg;
+      uint8_t _cmd_mask; 
+      volatile uint8_t *_cmd_oreg;
+      uint8_t _att_mask; 
+      volatile uint8_t *_att_oreg;
+      uint8_t _dat_mask; 
+      volatile uint8_t *_dat_ireg;
+    #else
+      uint8_t maskToBitNum(uint8_t);
+      uint16_t _clk_mask; 
+      volatile uint32_t *_clk_lport_set;
+      volatile uint32_t *_clk_lport_clr;
+      uint16_t _cmd_mask; 
+      volatile uint32_t *_cmd_lport_set;
+      volatile uint32_t *_cmd_lport_clr;
+      uint16_t _att_mask; 
+      volatile uint32_t *_att_lport_set;
+      volatile uint32_t *_att_lport_clr;
+      uint16_t _dat_mask; 
+      volatile uint32_t *_dat_lport;
+    #endif
+	
+    unsigned long last_read;
+    byte read_delay;
+    byte controller_type;
+    boolean en_Rumble;
+    boolean en_Pressures;
+};
+
+#endif
+
+
+
@@ -0,0 +1,150 @@
+#include <PS2X_lib.h>
+
+PS2X ps2x; // create PS2 Controller Class
+
+int error = 0; 
+byte type = 0;
+const int ledPin = 11;         // Mouse control LED (11 on Teensy 2.0, 13 on Arduino Leonardo)
+
+// parameters for reading the joystick:
+int range = 12;               // output range of X or Y movement
+int responseDelay = 5;        // response delay of the mouse, in ms
+int threshold = range/4;      // resting threshold
+int center = range/2;         // resting position value
+
+boolean mouseIsActive = false;    // whether or not to control the mouse
+int lastSwitchState = LOW;        // previous switch state
+
+void setup(){
+ Serial.begin(57600);
+  
+ error = ps2x.config_gamepad(15,14,13,12, true, true);   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
+ 
+ if(error == 0){
+   Serial.println("Found Controller, configured successful");
+ }
+   
+  else if(error == 1)
+   Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
+   
+  else if(error == 2)
+   Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
+   
+  else if(error == 3)
+   Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
+   
+   type = ps2x.readType(); 
+     switch(type) {
+       case 0:
+        Serial.println("Unknown Controller type");
+       break;
+       case 1:
+        Serial.println("DualShock Controller Found");
+       break;
+       case 2:
+         Serial.println("GuitarHero Controller Found");
+       break;
+     }
+  
+ // take control of the mouse:
+  Mouse.begin();
+  Keyboard.begin();
+}
+
+void loop()
+{
+   
+ if(error == 1) //skip loop if no controller found
+  return; 
+  
+  ps2x.read_gamepad(false, 0);          //read controller and set large motor to spin at 'vibrate' speed
+    
+  // read the switch:
+  int switchState = ps2x.ButtonPressed(PSB_RED);
+  // if it's changed and it's high, toggle the mouse state:
+  if (switchState != lastSwitchState) {
+    if (switchState == HIGH) {
+      mouseIsActive = !mouseIsActive;
+      // turn on LED to indicate mouse state:
+      digitalWrite(ledPin, mouseIsActive);
+    } 
+  }
+  // save switch state for next comparison:
+  lastSwitchState = switchState;
+
+  // read and scale the two axes:
+  int xReading = readAxis(PSS_LX);
+  int yReading = readAxis(PSS_LY);
+
+  // if the mouse control state is active, move the mouse:
+  if (mouseIsActive) {
+    Mouse.move(xReading, yReading, 0);
+  }  
+
+  // read the mouse button and click or not click:
+  // if the mouse button is pressed:
+  if (ps2x.ButtonPressed(PSB_BLUE)) {
+    // if the mouse is not pressed, press it:
+    if (!Mouse.isPressed(MOUSE_LEFT)) {
+      Mouse.press(MOUSE_LEFT); 
+    }
+  } 
+  // else the mouse button is not pressed:
+  else {
+    // if the mouse is pressed, release it:
+    if (Mouse.isPressed(MOUSE_LEFT)) {
+      Mouse.release(MOUSE_LEFT); 
+    }
+  }
+  
+  if (ps2x.Button(PSB_PAD_UP)) {
+    Keyboard.press(KEY_UP_ARROW); 
+  } else {
+    Keyboard.release(KEY_UP_ARROW); 
+  }
+  
+  if (ps2x.Button(PSB_PAD_DOWN)) {
+    Keyboard.press(KEY_DOWN_ARROW); 
+  } else {
+    Keyboard.release(KEY_DOWN_ARROW); 
+  }
+  
+  if (ps2x.Button(PSB_PAD_RIGHT)) {
+    Keyboard.press(KEY_RIGHT_ARROW); 
+  } else {
+    Keyboard.release(KEY_RIGHT_ARROW); 
+  }
+  
+  if (ps2x.Button(PSB_PAD_LEFT)) {
+    Keyboard.press(KEY_LEFT_ARROW); 
+  } else {
+    Keyboard.release(KEY_LEFT_ARROW); 
+  }
+ 
+ delay(5);
+     
+}
+
+/*
+  reads an axis (0 or 1 for x or y) and scales the 
+ analog input range to a range from 0 to <range>
+ */
+
+int readAxis(int thisAxis) { 
+  // read the analog input:
+  int reading = ps2x.Analog(thisAxis);
+
+  // map the reading from the analog input range to the output range:
+  reading = map(reading, 0, 255, 0, range);
+
+  // if the output reading is outside from the
+  // rest position threshold,  use it:
+  int distance = reading - center;
+
+  if (abs(distance) < threshold) {
+    distance = 0;
+  } 
+
+  // return the distance for this axis:
+  return distance;
+}
@@ -0,0 +1,189 @@
+#include <PS2X_lib.h>  //for v1.6
+
+/******************************************************************
+ * set pins connected to PS2 controller:
+ *   - 1e column: original 
+ *   - 2e colmun: Stef?
+ * replace pin numbers by the ones you use
+ ******************************************************************/
+#define PS2_DAT        13  //14    
+#define PS2_CMD        11  //15
+#define PS2_SEL        10  //16
+#define PS2_CLK        12  //17
+
+/******************************************************************
+ * select modes of PS2 controller:
+ *   - pressures = analog reading of push-butttons 
+ *   - rumble    = motor rumbling
+ * uncomment 1 of the lines for each mode selection
+ ******************************************************************/
+//#define pressures   true
+#define pressures   false
+//#define rumble      true
+#define rumble      false
+
+PS2X ps2x; // create PS2 Controller Class
+
+//right now, the library does NOT support hot pluggable controllers, meaning 
+//you must always either restart your Arduino after you connect the controller, 
+//or call config_gamepad(pins) again after connecting the controller.
+
+int error = 0;
+byte type = 0;
+byte vibrate = 0;
+
+void setup(){
+ 
+  Serial.begin(57600);
+  
+  delay(300);  //added delay to give wireless ps2 module some time to startup, before configuring it
+   
+  //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
+  
+  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
+  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
+  
+  if(error == 0){
+    Serial.print("Found Controller, configured successful ");
+    Serial.print("pressures = ");
+	if (pressures)
+	  Serial.println("true ");
+	else
+	  Serial.println("false");
+	Serial.print("rumble = ");
+	if (rumble)
+	  Serial.println("true)");
+	else
+	  Serial.println("false");
+    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
+    Serial.println("holding L1 or R1 will print out the analog stick values.");
+    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
+  }  
+  else if(error == 1)
+    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
+   
+  else if(error == 2)
+    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
+
+  else if(error == 3)
+    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
+  
+//  Serial.print(ps2x.Analog(1), HEX);
+  
+  type = ps2x.readType(); 
+  switch(type) {
+    case 0:
+      Serial.print("Unknown Controller type found ");
+      break;
+    case 1:
+      Serial.print("DualShock Controller found ");
+      break;
+    case 2:
+      Serial.print("GuitarHero Controller found ");
+      break;
+	case 3:
+      Serial.print("Wireless Sony DualShock Controller found ");
+      break;
+   }
+}
+
+void loop() {
+  /* You must Read Gamepad to get new values and set vibration values
+     ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
+     if you don't enable the rumble, use ps2x.read_gamepad(); with no values
+     You should call this at least once a second
+   */  
+  if(error == 1) //skip loop if no controller found
+    return; 
+  
+  if(type == 2){ //Guitar Hero Controller
+    ps2x.read_gamepad();          //read controller 
+   
+    if(ps2x.ButtonPressed(GREEN_FRET))
+      Serial.println("Green Fret Pressed");
+    if(ps2x.ButtonPressed(RED_FRET))
+      Serial.println("Red Fret Pressed");
+    if(ps2x.ButtonPressed(YELLOW_FRET))
+      Serial.println("Yellow Fret Pressed");
+    if(ps2x.ButtonPressed(BLUE_FRET))
+      Serial.println("Blue Fret Pressed");
+    if(ps2x.ButtonPressed(ORANGE_FRET))
+      Serial.println("Orange Fret Pressed"); 
+
+    if(ps2x.ButtonPressed(STAR_POWER))
+      Serial.println("Star Power Command");
+    
+    if(ps2x.Button(UP_STRUM))          //will be TRUE as long as button is pressed
+      Serial.println("Up Strum");
+    if(ps2x.Button(DOWN_STRUM))
+      Serial.println("DOWN Strum");
+ 
+    if(ps2x.Button(PSB_START))         //will be TRUE as long as button is pressed
+      Serial.println("Start is being held");
+    if(ps2x.Button(PSB_SELECT))
+      Serial.println("Select is being held");
+    
+    if(ps2x.Button(ORANGE_FRET)) {     // print stick value IF TRUE
+      Serial.print("Wammy Bar Position:");
+      Serial.println(ps2x.Analog(WHAMMY_BAR), DEC); 
+    } 
+  }
+  else { //DualShock Controller
+    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
+    
+    if(ps2x.Button(PSB_START))         //will be TRUE as long as button is pressed
+      Serial.println("Start is being held");
+    if(ps2x.Button(PSB_SELECT))
+      Serial.println("Select is being held");      
+
+    if(ps2x.Button(PSB_PAD_UP)) {      //will be TRUE as long as button is pressed
+      Serial.print("Up held this hard: ");
+      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_RIGHT)){
+      Serial.print("Right held this hard: ");
+      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_LEFT)){
+      Serial.print("LEFT held this hard: ");
+      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
+    }
+    if(ps2x.Button(PSB_PAD_DOWN)){
+      Serial.print("DOWN held this hard: ");
+      Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
+    }   
+
+    vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
+    if (ps2x.NewButtonState()) {        //will be TRUE if any button changes state (on to off, or off to on)
+      if(ps2x.Button(PSB_L3))
+        Serial.println("L3 pressed");
+      if(ps2x.Button(PSB_R3))
+        Serial.println("R3 pressed");
+      if(ps2x.Button(PSB_L2))
+        Serial.println("L2 pressed");
+      if(ps2x.Button(PSB_R2))
+        Serial.println("R2 pressed");
+      if(ps2x.Button(PSB_TRIANGLE))
+        Serial.println("Triangle pressed");        
+    }
+
+    if(ps2x.ButtonPressed(PSB_CIRCLE))               //will be TRUE if button was JUST pressed
+      Serial.println("Circle just pressed");
+    if(ps2x.NewButtonState(PSB_CROSS))               //will be TRUE if button was JUST pressed OR released
+      Serial.println("X just changed");
+    if(ps2x.ButtonReleased(PSB_SQUARE))              //will be TRUE if button was JUST released
+      Serial.println("Square just released");     
+
+    if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE
+      Serial.print("Stick Values:");
+      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX  
+      Serial.print(",");
+      Serial.print(ps2x.Analog(PSS_LX), DEC); 
+      Serial.print(",");
+      Serial.print(ps2x.Analog(PSS_RY), DEC); 
+      Serial.print(",");
+      Serial.println(ps2x.Analog(PSS_RX), DEC); 
+    }     
+  }
+  delay(50);  
+}
@@ -0,0 +1,78 @@
+#######################################
+# Syntax Coloring Map PS2X
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+PS2X	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+Button	KEYWORD2
+ButtonDataByte	KEYWORD2
+NewButtonState	KEYWORD2
+ButtonPressed	KEYWORD2
+ButtonReleased	KEYWORD2
+read_gamepad	KEYWORD2
+config_gamepad	KEYWORD2
+enableRumble	KEYWORD2
+enablePressures	KEYWORD2
+Analog	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+PSB_SELECT	LITERAL1
+PSB_L3	LITERAL1
+PSB_R3	LITERAL1
+PSB_START	LITERAL1
+PSB_PAD_UP	LITERAL1
+PSB_PAD_RIGHT	LITERAL1
+PSB_PAD_DOWN	LITERAL1
+PSB_PAD_LEFT	LITERAL1
+PSB_L2	LITERAL1
+PSB_R2	LITERAL1
+PSB_L1	LITERAL1
+PSB_R1	LITERAL1
+PSB_GREEN	LITERAL1
+PSB_RED	LITERAL1
+PSB_BLUE	LITERAL1
+PSB_PINK	LITERAL1
+PSB_TRIANGLE	LITERAL1
+PSB_CIRCLE	LITERAL1
+PSB_CROSS	LITERAL1
+PSB_SQUARE	LITERAL1
+PSS_RX	LITERAL1
+PSS_RY	LITERAL1
+PSS_LX	LITERAL1
+PSS_LY	LITERAL1
+
+PSAB_PAD_RIGHT	LITERAL1
+PSAB_PAD_UP	LITERAL1
+PSAB_PAD_DOWN	LITERAL1
+PSAB_PAD_LEFT	LITERAL1
+PSAB_L2	LITERAL1
+PSAB_R2	LITERAL1
+PSAB_L1	LITERAL1
+PSAB_R1	LITERAL1
+PSAB_GREEN	LITERAL1
+PSAB_RED	LITERAL1
+PSAB_BLUE	LITERAL1
+PSAB_PINK	LITERAL1
+PSAB_TRIANGLE	LITERAL1
+PSAB_CIRCLE	LITERAL1
+PSAB_CROSS	LITERAL1
+PSAB_SQUARE	LITERAL1
+
+GREEN_FRET LITERAL1
+RED_FRET LITERAL1
+YELLOW_FRET LITERAL1
+BLUE_FRET LITERAL1
+ORANGE_FRET LITERAL1
+STAR_POWER LITERAL1
+UP_STRUM LITERAL1
+DOWN_STRUM LITERAL1
+WHAMMY_BAR LITERAL1
@@ -0,0 +1,29 @@
+/* 
+ Controlling a servo position using a potentiometer (variable resistor) 
+ by Michal Rinott <http://people.interaction-ivrea.it/m.rinott> 
+
+ modified on 8 Nov 2013
+ by Scott Fitzgerald
+ http://arduino.cc/en/Tutorial/Knob
+*/
+
+#include <Servo.h>
+
+Servo myservo;  // create servo object to control a servo
+
+int potpin = 0;  // analog pin used to connect the potentiometer
+int val;    // variable to read the value from the analog pin
+
+void setup()
+{
+  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
+}
+
+void loop() 
+{ 
+  val = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023) 
+  val = map(val, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180) 
+  myservo.write(val);                  // sets the servo position according to the scaled value 
+  delay(15);                           // waits for the servo to get there 
+} 
+
@@ -0,0 +1,35 @@
+/* Sweep
+ by BARRAGAN <http://barraganstudio.com> 
+ This example code is in the public domain.
+
+ modified 8 Nov 2013
+ by Scott Fitzgerald
+ http://arduino.cc/en/Tutorial/Sweep
+*/ 
+
+#include <Servo.h> 
+ 
+Servo myservo;  // create servo object to control a servo 
+                // twelve servo objects can be created on most boards
+ 
+int pos = 0;    // variable to store the servo position 
+ 
+void setup() 
+{ 
+  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 
+} 
+ 
+void loop() 
+{ 
+  for(pos = 0; pos <= 180; pos += 1) // goes from 0 degrees to 180 degrees 
+  {                                  // in steps of 1 degree 
+    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
+    delay(15);                       // waits 15ms for the servo to reach the position 
+  } 
+  for(pos = 180; pos>=0; pos-=1)     // goes from 180 degrees to 0 degrees 
+  {                                
+    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
+    delay(15);                       // waits 15ms for the servo to reach the position 
+  } 
+} 
+
@@ -0,0 +1,24 @@
+#######################################
+# Syntax Coloring Map Servo
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+Servo	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+attach	KEYWORD2
+detach	KEYWORD2
+write	KEYWORD2
+read	KEYWORD2
+attached	KEYWORD2
+writeMicroseconds	KEYWORD2
+readMicroseconds	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
@@ -0,0 +1,24 @@
+#######################################
+# Syntax Coloring Map Servo
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+Servo	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+attach	KEYWORD2
+detach	KEYWORD2
+write	KEYWORD2
+read	KEYWORD2
+attached	KEYWORD2
+writeMicroseconds	KEYWORD2
+readMicroseconds	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
@@ -0,0 +1,8 @@
+name=Servo
+version=1.0
+author=Michael Margolis, Arduino
+maintainer=Arduino <info@arduino.cc>
+sentence=Controls a lot of Servos.
+paragraph=This library can control a great number of servos.<br />It makes careful use of timers: the library can control 12 servos using only 1 timer.<br />On the Arduino Due you can control up to 60 servos.<br />
+url=http://arduino.cc/en/Reference/Servo
+architectures=avr,sam
@@ -0,0 +1,8 @@
+name=Servo
+version=1.0
+author=Michael Margolis, Arduino
+maintainer=Arduino <info@arduino.cc>
+sentence=Controls a lot of Servos.
+paragraph=This library can control a great number of servos.<br />It makes careful use of timers: the library can control 12 servos using only 1 timer.<br />On the Arduino Due you can control up to 60 servos.<br />
+url=http://arduino.cc/en/Reference/Servo
+architectures=avr,sam
@@ -0,0 +1,110 @@
+/*
+  Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+  Copyright (c) 2009 Michael Margolis.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+/* 
+  A servo is activated by creating an instance of the Servo class passing 
+  the desired pin to the attach() method.
+  The servos are pulsed in the background using the value most recently 
+  written using the write() method.
+
+  Note that analogWrite of PWM on pins associated with the timer are 
+  disabled when the first servo is attached.
+  Timers are seized as needed in groups of 12 servos - 24 servos use two 
+  timers, 48 servos will use four.
+  The sequence used to sieze timers is defined in timers.h
+
+  The methods are:
+
+    Servo - Class for manipulating servo motors connected to Arduino pins.
+
+    attach(pin )  - Attaches a servo motor to an i/o pin.
+    attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
+    default min is 544, max is 2400  
+ 
+    write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
+    writeMicroseconds() - Sets the servo pulse width in microseconds 
+    read()      - Gets the last written servo pulse width as an angle between 0 and 180. 
+    readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
+    attached()  - Returns true if there is a servo attached. 
+    detach()    - Stops an attached servos from pulsing its i/o pin. 
+ */
+
+#ifndef Servo_h
+#define Servo_h
+
+#include <inttypes.h>
+
+/* 
+ * Defines for 16 bit timers used with  Servo library 
+ *
+ * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
+ * timer16_Sequence_t enumerates the sequence that the timers should be allocated
+ * _Nbr_16timers indicates how many 16 bit timers are available.
+ */
+
+// Architecture specific include
+#if defined(ARDUINO_ARCH_AVR)
+#include "avr/ServoTimers.h"
+#elif defined(ARDUINO_ARCH_SAM)
+#include "sam/ServoTimers.h"
+#else
+#error "This library only supports boards with an AVR or SAM processor."
+#endif
+
+#define Servo_VERSION           2     // software version of this library
+
+#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo  
+#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo 
+#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
+#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds 
+
+#define SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one timer 
+#define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
+
+#define INVALID_SERVO         255     // flag indicating an invalid servo index
+
+typedef struct  {
+  uint8_t nbr        :6 ;             // a pin number from 0 to 63
+  uint8_t isActive   :1 ;             // true if this channel is enabled, pin not pulsed if false 
+} ServoPin_t   ;  
+
+typedef struct {
+  ServoPin_t Pin;
+  volatile unsigned int ticks;
+} servo_t;
+
+class Servo
+{
+public:
+  Servo();
+  uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
+  uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 
+  void detach();
+  void write(int value);             // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 
+  void writeMicroseconds(int value); // Write pulse width in microseconds 
+  int read();                        // returns current pulse width as an angle between 0 and 180 degrees
+  int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
+  bool attached();                   // return true if this servo is attached, otherwise false 
+private:
+   uint8_t servoIndex;               // index into the channel data for this servo
+   int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH    
+   int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH   
+};
+
+#endif
@@ -0,0 +1,317 @@
+/*
+ Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+ Copyright (c) 2009 Michael Margolis.  All right reserved.
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#if defined(ARDUINO_ARCH_AVR)
+
+#include <avr/interrupt.h>
+#include <Arduino.h>
+
+#include "Servo.h"
+
+#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
+#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
+
+
+#define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
+
+//#define NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)
+
+static servo_t servos[MAX_SERVOS];                          // static array of servo structures
+static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
+
+uint8_t ServoCount = 0;                                     // the total number of attached servos
+
+
+// convenience macros
+#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
+#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
+#define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
+#define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
+
+#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
+#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
+
+/************ static functions common to all instances ***********************/
+
+static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
+{
+  if( Channel[timer] < 0 )
+    *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
+  else{
+    if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
+      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
+  }
+
+  Channel[timer]++;    // increment to the next channel
+  if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
+    *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
+    if(SERVO(timer,Channel[timer]).Pin.isActive == true)     // check if activated
+      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
+  }
+  else {
+    // finished all channels so wait for the refresh period to expire before starting over
+    if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) )  // allow a few ticks to ensure the next OCR1A not missed
+      *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
+    else
+      *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed
+    Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
+  }
+}
+
+#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
+// Interrupt handlers for Arduino
+#if defined(_useTimer1)
+SIGNAL (TIMER1_COMPA_vect)
+{
+  handle_interrupts(_timer1, &TCNT1, &OCR1A);
+}
+#endif
+
+#if defined(_useTimer3)
+SIGNAL (TIMER3_COMPA_vect)
+{
+  handle_interrupts(_timer3, &TCNT3, &OCR3A);
+}
+#endif
+
+#if defined(_useTimer4)
+SIGNAL (TIMER4_COMPA_vect)
+{
+  handle_interrupts(_timer4, &TCNT4, &OCR4A);
+}
+#endif
+
+#if defined(_useTimer5)
+SIGNAL (TIMER5_COMPA_vect)
+{
+  handle_interrupts(_timer5, &TCNT5, &OCR5A);
+}
+#endif
+
+#elif defined WIRING
+// Interrupt handlers for Wiring
+#if defined(_useTimer1)
+void Timer1Service()
+{
+  handle_interrupts(_timer1, &TCNT1, &OCR1A);
+}
+#endif
+#if defined(_useTimer3)
+void Timer3Service()
+{
+  handle_interrupts(_timer3, &TCNT3, &OCR3A);
+}
+#endif
+#endif
+
+
+static void initISR(timer16_Sequence_t timer)
+{
+#if defined (_useTimer1)
+  if(timer == _timer1) {
+    TCCR1A = 0;             // normal counting mode
+    TCCR1B = _BV(CS11);     // set prescaler of 8
+    TCNT1 = 0;              // clear the timer count
+#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
+    TIFR |= _BV(OCF1A);      // clear any pending interrupts;
+    TIMSK |=  _BV(OCIE1A) ;  // enable the output compare interrupt
+#else
+    // here if not ATmega8 or ATmega128
+    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts;
+    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt
+#endif
+#if defined(WIRING)
+    timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
+#endif
+  }
+#endif
+
+#if defined (_useTimer3)
+  if(timer == _timer3) {
+    TCCR3A = 0;             // normal counting mode
+    TCCR3B = _BV(CS31);     // set prescaler of 8
+    TCNT3 = 0;              // clear the timer count
+#if defined(__AVR_ATmega128__)
+    TIFR |= _BV(OCF3A);     // clear any pending interrupts;
+	ETIMSK |= _BV(OCIE3A);  // enable the output compare interrupt
+#else
+    TIFR3 = _BV(OCF3A);     // clear any pending interrupts;
+    TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt
+#endif
+#if defined(WIRING)
+    timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service);  // for Wiring platform only
+#endif
+  }
+#endif
+
+#if defined (_useTimer4)
+  if(timer == _timer4) {
+    TCCR4A = 0;             // normal counting mode
+    TCCR4B = _BV(CS41);     // set prescaler of 8
+    TCNT4 = 0;              // clear the timer count
+    TIFR4 = _BV(OCF4A);     // clear any pending interrupts;
+    TIMSK4 =  _BV(OCIE4A) ; // enable the output compare interrupt
+  }
+#endif
+
+#if defined (_useTimer5)
+  if(timer == _timer5) {
+    TCCR5A = 0;             // normal counting mode
+    TCCR5B = _BV(CS51);     // set prescaler of 8
+    TCNT5 = 0;              // clear the timer count
+    TIFR5 = _BV(OCF5A);     // clear any pending interrupts;
+    TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt
+  }
+#endif
+}
+
+static void finISR(timer16_Sequence_t timer)
+{
+    //disable use of the given timer
+#if defined WIRING   // Wiring
+  if(timer == _timer1) {
+    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
+    TIMSK1 &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
+    #else
+    TIMSK &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
+    #endif
+    timerDetach(TIMER1OUTCOMPAREA_INT);
+  }
+  else if(timer == _timer3) {
+    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
+    TIMSK3 &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
+    #else
+    ETIMSK &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
+    #endif
+    timerDetach(TIMER3OUTCOMPAREA_INT);
+  }
+#else
+    //For arduino - in future: call here to a currently undefined function to reset the timer
+#endif
+}
+
+static boolean isTimerActive(timer16_Sequence_t timer)
+{
+  // returns true if any servo is active on this timer
+  for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
+    if(SERVO(timer,channel).Pin.isActive == true)
+      return true;
+  }
+  return false;
+}
+
+
+/****************** end of static functions ******************************/
+
+Servo::Servo()
+{
+  if( ServoCount < MAX_SERVOS) {
+    this->servoIndex = ServoCount++;                    // assign a servo index to this instance
+	servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
+  }
+  else
+    this->servoIndex = INVALID_SERVO ;  // too many servos
+}
+
+uint8_t Servo::attach(int pin)
+{
+  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+}
+
+uint8_t Servo::attach(int pin, int min, int max)
+{
+  if(this->servoIndex < MAX_SERVOS ) {
+    pinMode( pin, OUTPUT) ;                                   // set servo pin to output
+    servos[this->servoIndex].Pin.nbr = pin;
+    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
+    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
+    this->max  = (MAX_PULSE_WIDTH - max)/4;
+    // initialize the timer if it has not already been initialized
+    timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
+    if(isTimerActive(timer) == false)
+      initISR(timer);
+    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
+  }
+  return this->servoIndex ;
+}
+
+void Servo::detach()
+{
+  servos[this->servoIndex].Pin.isActive = false;
+  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
+  if(isTimerActive(timer) == false) {
+    finISR(timer);
+  }
+}
+
+void Servo::write(int value)
+{
+  if(value < MIN_PULSE_WIDTH)
+  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+    if(value < 0) value = 0;
+    if(value > 180) value = 180;
+    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());
+  }
+  this->writeMicroseconds(value);
+}
+
+void Servo::writeMicroseconds(int value)
+{
+  // calculate and store the values for the given channel
+  byte channel = this->servoIndex;
+  if( (channel < MAX_SERVOS) )   // ensure channel is valid
+  {
+    if( value < SERVO_MIN() )          // ensure pulse width is valid
+      value = SERVO_MIN();
+    else if( value > SERVO_MAX() )
+      value = SERVO_MAX();
+
+    value = value - TRIM_DURATION;
+    value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
+
+    uint8_t oldSREG = SREG;
+    cli();
+    servos[channel].ticks = value;
+    SREG = oldSREG;
+  }
+}
+
+int Servo::read() // return the value as degrees
+{
+  return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
+}
+
+int Servo::readMicroseconds()
+{
+  unsigned int pulsewidth;
+  if( this->servoIndex != INVALID_SERVO )
+    pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION ;   // 12 aug 2009
+  else
+    pulsewidth  = 0;
+
+  return pulsewidth;
+}
+
+bool Servo::attached()
+{
+  return servos[this->servoIndex].Pin.isActive ;
+}
+
+#endif // ARDUINO_ARCH_AVR
+
@@ -0,0 +1,59 @@
+/*
+  Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+  Copyright (c) 2009 Michael Margolis.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+/*
+ * Defines for 16 bit timers used with  Servo library
+ *
+ * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
+ * timer16_Sequence_t enumerates the sequence that the timers should be allocated
+ * _Nbr_16timers indicates how many 16 bit timers are available.
+ */
+
+/**
+ * AVR Only definitions
+ * --------------------
+ */
+
+// Say which 16 bit timers can be used and in what order
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#define _useTimer5
+#define _useTimer1
+#define _useTimer3
+#define _useTimer4
+typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t;
+
+#elif defined(__AVR_ATmega32U4__)
+#define _useTimer1
+typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t;
+
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+#define _useTimer3
+#define _useTimer1
+typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t;
+
+#elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
+#define _useTimer3
+#define _useTimer1
+typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t;
+
+#else  // everything else
+#define _useTimer1
+typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t;
+#endif
+
@@ -0,0 +1,283 @@
+/*
+  Copyright (c) 2013 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+#if defined(ARDUINO_ARCH_SAM)
+
+#include <Arduino.h>
+#include <Servo.h>
+
+#define usToTicks(_us)    (( clockCyclesPerMicrosecond() * _us) / 32)     // converts microseconds to tick
+#define ticksToUs(_ticks) (( (unsigned)_ticks * 32)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
+
+#define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays
+
+static servo_t servos[MAX_SERVOS];                          // static array of servo structures
+
+uint8_t ServoCount = 0;                                     // the total number of attached servos
+
+static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
+
+// convenience macros
+#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
+#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
+#define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
+#define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
+
+#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
+#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
+
+/************ static functions common to all instances ***********************/
+
+//------------------------------------------------------------------------------
+/// Interrupt handler for the TC0 channel 1.
+//------------------------------------------------------------------------------
+void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel);
+#if defined (_useTimer1)
+void HANDLER_FOR_TIMER1(void) {
+    Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
+}
+#endif
+#if defined (_useTimer2)
+void HANDLER_FOR_TIMER2(void) {
+    Servo_Handler(_timer2, TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
+}
+#endif
+#if defined (_useTimer3)
+void HANDLER_FOR_TIMER3(void) {
+    Servo_Handler(_timer3, TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
+}
+#endif
+#if defined (_useTimer4)
+void HANDLER_FOR_TIMER4(void) {
+    Servo_Handler(_timer4, TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
+}
+#endif
+#if defined (_useTimer5)
+void HANDLER_FOR_TIMER5(void) {
+    Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
+}
+#endif
+
+void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel)
+{
+    // clear interrupt
+    tc->TC_CHANNEL[channel].TC_SR;
+    if (Channel[timer] < 0) {
+        tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
+    } else {
+        if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
+            digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
+        }
+    }
+
+    Channel[timer]++;    // increment to the next channel
+    if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
+        tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
+        if(SERVO(timer,Channel[timer]).Pin.isActive == true) {    // check if activated
+            digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
+        }
+    }
+    else {
+        // finished all channels so wait for the refresh period to expire before starting over
+        if( (tc->TC_CHANNEL[channel].TC_CV) + 4 < usToTicks(REFRESH_INTERVAL) ) { // allow a few ticks to ensure the next OCR1A not missed
+            tc->TC_CHANNEL[channel].TC_RA = (unsigned int)usToTicks(REFRESH_INTERVAL);
+        }
+        else {
+            tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + 4;  // at least REFRESH_INTERVAL has elapsed
+        }
+        Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
+    }
+}
+
+static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn)
+{
+    pmc_enable_periph_clk(id);
+    TC_Configure(tc, channel,
+   TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32
+            TC_CMR_WAVE |                // Waveform mode
+            TC_CMR_WAVSEL_UP_RC );       // Counter running up and reset when equals to RC
+
+    /* 84MHz, MCK/32, for 1.5ms: 3937 */
+    TC_SetRA(tc, channel, 2625); // 1ms
+
+    /* Configure and enable interrupt */
+    NVIC_EnableIRQ(irqn);
+    // TC_IER_CPAS: RA Compare
+    tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS;
+
+    // Enables the timer clock and performs a software reset to start the counting
+    TC_Start(tc, channel);
+}
+
+static void initISR(timer16_Sequence_t timer)
+{
+#if defined (_useTimer1)
+    if (timer == _timer1)
+        _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
+#endif
+#if defined (_useTimer2)
+    if (timer == _timer2)
+        _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
+#endif
+#if defined (_useTimer3)
+    if (timer == _timer3)
+        _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
+#endif
+#if defined (_useTimer4)
+    if (timer == _timer4)
+        _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
+#endif
+#if defined (_useTimer5)
+    if (timer == _timer5)
+        _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
+#endif
+}
+
+static void finISR(timer16_Sequence_t timer)
+{
+#if defined (_useTimer1)
+    TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
+#endif
+#if defined (_useTimer2)
+    TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
+#endif
+#if defined (_useTimer3)
+    TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
+#endif
+#if defined (_useTimer4)
+    TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
+#endif
+#if defined (_useTimer5)
+    TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
+#endif
+}
+
+
+static boolean isTimerActive(timer16_Sequence_t timer)
+{
+  // returns true if any servo is active on this timer
+  for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
+    if(SERVO(timer,channel).Pin.isActive == true)
+      return true;
+  }
+  return false;
+}
+
+/****************** end of static functions ******************************/
+
+Servo::Servo()
+{
+  if (ServoCount < MAX_SERVOS) {
+    this->servoIndex = ServoCount++;                    // assign a servo index to this instance
+    servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values
+  } else {
+    this->servoIndex = INVALID_SERVO;  // too many servos
+  }
+}
+
+uint8_t Servo::attach(int pin)
+{
+  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+}
+
+uint8_t Servo::attach(int pin, int min, int max)
+{
+  timer16_Sequence_t timer;
+
+  if (this->servoIndex < MAX_SERVOS) {
+    pinMode(pin, OUTPUT);                                   // set servo pin to output
+    servos[this->servoIndex].Pin.nbr = pin;
+    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
+    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
+    this->max  = (MAX_PULSE_WIDTH - max)/4;
+    // initialize the timer if it has not already been initialized
+    timer = SERVO_INDEX_TO_TIMER(servoIndex);
+    if (isTimerActive(timer) == false) {
+      initISR(timer);
+    }
+    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
+  }
+  return this->servoIndex;
+}
+
+void Servo::detach()
+{
+  timer16_Sequence_t timer;
+
+  servos[this->servoIndex].Pin.isActive = false;
+  timer = SERVO_INDEX_TO_TIMER(servoIndex);
+  if(isTimerActive(timer) == false) {
+    finISR(timer);
+  }
+}
+
+void Servo::write(int value)
+{
+  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+  if (value < MIN_PULSE_WIDTH)
+  {
+    if (value < 0)
+      value = 0;
+    else if (value > 180)
+      value = 180;
+
+    value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
+  }
+  writeMicroseconds(value);
+}
+
+void Servo::writeMicroseconds(int value)
+{
+  // calculate and store the values for the given channel
+  byte channel = this->servoIndex;
+  if( (channel < MAX_SERVOS) )   // ensure channel is valid
+  {
+    if (value < SERVO_MIN())          // ensure pulse width is valid
+      value = SERVO_MIN();
+    else if (value > SERVO_MAX())
+      value = SERVO_MAX();
+
+    value = value - TRIM_DURATION;
+    value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead
+    servos[channel].ticks = value;
+  }
+}
+
+int Servo::read() // return the value as degrees
+{
+  return map(readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
+}
+
+int Servo::readMicroseconds()
+{
+  unsigned int pulsewidth;
+  if (this->servoIndex != INVALID_SERVO)
+    pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION;
+  else
+    pulsewidth  = 0;
+
+  return pulsewidth;
+}
+
+bool Servo::attached()
+{
+  return servos[this->servoIndex].Pin.isActive;
+}
+
+#endif // ARDUINO_ARCH_SAM
+
@@ -0,0 +1,88 @@
+/*
+  Copyright (c) 2013 Arduino LLC. All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+/*
+ * Defines for 16 bit timers used with  Servo library
+ *
+ * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
+ * timer16_Sequence_t enumerates the sequence that the timers should be allocated
+ * _Nbr_16timers indicates how many 16 bit timers are available.
+ */
+
+/**
+ * SAM Only definitions
+ * --------------------
+ */
+
+// For SAM3X:
+#define _useTimer1
+#define _useTimer2
+#define _useTimer3
+#define _useTimer4
+#define _useTimer5
+
+/*
+  TC0, chan 0 => TC0_Handler
+  TC0, chan 1 => TC1_Handler
+  TC0, chan 2 => TC2_Handler
+  TC1, chan 0 => TC3_Handler
+  TC1, chan 1 => TC4_Handler
+  TC1, chan 2 => TC5_Handler
+  TC2, chan 0 => TC6_Handler
+  TC2, chan 1 => TC7_Handler
+  TC2, chan 2 => TC8_Handler
+ */
+
+#if defined (_useTimer1)
+#define TC_FOR_TIMER1       TC1
+#define CHANNEL_FOR_TIMER1  0
+#define ID_TC_FOR_TIMER1    ID_TC3
+#define IRQn_FOR_TIMER1     TC3_IRQn
+#define HANDLER_FOR_TIMER1  TC3_Handler
+#endif
+#if defined (_useTimer2)
+#define TC_FOR_TIMER2       TC1
+#define CHANNEL_FOR_TIMER2  1
+#define ID_TC_FOR_TIMER2    ID_TC4
+#define IRQn_FOR_TIMER2     TC4_IRQn
+#define HANDLER_FOR_TIMER2  TC4_Handler
+#endif
+#if defined (_useTimer3)
+#define TC_FOR_TIMER3       TC1
+#define CHANNEL_FOR_TIMER3  2
+#define ID_TC_FOR_TIMER3    ID_TC5
+#define IRQn_FOR_TIMER3     TC5_IRQn
+#define HANDLER_FOR_TIMER3  TC5_Handler
+#endif
+#if defined (_useTimer4)
+#define TC_FOR_TIMER4       TC0
+#define CHANNEL_FOR_TIMER4  2
+#define ID_TC_FOR_TIMER4    ID_TC2
+#define IRQn_FOR_TIMER4     TC2_IRQn
+#define HANDLER_FOR_TIMER4  TC2_Handler
+#endif
+#if defined (_useTimer5)
+#define TC_FOR_TIMER5       TC0
+#define CHANNEL_FOR_TIMER5  0
+#define ID_TC_FOR_TIMER5    ID_TC0
+#define IRQn_FOR_TIMER5     TC0_IRQn
+#define HANDLER_FOR_TIMER5  TC0_Handler
+#endif
+
+typedef enum { _timer1, _timer2, _timer3, _timer4, _timer5, _Nbr_16timers } timer16_Sequence_t ;
+