#include // #include // #include #include // #include // #include // #include // #include #include "servo.h" // #include void init_timer_servo(void) { //Fpwm = f_IO / (prescaler * (1 + TOP)) = 7200 Hz. */ OCR1B = 210; OCR1A = 279; /* Fast PWM 10bits with TOP=0x03FF */ TCCR1A |= (1< (max)) \ // (v) = (max); \ // } while (0) // // // // volatile int8_t servo_updating_id_; // volatile uint8_t servo_high_time_[SERVO_NUMBER]; // static const uint16_t servo_tic_cycle_ = 8000000 / 64 * 20 / 1000; // // union _utils_byte_access // { // uint8_t v8[4]; // uint16_t v16[2]; // uint32_t v32; // }; // // /** Byte packing macro, pack 4 bytes into a double word. */ // extern inline uint32_t // v8_to_v32 (uint8_t b3, uint8_t b2, uint8_t b1, uint8_t b0) // { // union _utils_byte_access ba; // ba.v8[0] = b0; // ba.v8[1] = b1; // ba.v8[2] = b2; // ba.v8[3] = b3; // return ba.v32; // } // // extern inline uint8_t // v16_to_v8 (uint16_t w, int pos) // { // union _utils_byte_access ba; // ba.v16[0] = w; // return ba.v8[pos]; // } // // // // SIGNAL (SIG_OVERFLOW2); // // // void servo_apbteam_init (void) // { // /* Set-up all the pins of the servo to out direction */ // DDRB |= (1< 4.44 ms TOP */ // TCCR1B |= (1<= 0) // { // /* Servos motor high state mode */ // // /* Set to low state the previous servo motor pin if needed (not for // * the first one) */ // if (servo_updating_id_ != 0) // PORTB &= ~_BV (servo_updating_id_ - 1); // /* Set to high state the current servo motor pin, unless is zero */ // if (servo_high_time_[servo_updating_id_]) // set_bit (PORTB, servo_updating_id_); // /* Plan next timer overflow to the TOP minus the current configuration // * of the servo motor */ // TCNT1 = SERVO_TCNT_TOP - servo_high_time_[servo_updating_id_]; // /* Update the time spent at high state by all servo motors for this // * cycle */ // servo_high_time_cycle += servo_high_time_[servo_updating_id_]; // /* Update the identifier of the current servo motor (and manage when // * we are at the last one) */ // if (++servo_updating_id_ == SERVO_NUMBER) // servo_updating_id_ = -1; // } // else // { // // /* Sleeping time mode */ // // /* Is it the first we are in this mode? */ // if (servo_overflow_count == -1) // { // /* Set to low state the previous servo motor pin */ // PORTB &= ~_BV (SERVO_NUMBER - 1); // /* Number of full overflow (from 0 to SERVO_TCNT_TOP) we need to // * wait (division by SERVO_TCNT_TOP or >> 8) */ // servo_overflow_count = servo_high_time_cycle >> 8; // /* Restart the counter from remaining TIC that are left and can // * not be used to make a full overflow */ // TCNT1 = SERVO_TCNT_TOP - v16_to_v8 (servo_high_time_cycle, 0); // } // else // { // /* We just have an overflow, are we at the last one needed? The -1 // * is normal: we do not count the first overflow of the sleeping // * mode because it is not a full one */ // if (--servo_overflow_count == -1) // { // /* Restart with first servo motor */ // servo_updating_id_ = 0; // /* Re-initialize the counter of time spent by each servo motor // * at high state */ // servo_high_time_cycle = servo_tic_cycle_; // } // } // } // }