//-------------------------------------------------------------- //-- Playing a sequence in the 4 servos //-- The servo positions are defined in a table. After some time //-- the servos are updated to their next positions. Also, if the //-- test button is clicked, the servos move the their next positions //-------------------------------------------------------------- //-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011 //-- GPL license //-------------------------------------------------------------- #include //-- Mapping between the servo name (in the skymega board) and the //-- arduino pins const int SERVO2 = 8; const int SERVO4 = 9; const int SERVO6 = 10; const int SERVO8 = 11; //-- Array for accesing the 4 servos Servo myservo[4]; //-- Skymega's test button is attached to pin 12 //-- It has no pull-up resistor const int BUTTON = 12; //-- Button states const int PRESSED = LOW; const int NOT_PRESSED = HIGH; void setup() { //-- Attaching the 4 servos myservo[0].attach(SERVO2); myservo[1].attach(SERVO4); myservo[2].attach(SERVO6); myservo[3].attach(SERVO8); //-- The button is an input pinMode(BUTTON, INPUT); //-- Activate the button pull-up resistor digitalWrite(BUTTON, HIGH); Serial.begin(9600); } //-- This function returns true when the test button //-- is pressed bool button_clicked() { static int button=1; static int oldbutton=1; //-- Save the previous button state oldbutton=button; // read the button button = digitalRead(BUTTON); //-- Detect a change in the button if (button!=oldbutton) { delay(20); //-- for debouncing if (button==PRESSED) return true; } return false; } //-- This function returns true when the time "time" //-- has passed since the last call bool timeout(long time) { static long previousMillis = 0; static long currentMillis; //-- Read current time currentMillis = millis(); //-- Check if the timeout has passed if(currentMillis - previousMillis > time) { previousMillis = currentMillis; return true; } return false; } //-- Matrix of positions. //-- There are one column per servo //-- The rows determine the position of the 4 //-- servos. The controller will read one row, //-- set the servo positions, wait and go to the //-- next row //-- The user can change the values of the table. Can be added //-- as many rows as desired int pos[][4] = { //S1 S2 S3 S4 //-- Servos {0, 45, -45, 0}, //-- Initial pos {-45, 0, 45, 0}, //-- Pos 1 {45, -45, 0, 0}, //-- Pos2 //-- More rows can be added... }; //-- Calculate the number of rows int size = sizeof(pos)/(4*sizeof(int)); //-- The row that is being read int i=0; //-- Time in ms waiting before moving to the next position const long TIME = 3000; void update_servos() { //-- Position the 4 servos to the their corresponding //-- angle according to the sequence for (int s=0; s<4; s++) { //-- Position servo s at the pos i myservo[s].write(pos[i][s]+90); } } void loop() { //-- If have passed TIME ms or the test button has been clicked... if (timeout(TIME) || button_clicked()) { //-- Update the servos to their new positions update_servos(); //-- change to the next position i=(i+1) % size; } }