bool Servo_Handler_1(void*)
{
timer16_Sequence_t timer = _timer2;
if (Channel[timer] < 0) {
//Timers.periodicHz(10000000, Servo_Handler_0, 0, 0); // channel set to -1 indicated that refresh interval completed so reset the timer
} else {
if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
Channel[timer]++; // increment to the next channel
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
Timers.timer(timer)->setFrequency(usToHz(ticksToUs(SERVO(timer,Channel[timer]).ticks)));
if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
}
}
else {
Timers.timer(timer)->setFrequency(usToHz(REFRESH_INTERVAL));
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
return true;
}
int Servo::readMicroseconds()
{
unsigned int pulsewidth;
if( this->servoIndex != INVALID_SERVO )
pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009
else
pulsewidth = 0;
return pulsewidth;
}
void gpioCallback(void *arg)
{
uint16 gpio_status = 0;
static uint32 currTicks = 0;
static uint32 prevTicks = 0;
/* get the GPIO interrupt status */
gpio_status = GPIO_REG_READ( GPIO_STATUS_ADDRESS );
/* clear the interrupt */
GPIO_REG_WRITE( GPIO_STATUS_W1TC_ADDRESS, gpio_status);
/* did GPIO 12 (connected to IR receiver) generate the ISR? */
if( gpio_status == BIT(12) )
{
/* yes, and is it the first edge of the IR message frame? */
if ( edgeIndex == 0 )
{
/* yes, then store counter value */
prevTicks = RTC_REG_READ(FRC1_COUNT_ADDRESS);
/* and start the HW TIMER */
RTC_REG_WRITE(FRC1_CTRL_ADDRESS,
DIVDED_BY_16 | FRC1_ENABLE_TIMER | TM_EDGE_INT);
/* reset relevant variables */
minInterval = 0xFFFFFFFF; // initialize to a very big number
rawIrMsgLen = 0;
#if 1
os_printf("\n\nBeginning of IR message frame detected");
#endif
//Set GPIO0 to HIGH
gpio_output_set( BIT0, 0, BIT0, 0 );
}
else
{
/* record time of current edge */
currTicks = RTC_REG_READ( FRC1_COUNT_ADDRESS );
/* record time interval between current edge and previous edge */
intervalArr[ edgeIndex - 1 ] = ticksToUs( prevTicks - currTicks );
/* keep track the shortest interval */
minInterval = ( intervalArr[ edgeIndex - 1 ] < minInterval ) ? intervalArr[ edgeIndex - 1 ] : minInterval;
/* save time of current edge */
prevTicks = currTicks;
}
edgeIndex++;
}
}
static mp_obj_t servo_obj_usecs(int n_args, const mp_obj_t *args) {
pyb_servo_obj_t *self = args[0];
uint usecs;
if (n_args == 1) {
// get
return MP_OBJ_NEW_SMALL_INT(ticksToUs(servo_ticks[self->servo_id]));
}
// Set
usecs = mp_obj_get_int(args[1]);
if (self->min_usecs < self->max_usecs) {
usecs = clamp(usecs, self->min_usecs, self->max_usecs);
} else {
usecs = clamp(usecs, self->max_usecs, self->min_usecs);
}
servo_ticks[self->servo_id] = usToTicks(usecs);
return mp_const_none;
}
int Servo::readMicroseconds() {
return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
}
// Read the current time, for this Pin, in us (as a float)
float SoftPWMServoServoRead(uint32_t Pin)
{
return (ticksToUs(SoftPWMServoRawRead(Pin)));
}
int Servo::readMicroseconds()
{
if (servoIndex >= MAX_SERVOS) return 0;
return ticksToUs(servo_ticks[servoIndex]);
}