void Servotor32::begin(){
// Serial setup moved to .ino setup()
// Setup pwm output
pwm.begin();
pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
pwm2.begin();
pwm2.setPWMFreq(60);
Timer1.initialize(10);
Timer1.attachInterrupt(callback);
for(byte i=0; i<SERVOS; i++){
servo_positions[i] = -1;
}
for(byte i=0; i<GROUPS; i++){
for(byte j=0; j<SERVOS_PER_GROUP; j++){
servos_sorted[i][j] = -1;
}
}
for(uint8_t i=0; i<MAX_TIMINGS; i++){
servo_timings[i] = 0;
shift_output[i] = 0xFF;
shift_latch[i] = 0xFF;
}
// Disable these for now, maybe enable later. From old Servotor32 code
// TIMSK0 &= ~(_BV(TOIE0)); // disables the arduino delay function, but also
// // all but eliminates servo jitter
// TIMSK2 &= ~(_BV(TOIE2)); // disable the arduino tone function, but also
// // also helps eliminate some jitter
// Doesn't work on Uno
// TIMSK3 &= ~(_BV(TOIE3)); // for good measure
// TIMSK4 &= ~(_BV(TOIE4)); // for good measure
// From pwmtest.pde in Adafruit PWM Servo Driver Library:
// if you want to really speed stuff up, you can go into 'fast 400khz I2C' mode
// some i2c devices dont like this so much so if you're sharing the bus, watch
// out for this!
// pwm.setPWMFreq(1600); // This is the maximum PWM frequency
// save I2C bitrate
// uint8_t twbrbackup = TWBR;
// must be changed after calling Wire.begin() (inside pwm.begin())
// TWBR = 12; // upgrade to 400KHz!
}
void setup() {
Serial.begin(9600);
Serial.println("16 channel Servo test!");
// LED Indicator
pinMode(ledPin, OUTPUT);
pwm.begin();
pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
pwm.setPWM(servo_00, 0, STOP);
pwm.setPWM(servo_01, 0, STOP);
delay(1000);
yield();
}
void loop() {
digitalWrite(ledPin, LOW);
pwm.setPWM(servo_00, 0, FORWARD);
delay(1000);
pwm.setPWM(servo_01, 0, FORWARD);
delay(1000);
digitalWrite(ledPin, HIGH);
pwm.setPWM(servo_00, 0, BACKWARD);
delay(1000);
pwm.setPWM(servo_01, 0, BACKWARD);
delay(1000);
}
// modify the state of a servo
void Servotor32::changeServo(byte servo, short pos){
if(pos == 0){
pos = -1;
}
if(pos == -1){
update_registers_fast(servo, pos);
if (servo < 15) {
pwm.setPWM(servo, 0, 0);
} else {
pwm2.setPWM(servo - 16, 0, 0);
}
}
else{
update_registers_fast(servo, pos/10);
if (servo < 15) {
pwm.setPWM(servo, 0, constrain(map(pos, 500, 2500, SERVOMIN, SERVOMAX), 0, 4096));
} else {
pwm2.setPWM(servo - 16, 0, constrain(map(pos, 500, 2500, SERVOMIN, SERVOMAX), 0, 4096));
}
}
}
void setServoPulse(uint8_t servo_num, double pulse) {
double pulselength;
pulselength = 1000000; // 1,000,000 us per second
pulselength /= 60; // 60 Hz
//cout << pulselength << " us per period" << endl;
pulselength /= 4096; // 12 bits of resolution
//cout << pulselength << "us per bit" << endl;
pulse *= 1000;
pulse /= pulselength;
//cout << (uint16_t) pulse << endl;
pwm.setPWM(servo_num, 0, (uint16_t) pulse);
//cout << endl;
}
Target::Target(int targetNumber, Adafruit_PWMServoDriver pwm, int servoNumber) : number(targetNumber), pwm(pwm), servoNumber(servoNumber) {
//Set Servo to initial position
pwm.setPWM(servoNumber, 0, servoOffPosition);
}
/**
* Desc: This function sets up the breakout board communication with I2C
* using Adafruits_PWMServoDriver.cpp and to set the frequency of the
* servos to 60Hz.
*/
void setup() {
//cout << "Testing Servos" << endl;
pwm.begin();
pwm.setPWMFreq(60.0); // Analog servos run at ~60 Hz updates
}
