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;
}
void Servo::detach()
{
servos[this->servoIndex].Pin.isActive = false;
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if(isTimerActive(timer) == false) {
finISR(timer);
}
}
void Servo::detach()
{
ServoTimerSequence timerId;
s_servos[_servoIndex].info.isActive = false;
timerId = SERVO_INDEX_TO_TIMER(_servoIndex);
if (!isTimerActive(timerId)) {
finISR(timerId);
}
}
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 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;
}
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;
boolean timer_active = isTimerActive();
servos[this->servoIndex].Pin.isActive = true;
if (!timer_active)
enableTimer();
}
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);
}
}
static void Servo_Handler(T* timer)
{
uint8_t servoIndex;
// clear interrupt
timer->ResetInterrupt();
if (timer->isEndOfCycle()) {
timer->StartCycle();
}
else {
servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel());
if (servoIndex < s_servoCount && s_servos[servoIndex].info.isActive) {
// pulse this channel low if activated
digitalWrite(s_servos[servoIndex].info.pin, LOW);
if (s_servos[servoIndex].info.isDetaching) {
s_servos[servoIndex].info.isActive = false;
s_servos[servoIndex].info.isDetaching = false;
}
}
timer->nextChannel();
}
servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel());
if (servoIndex < s_servoCount &&
timer->getCurrentChannel() < SERVOS_PER_TIMER) {
timer->SetPulseCompare(timer->usToTicks(s_servos[servoIndex].usPulse) - c_CycleCompensation);
if (s_servos[servoIndex].info.isActive) {
if (s_servos[servoIndex].info.isDetaching) {
// it was active, reset state and leave low
s_servos[servoIndex].info.isActive = false;
s_servos[servoIndex].info.isDetaching = false;
}
else {
// its an active channel so pulse it high
digitalWrite(s_servos[servoIndex].info.pin, HIGH);
}
}
}
else {
if (!isTimerActive(timer->timerId())) {
// no active running channels on this timer, stop the ISR
finISR(timer->timerId());
}
else {
// finished all channels so wait for the refresh period to expire before starting over
// allow a few ticks to ensure the next match is not missed
uint32_t refreshCompare = timer->usToTicks(REFRESH_INTERVAL);
if ((timer->GetCycleCount() + c_CycleCompensation * 2) < refreshCompare) {
timer->SetCycleCompare(refreshCompare - c_CycleCompensation);
}
else {
// at least REFRESH_INTERVAL has elapsed
timer->SetCycleCompare(timer->GetCycleCount() + c_CycleCompensation * 2);
}
}
timer->setEndOfCycle();
}
}
void Servo::detach()
{
servos[this->servoIndex].Pin.isActive = false;
if (!isTimerActive())
disableTimer();
}
void
TTwaitForChar(void)
{
#if MEOPT_MOUSE
union REGS rg ;
if(meMouseCfg & meMOUSE_ENBLE)
{
meUShort mc ;
meUInt arg ;
/* If mouse state flags that the mouse should be visible then we
* must make it visible. We had to make it invisible to get the
* screen-updates correct
*/
if(mouseState & MOUSE_STATE_SHOW)
{
mouseState |= MOUSE_STATE_VISIBLE ;
rg.x.ax = 0x0001 ;
int86(0x33, &rg, &rg) ;
}
/* If no keys left then if theres currently no mouse timer and
* theres a button press (No mouse-time key) then check for a
* time-mouse-? key, if found add a timer start a mouse checking
*/
if(!isTimerActive(MOUSE_TIMER_ID) &&
((mc=(mouseState & MOUSE_STATE_BUTTONS)) != 0))
{
mc = ME_SPECIAL | TTmodif | (SKEY_mouse_time+mouseKeys[mc]) ;
if((!TTallKeys && (decode_key(mc,&arg) != -1)) || (TTallKeys & 0x2))
{
/* Start a timer and move to timed state 1 */
/* Start a new timer to clock in at 'delay' intervals */
/* printf("Setting mouse timer for delay %1x %1x %d\n",TTallKeys,meTimerState[MOUSE_TIMER_ID],delayTime) ;*/
timerSet(MOUSE_TIMER_ID,-1,delayTime);
}
}
}
#endif
#if MEOPT_CALLBACK
/* IDLE TIME: Check the idle time events */
if(kbdmode == meIDLE)
/* Check the idle event */
doIdlePickEvent() ;
#endif
for(;;)
{
handleTimerExpired() ;
if(TTahead())
break ;
#if MEOPT_MOUSE
if(meMouseCfg & meMOUSE_ENBLE)
{
meShort row, col, but ;
meUShort cc ;
/* Get new mouse status. It appears that the fractional bits of
* the mouse change across reads. ONLY Compare the non-fractional
* components.
*/
rg.x.ax = 0x0003 ;
int86(0x33, &rg, &rg) ;
but = rg.x.bx & MOUSE_STATE_BUTTONS ;
col = rg.x.cx>>3 ;
row = rg.x.dx>>3 ;
/* Check for mouse state changes */
if(((mouseState ^ but) & MOUSE_STATE_BUTTONS) ||
(mouse_X != col) || (mouse_Y != row))
{
int ii ;
/* Get shift status */
rg.h.ah = 0x02;
int86(0x16,&rg,&rg) ;
TTmodif = ((rg.h.al & 0x01) << 8) | ((rg.h.al & 0x0E) << 7) ;
/* Record the new mouse positions */
mouse_X = col ;
mouse_Y = row ;
/* Check for button changes, these always create keys */
if((mouseState ^ but) & MOUSE_STATE_BUTTONS)
{
/* Iterate down the bit pattern looking for changes
* of state */
for (ii=1 ; ii<8 ; ii<<=1)
{
/* Test each key and add to the stack */
if((mouseState ^ but) & ii)
{
/* Up or down press ?? */
if(but & ii)
cc = ME_SPECIAL+SKEY_mouse_pick_1-1 ; /* Down */
else
cc = ME_SPECIAL+SKEY_mouse_drop_1-1 ; /* Up !! */
cc = TTmodif | (cc + mouseKeys[ii]) ;
addKeyToBuffer(cc) ;
}
}
/* Correct the mouse state */
mouseState = (mouseState & ~MOUSE_STATE_BUTTONS) | but ;
}
else
{
meUInt arg; /* Decode key argument */
/* Check for a mouse-move key */
cc = (ME_SPECIAL+SKEY_mouse_move+mouseKeys[but]) | TTmodif ;
/* Are we after all movements or mouse-move bound ?? */
if((!TTallKeys && (decode_key(cc,&arg) != -1)) || (TTallKeys & 0x1))
addKeyToBuffer(cc) ; /* Add key to keyboard buffer */
else
{
/* Mouse has been moved so we must make it visible */
if(!(mouseState & MOUSE_STATE_VISIBLE))
{
mouseState |= MOUSE_STATE_VISIBLE ;
rg.x.ax = 0x0001 ;
int86(0x33, &rg, &rg) ;
}
continue ;
}
}
/* As we are adding a mouse key, the mouse is active
* so add the 'show' flag, so on entry next time
* the mouse will automatically become visible.
*/
mouseState |= MOUSE_STATE_SHOW ;
break ;
}
}
#endif
} /* ' for' */
int isCiaActive(struct timer *t) {
return (!isIrqBlocked() && isTimerActive(t)) || isTimerDrivenPsid();
}
