#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// variables:
int LEFTsensorValue = 0; // the sensor value
int leftVal1;
int leftVal2;
int leftVal3;
int leftVal4;
//int LEFTsensorMin = 0; // minimum sensor value
//int LEFTsensorMax = 100; // maximum sensor value
const int LEFTsensorPin0 = A0; //eye
int RIGHTsensorValue = 0; // the sensor value
//int RIGHTsensorMin = 0; // minimum sensor value
//int RIGHTsensorMax = 100; // maximum sensor value
const int RIGHTsensorPin1 = A1; //eye
long timer=0;//will store the length of time it takes before running the tilt routine
long timetoWait=5000;//length of time before running the tilt routine
#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 400 // this is the 'maximum' pulse length count (out of 4096)
uint16_t NECK = 0; //neck servo
uint16_t rshoulderX=1;
uint16_t LshoulderX=2;
uint16_t rshoulderY=3;
uint16_t LshoulderY=4;
uint16_t Llip=5;
uint16_t RULlip=6;
uint16_t LULlip=7;
uint16_t RBrow=8;
uint16_t LBrow=9;
uint16_t RXeye=10;
uint16_t RYeye=11;
uint16_t LXeye=12;
uint16_t LYeye=13;
uint16_t RLED=14;
uint16_t LLED=15;
int NECKPOS = 165; // assigned value
int NECKPOS1 = 225; // assigned value
int NECKPOS2 = 375; // assigned value
int ALTPOS = 325; // assigned value
int ALTPOS1 = 300; // assigned value
int ALTPOS2 = 375; // assigned value
// the setup routine runs once when you press reset:
void setup() {
// initialize serial communication at 9600 bits per second:
Serial.begin(9600);
pwm.begin();
pwm.setPWMFreq(60);
//timer = millis();//start the timer
}
/***********************************************************************/
void setServoPulse(uint8_t n, double pulse) {
double pulselength;
pulselength = 1000000; // 1,000,000 us per second
pulselength /=60; // 60 Hz
//Serial.print(pulselength); Serial.println(" us per period");
pulselength /= 4096; // 12 bits of resolution
//Serial.print(pulselength); Serial.println(" us per bit");
pulse *= 1000;
pulse /= pulselength;
// Serial.println(pulse);
pwm.setPWM(n,0,pulse);
}
/**********************************************************************/
// the loop routine runs over and over again forever:
void loop()
{
// calibrate during the first five seconds
// while (millis() < 5000);
pwm.setPWM(RLED, 0,4095);
pwm.setPWM(LLED, 0,4095);
LEFTsensorValue= analogRead(LEFTsensorPin0);
//Serial.println(LEFTsensorValue); // the raw analog reading
RIGHTsensorValue= analogRead(RIGHTsensorPin1);
//Serial.println(RIGHTsensorValue); // the raw analog reading
// delay(10);
(LEFTsensorValue>RIGHTsensorValue);
pwm.setPWM(NECK, 0, NECKPOS);
delay(50);
pwm.setPWM(rshoulderX, 0, NECKPOS);
delay(10);
pwm.setPWM(LshoulderX, 0, NECKPOS);
delay(10);
pwm.setPWM(rshoulderY, 0, NECKPOS);
delay(10);
pwm.setPWM(LshoulderY, 0, NECKPOS);
delay(10);
pwm.setPWM(Llip, 0, ALTPOS);
delay(10);
pwm.setPWM(RULlip, 0, ALTPOS2);
delay(10);
pwm.setPWM(LULlip, 0, ALTPOS2);
delay(10);
pwm.setPWM(RBrow, 0, ALTPOS2);
delay(10);
pwm.setPWM(LBrow, 0, ALTPOS2);
delay(10);
pwm.setPWM(RXeye, 0, ALTPOS2);
delay(10);
pwm.setPWM(RYeye, 0, ALTPOS);
delay(10);
pwm.setPWM(LXeye, 0, ALTPOS2);
delay(10);
pwm.setPWM(LYeye, 0, ALTPOS);
(RIGHTsensorValue>LEFTsensorValue);
pwm.setPWM(NECK, 0, NECKPOS2);
delay(10);
pwm.setPWM(rshoulderX, 0, NECKPOS2);
delay(10);
pwm.setPWM(LshoulderX, 0, NECKPOS2);
delay(10);
pwm.setPWM(rshoulderY, 0, NECKPOS2);
delay(10);
pwm.setPWM(LshoulderY, 0, NECKPOS2);
delay(10);
pwm.setPWM(Llip, 0, ALTPOS2);
delay(10);
pwm.setPWM(RULlip, 0, ALTPOS);
delay(10);
pwm.setPWM(LULlip, 0, ALTPOS);
delay(10);
pwm.setPWM(RBrow, 0, ALTPOS);
delay(10);
pwm.setPWM(LBrow, 0, ALTPOS);
delay(10);
pwm.setPWM(RXeye, 0, ALTPOS);
delay(10);
pwm.setPWM(RYeye, 0, ALTPOS2);
delay(10);
pwm.setPWM(LXeye, 0, ALTPOS);
delay(10);
pwm.setPWM(LYeye, 0, ALTPOS2);
}
//}
Comments
Please log in or sign up to comment.