📄 ipd_c.htm

📁 一些关于机器人开发的开源项目
💻 HTM
字号:
<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0 Transitional//EN">
<!-- saved from url=(0084)http://www.openservo.com/viewcvs/OpenServo/AVR_OpenServo/ipd.c?revision=1.6&root=cvs -->
<HTML><HEAD>
<META http-equiv=Content-Type content="text/html; charset=gb2312">
<META content="MSHTML 6.00.2900.3020" name=GENERATOR></HEAD>
<BODY><PRE>/*
    Copyright (c) 2006 Michael P. Thompson &lt;mpthompson@gmail.com&gt;

    Permission is hereby granted, free of charge, to any person 
    obtaining a copy of this software and associated documentation 
    files (the "Software"), to deal in the Software without 
    restriction, including without limitation the rights to use, copy, 
    modify, merge, publish, distribute, sublicense, and/or sell copies 
    of the Software, and to permit persons to whom the Software is 
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be 
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 
    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
    DEALINGS IN THE SOFTWARE.

    $Id: ipd.c,v 1.6 2007/02/01 20:05:25 mpthompson Exp $
*/

#include &lt;inttypes.h&gt;

#include "openservo.h"
#include "config.h"
#include "ipd.h"
#include "registers.h"

// Compile following for IPD motion control algorithm.
#if IPD_MOTION_ENABLED

// The minimum and maximum servo position as defined by 10-bit ADC values.
#define MIN_POSITION            (0)
#define MAX_POSITION            (1023)

// The minimum and maximum output.
#define MAX_OUTPUT              (255)
#define MIN_OUTPUT              (-MAX_OUTPUT)

// Static structure for managing position and velocity values.
static int16_t previous_position;

// The accumulator is a structure arranged so that both the most significant
// and least significant words can be indepently accessed.  The accumulator
// maintains 32-bit resolution, but only 16-bits are needed for output.
static union
{
    struct
    {
        int16_t lo;
        int16_t hi;
    } small;
    int32_t big;
} integral_accumulator;


static int16_t fixed_multiply(int16_t component, uint16_t fixed_gain)
// Multiplies the signed 16-bit component by the unsigned 16-bit fixed
// point gain to return a 32-bit output. The result is scaled to account
// for the fixed point value.
{
    int32_t output;

    // Multiply the value by the fixed gain value.
    output = (int32_t) component * (int32_t) fixed_gain;

    // Scale result by 256 to account for fixed point gain.
    output /= 256;

    return (int16_t) output;
}


static inline int16_t integral_accumulator_get(void)
// This function returns the most significant word of the integral gain.
{
    return integral_accumulator.small.hi;
}


static inline void integral_accumulator_update(int16_t command_error, uint16_t integral_gain)
// This function updates the integral accumulator with the command error.
// Note: The command error is maintained with 32 bit precision, although 
// only the upper 16 bits are used for output calculations.
{
    int32_t temp;

    // Multiply the command error by the fixed integral gain value.
    temp = (int32_t) command_error * (int32_t) integral_gain;

    // Add to the accumulator adjusting for multiplication.
    integral_accumulator.big += temp;
}


static inline void integral_accumulator_reset(int16_t new_value)
// This function resets the integral accumulator to a new value.
{
    integral_accumulator.small.hi = new_value;
    integral_accumulator.small.lo = 0;
}


void ipd_init(void)
// Initialize the PID algorithm module.
{
    // Initialize accumulator.
    integral_accumulator.big = 0;
}


void ipd_registers_defaults(void)
// Initialize the PID algorithm related register values.  This is done 
// here to keep the PID related code in a single file.  
{
    // Default gain values.
    registers_write_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO, 0x0400);
    registers_write_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO, 0x0300);
    registers_write_word(REG_PID_IGAIN_HI, REG_PID_IGAIN_LO, 0x4000);

    // Default position limits.
    registers_write_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO, 0x0060);
    registers_write_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO, 0x03A0);

    // Default reverse seek setting.
    registers_write_byte(REG_REVERSE_SEEK, 0x00);
}


int16_t ipd_position_to_pwm(int16_t current_position)
// This function takes the current servo position as input and outputs a pwm
// value for the servo motors.  The current position value must be within the
// range 0 and 1023. The output will be within the range of -255 and 255 with
// values less than zero indicating clockwise rotation and values more than
// zero indicating counter-clockwise rotation.
//
// The feedback approach implemented here was first published in Richard Phelan's
// Automatic Control Systems, Cornell University Press, 1977 (ISBN 0-8014-1033-9)
//
// The theory of operation of this function will be filled in later, but the
// diagram below should gives a general picture of how it is intended to work.
//
//
//                           +&lt;------- bounds checking -------+
//                           |                                |
//             |
⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -