/****************************
Targus - Operation - Servos
*****************************/
/******
Notes
*******/
// Digital Pins 0 and 1 are normally used for serial commucation when uploading and monitoring an Arduino from a computer.
// Digital Pins 0 and 1 can be used for servos if the Arduino is not connected to a computer.
// If Digital Pins 0 and 1 are being used for servos, comment out any lines beginning with 'Serial.' in this file.
// Make sure this Arduino is powered on before the other Arduino since this Arduino will be receiving 5V signals.
// Make sure a GND wire on this Arduino is connected to GND on the other Arduino.
/*********
Includes
**********/
#include <Servo.h>
/**********
Variables
***********/
Servo servo5;
Servo servo6;
Servo servo7;
Servo servo8;
Servo servo9;
Servo servo10;
Servo servo11;
int pin2 = 2;
int pin3 = 3;
int pin4 = 4;
/**************
Arduino Setup
***************/
void setup() {
Serial.begin(9600); // enable serial communication for development and troubleshooting
Serial.println("Targus - Operation - Servos\n");
/*****************************************
Connect Servos and set initial positions
******************************************/
servo5.attach(5); // digital pin 5
servo5.write(90); // move to 90 degrees
servo6.attach(6); // digital pin 6
servo6.write(90);
servo7.attach(7); // digital pin 7
servo7.write(90);
servo8.attach(8); // digital pin 8
servo8.write(90);
servo9.attach(9); // digital pin 9
servo9.write(80);
servo10.attach(10); // digital pin 10
servo10.write(90);
servo11.attach(11); // digital pin 11
servo11.write(80);
/*************************
Setup Digital Input Pins
**************************/
// Setup input pins so the Arduino with sound effects can tell us when to activate servos.
pinMode(pin2, INPUT_PULLUP);
pinMode(pin3, INPUT_PULLUP);
pinMode(pin4, INPUT_PULLUP);
}
/*************
Arduino Loop
**************/
void loop() {
if (digitalRead(pin2) == HIGH) {
zap2();
} else if (digitalRead(pin3) == HIGH) {
zap3();
} else if (digitalRead(pin4) == HIGH) {
zap4();
}
delay(300);
}
/**********
Functions
***********/
int moveServo(Servo &servo, int degreeStart, int degreeEnd, unsigned long timeEnd, unsigned long timeStart, float (*easing)(float), unsigned long timeNow) {
// this function will return a number 1 if there is still work to be done
timeEnd += timeStart; // add any delay to the end time
if (timeNow < timeStart) {
// servo movement delayed, nothing to do yet so return early
return 1;
}
if (timeNow > timeEnd) {
// servo movement phase done, nothing to do
return 0;
}
// if we get this far, prepare to move a servo
float percentToMove = float(timeNow - timeStart) / float(timeEnd - timeStart);
percentToMove = easing(percentToMove);
// map degree ranges 0-180 to microsecond range 500-2400 for a SG-92R http://www.servodatabase.com/servo/towerpro/sg92r
degreeStart = map(degreeStart, 0, 180, 500, 2400);
degreeEnd = map(degreeEnd, 0, 180, 500, 2400);
float servoTo = 0;
if (degreeEnd > degreeStart) {
// rotate anti-clockwise
servoTo = ((degreeEnd - degreeStart) * percentToMove) + degreeStart;
} else {
// rotate clockwise
percentToMove = 1 - percentToMove; // inverse percent so values like 0.8 become 0.2
servoTo = ((degreeStart - degreeEnd) * percentToMove) + degreeEnd;
}
servo.writeMicroseconds(servoTo);
// Serial.print("Would map to: "); Serial.println(servoTo);
// Serial.print("degreeStart: "); Serial.println(degreeStart);
// Serial.print("degreeEnd: "); Serial.println(degreeEnd);
// Serial.print("timeEnd: "); Serial.println(timeEnd);
// Serial.print("timeStart: "); Serial.println(timeStart);
// Serial.print("timeNow: "); Serial.println(timeNow);
// Serial.print("percentToMove: "); Serial.println(percentToMove);
// Serial.print("servoTo: "); Serial.println(servoTo);
// Serial.print("\n");
return 1;
}
/******************
Functions: Easing
*******************/
// Easing functions from https://github.com/warrenm/AHEasing/blob/master/AHEasing/easing.c renamed to match http://easings.net/ for easy previewing.
float easeInBack(float pos) {
// Modeled after the overshooting cubic y = x^3-x*sin(x*pi)
return pos * pos * pos - pos * sin(pos * M_PI);
}
float easeOutBack(float pos) {
// Modeled after overshooting cubic y = 1-((1-x)^3-(1-x)*sin((1-x)*pi))
float f = (1 - pos);
return 1 - (f * f * f - f * sin(f * M_PI));
}
float easeInOutBack(float pos) {
// Modeled after the piecewise overshooting cubic function:
// y = (1/2)*((2x)^3-(2x)*sin(2*x*pi)) ; [0, 0.5)
// y = (1/2)*(1-((1-x)^3-(1-x)*sin((1-x)*pi))+1) ; [0.5, 1]
if (pos < 0.5) {
float f = 2 * pos;
return 0.5 * (f * f * f - f * sin(f * M_PI));
} else {
float f = (1 - (2 * pos - 1));
return 0.5 * (1 - (f * f * f - f * sin(f * M_PI))) + 0.5;
}
}
float easeInBounce(float pos) {
return 1 - easeOutBounce(1 - pos);
}
float easeOutBounce(float pos) {
if (pos < 4 / 11.0) {
return (121 * pos * pos) / 16.0;
} else if (pos < 8 / 11.0) {
return (363 / 40.0 * pos * pos) - (99 / 10.0 * pos) + 17 / 5.0;
} else if (pos < 9 / 10.0) {
return (4356 / 361.0 * pos * pos) - (35442 / 1805.0 * pos) + 16061 / 1805.0;
} else {
return (54 / 5.0 * pos * pos) - (513 / 25.0 * pos) + 268 / 25.0;
}
}
float easeInOutBounce(float pos) {
if (pos < 0.5) {
return 0.5 * easeInBounce(pos * 2);
} else {
return 0.5 * easeOutBounce(pos * 2 - 1) + 0.5;
}
}
float easeInCirc(float pos) {
// Modeled after shifted quadrant IV of unit circle
return 1 - sqrt(1 - (pos * pos));
}
float easeOutCirc(float pos) {
// Modeled after shifted quadrant II of unit circle
return sqrt((2 - pos) * pos);
}
float easeInOutCirc(float pos) {
// Modeled after the piecewise circular function
// y = (1/2)(1 - sqrt(1 - 4x^2)) ; [0, 0.5)
// y = (1/2)(sqrt(-(2x - 3)*(2x - 1)) + 1) ; [0.5, 1]
if (pos < 0.5) {
return 0.5 * (1 - sqrt(1 - 4 * (pos * pos)));
} else {
return 0.5 * (sqrt(-((2 * pos) - 3) * ((2 * pos) - 1)) + 1);
}
}
float easeInCubic(float pos) {
// Modeled after the cubic y = x^3
return pos * pos * pos;
}
float easeOutCubic(float pos) {
// Modeled after the cubic y = (x - 1)^3 + 1
float f = (pos - 1);
return f * f * f + 1;
}
float easeInOutCubic(float pos) {
// Modeled after the piecewise cubic
// y = (1/2)((2x)^3) ; [0, 0.5)
// y = (1/2)((2x-2)^3 + 2) ; [0.5, 1]
if (pos < 0.5) {
return 4 * pos * pos * pos;
} else {
float f = ((2 * pos) - 2);
return 0.5 * f * f * f + 1;
}
}
float easeInElastic(float pos) {
// Modeled after the damped sine wave y = sin(13pi/2*x)*pow(2, 10 * (x - 1))
return sin(13 * M_PI_2 * pos) * pow(2, 10 * (pos - 1));
}
float easeOutElastic(float pos) {
// Modeled after the damped sine wave y = sin(-13pi/2*(x + 1))*pow(2, -10x) + 1
return sin(-13 * M_PI_2 * (pos + 1)) * pow(2, -10 * pos) + 1;
}
float easeInOutElastic(float pos) {
// Modeled after the piecewise exponentially-damped sine wave:
// y = (1/2)*sin(13pi/2*(2*x))*pow(2, 10 * ((2*x) - 1)) ; [0,0.5)
// y = (1/2)*(sin(-13pi/2*((2x-1)+1))*pow(2,-10(2*x-1)) + 2) ; [0.5, 1]
if (pos < 0.5) {
return 0.5 * sin(13 * M_PI_2 * (2 * pos)) * pow(2, 10 * ((2 * pos) - 1));
} else {
return 0.5 * (sin(-13 * M_PI_2 * ((2 * pos - 1) + 1)) * pow(2, -10 * (2 * pos - 1)) + 2);
}
}
float easeInExpo(float pos) {
// Modeled after the exponential function y = 2^(10(x - 1))
return (pos == 0.0) ? pos : pow(2, 10 * (pos - 1));
}
float easeOutExpo(float pos) {
// Modeled after the exponential function y = -2^(-10x) + 1
return (pos == 1.0) ? pos : 1 - pow(2, -10 * pos);
}
float easeInOutExpo(float pos) {
// Modeled after the piecewise exponential
// y = (1/2)2^(10(2x - 1)) ; [0,0.5)
// y = -(1/2)*2^(-10(2x - 1))) + 1 ; [0.5,1]
if (pos == 0.0 || pos == 1.0) return pos;
if (pos < 0.5) {
return 0.5 * pow(2, (20 * pos) - 10);
} else {
return -0.5 * pow(2, (-20 * pos) + 10) + 1;
}
}
float linear(float pos) {
return pos;
}
float easeInQuad(float pos) {
// Modeled after the parabola y = x^2
return pos * pos;
}
float easeOutQuad(float pos) {
// Modeled after the parabola y = -x^2 + 2x
return -(pos * (pos - 2));
}
float easeInOutQuad(float pos) {
// Modeled after the piecewise quadratic
// y = (1/2)((2x)^2) ; [0, 0.5)
// y = -(1/2)((2x-1)*(2x-3) - 1) ; [0.5, 1]
if (pos < 0.5) {
return 2 * pos * pos;
} else {
return (-2 * pos * pos) + (4 * pos) - 1;
}
}
float easeInQuart(float pos) {
// Modeled after the quartic x^4
return pos * pos * pos * pos;
}
float easeOutQuart(float pos) {
// Modeled after the quartic y = 1 - (x - 1)^4
float f = (pos - 1);
return f * f * f * (1 - pos) + 1;
}
float easeInOutQuart(float pos) {
// Modeled after the piecewise quartic
// y = (1/2)((2x)^4) ; [0, 0.5)
// y = -(1/2)((2x-2)^4 - 2) ; [0.5, 1]
if (pos < 0.5) {
return 8 * pos * pos * pos * pos;
} else {
float f = (pos - 1);
return -8 * f * f * f * f + 1;
}
}
float easeInQuint(float pos) {
// Modeled after the quintic y = x^5
return pos * pos * pos * pos * pos;
}
float easeOutQuint(float pos) {
// Modeled after the quintic y = (x - 1)^5 + 1
float f = (pos - 1);
return f * f * f * f * f + 1;
}
float easeInOutQuint(float pos) {
// Modeled after the piecewise quintic
// y = (1/2)((2x)^5) ; [0, 0.5)
// y = (1/2)((2x-2)^5 + 2) ; [0.5, 1]
if (pos < 0.5) {
return 16 * pos * pos * pos * pos * pos;
} else {
float f = ((2 * pos) - 2);
return 0.5 * f * f * f * f * f + 1;
}
}
float easeInSine(float pos) {
// Modeled after quarter-cycle of sine wave
return sin((pos - 1) * M_PI_2) + 1;
}
float easeOutSine(float pos) {
// Modeled after quarter-cycle of sine wave (different phase)
return sin(pos * M_PI_2);
}
float easeInOutSine(float pos) {
// Modeled after half sine wave
return 0.5 * (1 - cos(pos * M_PI));
}
/***************
Functions: Zap
****************/
/******
Zap 2
*******/
void zap2() {
Serial.println("ZAP 2 called!"); //Bone
unsigned long timeStart = millis();
int todo;
do {
unsigned long timeNow = millis() - timeStart;
todo = 0;
// M Leg Kick
//todo += moveServo(servo5, 90, 50, 100, 0, easeInOutCubic, timeNow); // move servo5 from 90 to 180 degrees for 1 second after a 0 second delay
// todo += moveServo(servo5, 50, 90, 500, 500, easeOutBounce, timeNow); // move servo5 from 180 to 90 degrees for 1 second after a 1 second delay
// M Head side to side
todo += moveServo(servo7, 90, 110, 500,0, easeInOutCubic, timeNow); // move servo7 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo7, 110, 70, 500, 500, easeInOutCubic, timeNow); // move servo7 from 180 to 90 degrees for 1 second after a 1 second delay
todo += moveServo(servo7, 70, 110, 500, 1000, easeInOutCubic, timeNow);
todo += moveServo(servo7, 110, 70, 500, 1500, easeInOutCubic, timeNow);
todo += moveServo(servo7, 70, 110, 500, 2000, easeInOutCubic, timeNow);
todo += moveServo(servo7, 110, 70, 500, 2500, easeInOutCubic, timeNow);
todo += moveServo(servo7, 70, 90, 500, 3000, easeInOutCubic, timeNow);
// M left arm up and down
todo += moveServo(servo8, 90, 170, 1000, 0, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo8, 170, 90, 1000, 4000, easeOutBounce, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// M right arm up and down
todo += moveServo(servo6, 90, 130, 1000, 1500, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo6, 130, 90, 1000, 5000, easeOutBounce, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// S Head side to side
todo += moveServo(servo10, 90, 40, 1000, 500, easeInOutCubic, timeNow); // move servo7 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo10, 40, 105, 1000, 2000, easeInOutCubic, timeNow); // move servo7 from 180 to 90 degrees for 1 second after a 1 second delay
todo += moveServo(servo10, 105, 90, 1000, 6000, easeInOutCubic, timeNow);
// S left arm up and down
todo += moveServo(servo11, 80, 160, 1000, 2000, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo11, 160, 80, 1000, 5000, easeInOutCubic, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// S right arm up and down
todo += moveServo(servo9, 80, 20, 1000, 1000, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo9, 20, 80, 1000, 2000, easeInOutCubic, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
delay(20);
} while (todo > 0);
}
/******
Zap 3
*******/
void zap3() {
Serial.println("ZAP 3 called!");
unsigned long timeStart = millis();
int todo;
do {
unsigned long timeNow = millis() - timeStart;
todo = 0;
// M Head side to side
todo += moveServo(servo7, 90, 130, 1000, 0, easeInOutCubic, timeNow); // move servo7 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo7, 130, 90, 1000, 5000, easeInOutCubic, timeNow); // move servo7 from 180 to 90 degrees for 1 second after a 1 second delay
// M left arm up and down
todo += moveServo(servo8, 90, 170, 1000, 0, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo8, 170, 90, 1000, 4000, easeOutBounce, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// M right arm up and down
todo += moveServo(servo6, 90, 130, 1000, 1500, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo6, 130, 90, 1000, 5000, easeOutBounce, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// S Head side to side
todo += moveServo(servo10, 90, 40, 1000, 500, easeInOutCubic, timeNow); // move servo7 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo10, 40, 105, 1000, 2000, easeInOutCubic, timeNow); // move servo7 from 180 to 90 degrees for 1 second after a 1 second delay
todo += moveServo(servo10, 105, 90, 1000, 6000, easeInOutCubic, timeNow);
// S left arm up and down
todo += moveServo(servo11, 80, 160, 1000, 0, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo11, 160, 80, 1000, 5000, easeInOutCubic, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// S right arm up and down
todo += moveServo(servo9, 80, 20, 1000, 1000, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo9, 20, 80, 1000, 6000, easeInOutCubic, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
delay(10);
} while (todo > 0);
}
/******
Zap 4
*******/
void zap4() {
Serial.println("ZAP 4 called!");
unsigned long timeStart = millis();
int todo;
do {
unsigned long timeNow = millis() - timeStart;
todo = 0;
// M Head side to side
todo += moveServo(servo7, 90, 130, 1000, 0, easeInOutCubic, timeNow); // move servo7 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo7, 130, 90, 1000, 5000, easeInOutCubic, timeNow); // move servo7 from 180 to 90 degrees for 1 second after a 1 second delay
// M left arm up and down
todo += moveServo(servo8, 90, 170, 1000, 0, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo8, 170, 90, 1000, 4000, easeOutBounce, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// M right arm up and down
todo += moveServo(servo6, 90, 130, 1000, 1500, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo6, 130, 90, 1000, 5000, easeOutBounce, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// S Head side to side
todo += moveServo(servo10, 90, 40, 1000, 500, easeInOutCubic, timeNow); // move servo7 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo10, 40, 105, 1000, 2000, easeInOutCubic, timeNow); // move servo7 from 180 to 90 degrees for 1 second after a 1 second delay
todo += moveServo(servo10, 105, 90, 1000, 6000, easeInOutCubic, timeNow);
// S left arm up and down
todo += moveServo(servo11, 80, 160, 1000, 2000, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo11, 160, 80, 1000, 5000, easeInOutCubic, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
// S right arm up and down
todo += moveServo(servo9, 80, 20, 1000, 1000, easeInOutCubic, timeNow); // move servo8 from 90 to 180 degrees for 1 second after a 0 second delay
todo += moveServo(servo9, 20, 80, 1000, 2000, easeInOutCubic, timeNow); // move servo8 from 180 to 90 degrees for 1 second after a 1 second delay
delay(10);
} while (todo > 0);
}
Comments