예제 #1
0
파일: main.c 프로젝트: ming-hai/OpenServo
static void handle_twi_command(void)
{
    uint8_t command;

    // Get the command from the receive buffer.
    command = twi_receive_byte();

    switch (command)
    {
    case TWI_CMD_RESET:

        // Reset the servo.
        watchdog_hard_reset();

        break;

    case TWI_CMD_PWM_ENABLE:

        // Enable PWM to the servo motor.
        pwm_enable();

        break;

    case TWI_CMD_PWM_DISABLE:

        // Disable PWM to the servo motor.
        pwm_disable();

        break;

    case TWI_CMD_WRITE_ENABLE:

        // Enable write to read/write protected registers.
        registers_write_enable();

        break;

    case TWI_CMD_WRITE_DISABLE:

        // Disable write to read/write protected registers.
        registers_write_disable();

        break;

    case TWI_CMD_REGISTERS_SAVE:

        // Save register values into EEPROM.
        eeprom_save_registers();

        break;

    case TWI_CMD_REGISTERS_RESTORE:

        // Restore register values into EEPROM.
        eeprom_restore_registers();

        break;

    case TWI_CMD_REGISTERS_DEFAULT:

        // Restore register values to factory defaults.
        registers_defaults();
        break;

    case TWI_CMD_EEPROM_ERASE:

        // Erase the EEPROM.
        eeprom_erase();

        break;

    case TWI_CMD_VOLTAGE_READ:

        // Request a voltage reading.
        adc_read_voltage();

        break;

#if CURVE_MOTION_ENABLED
    case TWI_CMD_CURVE_MOTION_ENABLE:

        // Enable curve motion handling.
        motion_enable();

        break;

    case TWI_CMD_CURVE_MOTION_DISABLE:

        // Disable curve motion handling.
        motion_disable();

        break;

    case TWI_CMD_CURVE_MOTION_RESET:

        // Reset the motion to the current position.
#if ENCODER_ENABLED
        motion_reset(enc_get_position_value());
#else
        motion_reset(adc_get_position_value());
#endif

        break;

    case TWI_CMD_CURVE_MOTION_APPEND:

        // Append motion curve data stored in the registers.
        motion_append();

        break;
#endif

    default:

        // Ignore unknown command.
        break;
    }
}
예제 #2
0
파일: main.c 프로젝트: ming-hai/OpenServo
int main (void)
{
    // Configure pins to the default states.
    config_pin_defaults();

    // Initialize the watchdog module.
    watchdog_init();

    // First, initialize registers that control servo operation.
    registers_init();

    // Initialize the PWM module.
    pwm_init();

    // Initialize the ADC module.
    adc_init();

    // Initialize the PID algorithm module.
    pid_init();

#if CURVE_MOTION_ENABLED
    // Initialize curve motion module.
    motion_init();
#endif

    // Initialize the power module.
    power_init();

#if PULSE_CONTROL_ENABLED
    pulse_control_init();
#endif

#if ENCODER_ENABLED
    // Initialize software I2C to talk with encoder.
    swi2c_init();
#endif

    // Initialize the TWI slave module.
    twi_slave_init(registers_read_byte(REG_TWI_ADDRESS));

    // Finally initialize the timer.
    timer_set(0);

    // Enable interrupts.
    sei();

    // Wait until initial position value is ready.
    {
        int16_t position;
        // Get start-up position
#if ENCODER_ENABLED
        position=(int16_t) enc_get_position_value();
#else
        while (!adc_position_value_is_ready());
        position=(int16_t) adc_get_position_value();
#endif

#if CURVE_MOTION_ENABLED
        // Reset the curve motion with the current position of the servo.
        motion_reset(position);
#endif

        // Set the initial seek position and velocity.
        registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, position);
        registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
    }


    // XXX Enable PWM and writing.  I do this for now to make development and
    // XXX tuning a bit easier.  Constantly manually setting these values to
    // XXX turn the servo on and write the gain values get's to be a pain.
    pwm_enable();
    registers_write_enable();

    // This is the main processing loop for the servo.  It basically looks
    // for new position, power or TWI commands to be processed.
    for (;;)
    {
        uint8_t tick;
        int16_t pwm;
        int16_t position;
        // Is ADC position value ready?
        // NOTE: Even when the encoder is enabled, we still need the ADC potmeasurement as the
        //       clock because that is how the original firmware was written.
        tick=adc_position_value_is_ready();
        if(tick)
        {
#if PULSE_CONTROL_ENABLED
            // Give pulse control a chance to update the seek position.
            pulse_control_update();
#endif

#if CURVE_MOTION_ENABLED
            // Give the motion curve a chance to update the seek position and velocity.
            motion_next(10);
#endif
        }

        // Get the new position value.
        if(tick)
        {
            position = (int16_t) adc_get_position_value(); // NOTE: For encoder builds, this is the clock: clear the flag
#if ENCODER_ENABLED
        } // Always run the encoder (faster PID to PWM loop = better?)
        position = (int16_t) enc_get_position_value();
        if (position >= 0)
        {
#endif
            // Call the PID algorithm module to get a new PWM value.
            pwm = pid_position_to_pwm(position, tick);

            // Update the servo movement as indicated by the PWM value.
            // Sanity checks are performed against the position value.
            pwm_update(position, pwm);
        }

        // Is a power value ready?
        if (adc_power_value_is_ready())
        {
            // Get the new power value.
            uint16_t power = adc_get_power_value();

            // Update the power value for reporting.
            power_update(power);
        }

        // Was a command recieved?
        if (twi_data_in_receive_buffer())
        {
            // Handle any TWI command.
            handle_twi_command();
        }

#if MAIN_MOTION_TEST_ENABLED
        // This code is in place for having the servo drive itself between
        // two positions to aid in the servo tuning process.  This code
        // should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
        if (motion_time_left() == 0)
        {
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
            motion_append();
        }
#else
        {
            // Get the timer.
            uint16_t timer = timer_get();

            // Reset the timer if greater than 800.
            if (timer > 800) timer_set(0);

            // Look for specific events.
            if (timer == 0)
            {
                registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0100);
            }
            else if (timer == 400)
            {
                registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0300);
            }
        }
#endif
#endif
    }

    return 0;
}
예제 #3
0
파일: main.c 프로젝트: AndySze/OpenServo
static void handle_twi_command(void)
{
    uint8_t command;

    // Get the command from the receive buffer.
    command = twi_receive_byte();

    switch (command)
    {
        case TWI_CMD_RESET:

            // Reset the servo.
            watchdog_hard_reset();

            break;

        case TWI_CMD_PWM_ENABLE:
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
            // Enable PWM to the servo motor.
            pwm_enable();
#endif
#if STEP_ENABLED
            // Enable Stepper motor control.
            step_enable();
#endif

            break;

        case TWI_CMD_PWM_DISABLE:
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
            // Disable PWM to the servo motor.
            pwm_disable();
#endif

#if STEP_ENABLED
            // Enable Stepper motor control.
            step_disable();
#endif

            break;

        case TWI_CMD_WRITE_ENABLE:

            // Enable write to read/write protected registers.
            registers_write_enable();

            break;

        case TWI_CMD_WRITE_DISABLE:

            // Disable write to read/write protected registers.
            registers_write_disable();

            break;

        case TWI_CMD_REGISTERS_SAVE:

            // Save register values into EEPROM.
            eeprom_save_registers();

            break;

        case TWI_CMD_REGISTERS_RESTORE:

            // Restore register values into EEPROM.
            eeprom_restore_registers();

            break;

        case TWI_CMD_REGISTERS_DEFAULT:

            // Restore register values to factory defaults.
            registers_defaults();
            break;

        case TWI_CMD_EEPROM_ERASE:

            // Erase the EEPROM.
            eeprom_erase();

            break;

#if CURVE_MOTION_ENABLED
        case TWI_CMD_CURVE_MOTION_ENABLE:

            // Enable curve motion handling.
            motion_enable();

            break;

        case TWI_CMD_CURVE_MOTION_DISABLE:

            // Disable curve motion handling.
            motion_disable();

            break;

        case TWI_CMD_CURVE_MOTION_RESET:

            // Reset the motion to the current position.
            motion_reset(adc_get_position_value());

            break;

        case TWI_CMD_CURVE_MOTION_APPEND:

            // Append motion curve data stored in the registers.
            motion_append();

            break;
#endif
        case TWI_CMD_GCALL_ENABLE:

            // Enable the general call functionality
            general_call_enable();
            break;

        case TWI_CMD_GCALL_DISABLE:

            // Disable General call functionaility
            general_call_disable();

            break;

        case TWI_CMD_GCALL_START_MOVE:

            // start the general call movement
            general_call_start_move();

            break;

        case TWI_CMD_GCALL_START_WAIT:

            // dont move unless we get the start command or the group command
            general_call_start_wait();

            break;

        case TWI_CMD_PWM_BRAKE_ENABLE:
#if PWM_ENH_ENABLED
            pwm_brake_enable();
#endif

            break;

        case TWI_CMD_PWM_BRAKE_DISABLE:
#if PWM_ENH_ENABLED
            pwm_brake_disable();
#endif

            break;

        default:

            // Ignore unknown command.
            break;
    }
}
예제 #4
0
파일: main.c 프로젝트: mokis/MServo
int main (void)
{
	// Configure pins to the default states.
	config_pin_defaults();

    // Initialize the watchdog module.
    watchdog_init();

    // First, initialize registers that control servo operation.
    registers_init();

    // Initialize the PWM module.
    pwm_init();

    // Initialize the ADC module.
    adc_init();

#if ESTIMATOR_ENABLED
    // Initialize the state estimator module.
    estimator_init();
#endif

#if REGULATOR_MOTION_ENABLED
    // Initialize the regulator algorithm module.
    regulator_init();
#endif

#if PID_MOTION_ENABLED
    // Initialize the PID algorithm module.
    pid_init();
#endif

#if IPD_MOTION_ENABLED
    // Initialize the IPD algorithm module.
    ipd_init();
#endif

#if CURVE_MOTION_ENABLED
    // Initialize curve motion module.
    motion_init();
#endif

    // Initialize the power module.
    power_init();

#if PULSE_CONTROL_ENABLED
    pulse_control_init();
#endif

    // Initialize the TWI slave module.
    twi_slave_init(registers_read_byte(REG_TWI_ADDRESS));

    // Finally initialize the timer.
    timer_set(0);

    // Enable interrupts.
    sei();

    // Wait until initial position value is ready.
    while (!adc_position_value_is_ready());

#if CURVE_MOTION_ENABLED
    // Reset the curve motion with the current position of the servo.
    motion_reset(adc_get_position_value());
#endif

    // Set the initial seek position and velocity.
    registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, adc_get_position_value());
    registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);

    // XXX Enable PWM and writing.  I do this for now to make development and
    // XXX tuning a bit easier.  Constantly manually setting these values to 
    // XXX turn the servo on and write the gain values get's to be a pain.
    pwm_enable();
    registers_write_enable();

    // This is the main processing loop for the servo.  It basically looks
    // for new position, power or TWI commands to be processed.
    for (;;)
    {
        // Is position value ready?
        if (adc_position_value_is_ready())
        {
            int16_t pwm;
            int16_t position;

#if PULSE_CONTROL_ENABLED
            // Give pulse control a chance to update the seek position.
            pulse_control_update();
#endif

#if CURVE_MOTION_ENABLED
            // Give the motion curve a chance to update the seek position and velocity.
            motion_next(10);
#endif

            // Get the new position value.
            position = (int16_t) adc_get_position_value();

#if ESTIMATOR_ENABLED
            // Estimate velocity.
            estimate_velocity(position);
#endif

#if PID_MOTION_ENABLED
            // Call the PID algorithm module to get a new PWM value.
            pwm = pid_position_to_pwm(position);
#endif

#if IPD_MOTION_ENABLED
            // Call the IPD algorithm module to get a new PWM value.
            pwm = ipd_position_to_pwm(position);
#endif

#if REGULATOR_MOTION_ENABLED
            // Call the state regulator algorithm module to get a new PWM value.
            pwm = regulator_position_to_pwm(position);
#endif

            // Update the servo movement as indicated by the PWM value.
            // Sanity checks are performed against the position value.
            pwm_update(position, pwm);
        }

        // Is a power value ready?
        if (adc_power_value_is_ready())
        {
            // Get the new power value.
            uint16_t power = adc_get_power_value();

            // Update the power value for reporting.
            power_update(power);
        }

        // Was a command recieved?
        if (twi_data_in_receive_buffer())
        {
            // Handle any TWI command.
            handle_twi_command();
        }

#if MAIN_MOTION_TEST_ENABLED
        // This code is in place for having the servo drive itself between 
        // two positions to aid in the servo tuning process.  This code 
        // should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
        if (motion_time_left() == 0)
        {
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
            motion_append();
        }
#else
        {
            // Get the timer.
            uint16_t timer = timer_get();

            // Reset the timer if greater than 800.
            if (timer > 800) timer_set(0);

            // Look for specific events.
            if (timer == 0)
            {
                registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0100);
            }
            else if (timer == 400)
            {
                registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0300);
            }
        }
#endif
#endif
    }

    return 0;
}
예제 #5
0
파일: main.c 프로젝트: AndySze/OpenServo
int main (void)
{
    // Configure pins to the default states.
    config_pin_defaults();

    // Initialize the watchdog module.
    watchdog_init();

    // First, initialize registers that control servo operation.
    registers_init();

#if PWM_STD_ENABLED || PWM_ENH_ENABLED
    // Initialize the PWM module.
    pwm_init();
#endif

#if STEP_ENABLED
    // Initialise the stepper motor
    step_init();
#endif
    
    // Initialize the ADC module.
    adc_init();

    // Initialise the Heartbeart
    heartbeat_init();

    // Initialize the PID algorithm module.
    pid_init();

#if CURVE_MOTION_ENABLED
    // Initialize curve motion module.
    motion_init();
#endif

    // Initialize the power module.
    power_init();

#if PULSE_CONTROL_ENABLED
    pulse_control_init();
#endif

#if BACKEMF_ENABLED
    // Initialise the back emf module
    backemf_init();
#endif

#if ALERT_ENABLED
    //initialise the alert registers
    alert_init();
#endif

    // Initialize the TWI slave module.
    twi_slave_init(banks_read_byte(POS_PID_BANK, REG_TWI_ADDRESS));

    // Finally initialize the timer.
    timer_set(0);

    // Enable interrupts.
    sei();

    // Trigger the adc sampling hardware
    adc_start(ADC_CHANNEL_POSITION);

    // Wait until initial position value is ready.
    while (!adc_position_value_is_ready());

#if CURVE_MOTION_ENABLED
    // Reset the curve motion with the current position of the servo.
    motion_reset(adc_get_position_value());
#endif

    // Set the initial seek position and velocity.
    registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, adc_get_position_value());
    registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);

    // XXX Enable PWM and writing.  I do this for now to make development and
    // XXX tuning a bit easier.  Constantly manually setting these values to
    // XXX turn the servo on and write the gain values get's to be a pain.
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
    pwm_enable();
#endif
#if STEP_ENABLED
    step_enable();
#endif

    registers_write_enable();

    // This is the main processing loop for the servo.  It basically looks
    // for new position, power or TWI commands to be processed.
    for (;;)
    {
        static uint8_t emf_motor_is_coasting = 0;

        // Is the system heartbeat ready?
        if (heartbeat_is_ready())
        {
            static int16_t last_seek_position;
            static int16_t wait_seek_position;
            static int16_t new_seek_position;

            // Clear the heartbeat flag
            heartbeat_value_clear_ready();

#if PULSE_CONTROL_ENABLED
            // Give pulse control a chance to update the seek position.
            pulse_control_update();
#endif

#if CURVE_MOTION_ENABLED
            // Give the motion curve a chance to update the seek position and velocity.
            motion_next(10);
#endif

            // General call support
            // Check to see if we have the wait flag enabled. If so save the new position, and write in the
            // old position until we get the move command
            if (general_call_enabled()) 
            {
                //we need to wait for the go command before moving
                if (general_call_wait())
                {
                    // store the new position, but let the servo lock to the last seek position
                    wait_seek_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
                    if (wait_seek_position != last_seek_position) // do we have a new position?
                    {
                        new_seek_position = wait_seek_position;
                        registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, last_seek_position);
                    }
                }
                last_seek_position = registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);

                //check to make sure that we can start the move.
                if (general_call_start() || 
                    ( registers_read_byte(REG_GENERAL_CALL_GROUP_START) == banks_read_byte(CONFIG_BANK, REG_GENERAL_CALL_GROUP)))
                {
                    // write the new position with the previously saved position
                    registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, new_seek_position);  
                    general_call_start_wait_reset();  // reset the wait flag
                    general_call_start_reset();  // reset the start flag
                }
            }

#if BACKEMF_ENABLED
            // Quick and dirty check to see if pwm is active. This is done to make sure the motor doesn't
            // whine in the audible range while idling.
            uint8_t pwm_a = registers_read_byte(REG_PWM_DIRA);
            uint8_t pwm_b = registers_read_byte(REG_PWM_DIRB);
            if (pwm_a || pwm_b)
            {
                // Disable PWM
                backemf_coast_motor();
                emf_motor_is_coasting = 1;
            }
            else
            {
                // reset the back EMF value to 0
                banks_write_word(INFORMATION_BANK, REG_BACKEMF_HI, REG_BACKEMF_LO, 0);
                emf_motor_is_coasting = 0;
            }
#endif

#if ADC_ENABLED
            // Trigger the adc sampling hardware. This triggers the position and temperature sample
            adc_start(ADC_FIRST_CHANNEL);
#endif

        }
    
    
        // Wait for the samples to complete
#if TEMPERATURE_ENABLED
        if (adc_temperature_value_is_ready())
        {
            // Save temperature value to registers
            registers_write_word(REG_TEMPERATURE_HI, REG_TEMPERATURE_LO, (uint16_t)adc_get_temperature_value());
        }
#endif
#if CURRENT_ENABLED
        if (adc_power_value_is_ready())
        {

            // Get the new power value.
            uint16_t power = adc_get_power_value();

            // Update the power value for reporting.
            power_update(power);
        }
#endif
#if ADC_POSITION_ENABLED
        if (adc_position_value_is_ready())
        {
            int16_t position;
            // Get the new position value from the ADC module.
            position = (int16_t) adc_get_position_value();
#else
        if (position_value_is_ready())
        {
            int16_t position;
            // Get the position value from an external module.
            position = (int16_t) get_position_value();
#endif
            int16_t pwm;
#if BACKEMF_ENABLED
            if (emf_motor_is_coasting == 1)
            {
                uint8_t pwm_a = registers_read_byte(REG_PWM_DIRA);
                uint8_t pwm_b = registers_read_byte(REG_PWM_DIRB);

                // Quick and dirty check to see if pwm is active
                if (pwm_a || pwm_b)
                {
                    // Get the backemf sample.
                    backemf_get_sample();

                    // Turn the motor back on
                    backemf_restore_motor();
		    emf_motor_is_coasting = 0;
                }
            }
#endif

            // Call the PID algorithm module to get a new PWM value.
            pwm = pid_position_to_pwm(position);

#if ALERT_ENABLED
            // Update the alert status registers and do any throttling
            alert_check();
#endif

            // Allow any alerts to modify the PWM value.
            pwm = alert_pwm_throttle(pwm);

#if PWM_STD_ENABLED || PWM_ENH_ENABLED
            // Update the servo movement as indicated by the PWM value.
            // Sanity checks are performed against the position value.
            pwm_update(position, pwm);
#endif

#if STEP_ENABLED
            // Update the stepper motor as indicated by the PWM value.
            // Sanity checks are performed against the position value.
            step_update(position, pwm);
#endif
        }
    
        // Was a command recieved?
        if (twi_data_in_receive_buffer())
        {
            // Handle any TWI command.
            handle_twi_command();
        }

        // Update the bank register operations
        banks_update_registers();

#if MAIN_MOTION_TEST_ENABLED
        // This code is in place for having the servo drive itself between
        // two positions to aid in the servo tuning process.  This code
        // should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
        if (motion_time_left() == 0)
        {
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
            motion_append();
            registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
            registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
            motion_append();
        }
#else
        {
            // Get the timer.
            uint16_t timer = timer_get();

            // Reset the timer if greater than 800.
            if (timer > 800) timer_set(0);

            // Look for specific events.
            if (timer == 0)
            {
                registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, 0x0100);
            }
            else if (timer == 400)
            {
                registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, 0x0300);
            }
        }
#endif
#endif
    }

    return 0;
}