void servo_init() {
volatile uint8_t *ports[] = {&PORTL, &PORTD};
for (uint8_t i = 0; i < SERVO_COUNT; ++i) {
_servo[i].port = ports[i / 8];
_servo[i].ticks = 0;
_servo[i].min_ticks = 2000;
_servo[i].max_ticks = 4000;
_servo[i].setmask = 1<<(i % 8);
if (i > 7) {
_servo[i].setmask = _servo[i].setmask << 4;
}
_servo[i].clearmask = ~_servo[i].setmask;
}
_channel = -1;
PORTL = 0;
DDRL = 0xFF;
PORTD &= 0xF;
DDRD |= 0x0F;
initISR();
}
uint8_t Servo::attach(int pin, int minUs, int maxUs)
{
ServoTimerSequence timerId;
if (_servoIndex < MAX_SERVOS) {
if (s_servos[_servoIndex].info.pin == INVALID_PIN) {
pinMode(pin, OUTPUT); // set servo pin to output
digitalWrite(pin, LOW);
s_servos[_servoIndex].info.pin = pin;
}
// keep the min and max within 200-3000 us, these are extreme
// ranges and should support extreme servos while maintaining
// reasonable ranges
_maxUs = max(250, min(3000, maxUs));
_minUs = max(200, min(_maxUs, minUs));
// initialize the timerId if it has not already been initialized
timerId = SERVO_INDEX_TO_TIMER(_servoIndex);
if (!isTimerActive(timerId)) {
initISR(timerId);
}
s_servos[_servoIndex].info.isDetaching = false;
s_servos[_servoIndex].info.isActive = true; // this must be set after the check for isTimerActive
}
return _servoIndex;
}
uint8_t Stepper::begin(){
if(this->stepperIndex < MAX_STEPPERS){
pinMode(dirPin, OUTPUT);
pinMode(stepPin, OUTPUT);
setMaxAccel(DEFAULT_ACCEL);
if(isTimerActive() == false)
initISR();
steppers[this->stepperIndex].isActive = true;
steppers[this->stepperIndex].stepper = this;
}
return this->stepperIndex;
}
uint8_t ServoTimer2::attach(int pin)
{
if( isStarted == false)
initISR();
if(this->chanIndex > 0)
{
//debug("attaching chan = ", chanIndex);
pinMode( pin, OUTPUT) ; // set servo pin to output
servos[this->chanIndex].Pin.nbr = pin;
servos[this->chanIndex].Pin.isActive = true;
}
return this->chanIndex ;
}
uint8_t Servo_attach2(byte servoIndex,int pin, int min, int max)
{
if(servoIndex < MAX_SERVOS ) {
pinMode( pin, OUTPUT) ; // set servo pin to output
servos[servoIndex].Pin.nbr = pin;
// todo dit moet per channel worden gedaan, nog uitwerken
min = (SERVO_MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
max = (SERVO_MAX_PULSE_WIDTH - max)/4;
if(isTimerActive() == false)
initISR();
servos[servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
}
return servoIndex ;
}
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;
// 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 ;
}
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 ;
}
uint8_t Servo::attach(int pin, int min, int max) {
if (this->servoIndex < MAX_SERVOS ) {
#if defined(ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
if (pin > 0) this->pin = pin; else pin = this->pin;
#endif
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)) initISR(timer);
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
}
return this->servoIndex;
}
int8_t Servo::attach(const int pin, const int min, const int max) {
if (this->servoIndex >= MAX_SERVOS) return -1;
if (pin > 0) servo_info[this->servoIndex].Pin.nbr = pin;
pinMode(servo_info[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output
// 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)) initISR(timer);
servo_info[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
return this->servoIndex;
}
uint8_t Servo::attach(int pin, int min, int max)
{
if(this->servoIndex < MAX_SERVOS ) {
if(pin == 0 | pin == 1)
{
// disable UART
U1MODE = 0x0;
}
pinMode( pin, OUTPUT) ; // set servo pin to output
servos[this->servoIndex].Pin.nbr = pin;
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
int timer = SERVO_INDEX_TO_TIMER(this->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 ;
}
}
//************************************************************************
uint8_t Servo::attach(int pin, int min, int max)
{
if (this->servoIndex < ServoCount)
{
pinMode(pin, OUTPUT); // set servo pin to output
servos[this->servoIndex].Pin.nbr = pin;
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
int timer = SERVO_INDEX_TO_TIMER(this->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;
}
else
{
// return bogus value if this->ServoIndex is invalid
return(INVALID_SERVO);
}
}
