示例#1
0
int main(int argc, char *argv[])
{
    int log_taint = 0;
    for (int i = 3; i < argc; i++) {
        if(!strcmp(argv[i], "--main-module")) {
            set_main_module();
            log_info("Whitelisted the main module\n");
        }
        else if(!strcmp(argv[i], "--range")) {
            ADDRINT start = strtoul(argv[++i], NULL, 16);
            ADDRINT end = strtoul(argv[++i], NULL, 16);
            add_instrument_range(start, end);
            log_info("Whitelisted range 0x%08x..0x%08x\n", start, end);
        }
        else if(!strcmp(argv[i], "--ins")) {
            log_info("Whitelisted mnemonic(s): %s\n", argv[++i]);
            add_instrument_instruction(argv[i]);
        }
        else if(!strcmp(argv[i], "--inside")) {
            ADDRINT start = strtoul(argv[++i], NULL, 16);
            ADDRINT end = strtoul(argv[++i], NULL, 16);
            add_instrument_inside(start, end);
            log_info("Whitelisted inside sequence 0x%08x..0x%08x\n",
                start, end);
        }
        else if(!strcmp(argv[i], "--module")) {
            add_instrument_module(argv[++i]);
            log_info("Whitelisted modules containing: %s\n", argv[i]);
        }
    }

    PIN_Init(argc, argv);
    PIN_InitSymbols();

    registers_init();
    taint_init(log_taint);

    taint_source_enable("getc");
    taint_source_enable("scanf");

    IMG_AddInstrumentFunction(&module_range_handler, NULL);
    IMG_AddInstrumentFunction(&taint_sources_handler, NULL);
    INS_AddInstrumentFunction(&insns, NULL);

    PIN_StartProgram();
    return 0;
}
示例#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 项目: 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;
}
示例#4
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;
}