utils.c

来自「一个机器人开发的相关嵌入式开发源码」· C语言 代码 · 共 585 行 · 第 1/2 页

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *  utils.c - utility library for SRV-1 robot
 *    Copyright (C) 2005-2006  Surveyor Corporation
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details (www.gnu.org/licenses)
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include "utils.h"


/*****************************************************************************
 *
 * Description:
 *    Routine for printing integer numbers in various formats. The number is 
 *    printed in the specified 'base' using exactly 'noDigits', using +/- if 
 *    signed flag 'sign' is TRUE, and using the character specified in 'pad' 
 *    to pad extra characters. 
 *
 * Params:
 *    [in] base     - Base to print number in (2-16) 
 *    [in] noDigits - Number of digits to print (max 32) 
 *    [in] sign     - Flag if sign is to be used (TRUE), or not (FALSE) 
 *    [in] pad      - Character to pad any unused positions 
 *    [in] number   - Signed number to print 
 *
 ****************************************************************************/
void
printNumber(unsigned char  base,
            unsigned char  noDigits,
            unsigned char  sign,
            unsigned char  pad,
            unsigned int number)
{
  static unsigned char  hexChars[16] = "0123456789ABCDEF";
  unsigned char        *pBuf;
  unsigned char         buf[32];
  unsigned int        numberAbs;
  unsigned int        count;

  // prepare negative number
  if(sign && (number < 0))
    numberAbs = -number;
  else
    numberAbs = number;

  // setup little string buffer
  count = (noDigits - 1) - (sign ? 1 : 0);
  pBuf = buf + sizeof(buf);
  *--pBuf = '\0';

  // force calculation of first digit
  // (to prevent zero from not printing at all!!!)
  *--pBuf = hexChars[(numberAbs % base)];
  numberAbs /= base;

  // calculate remaining digits
  while(count--)
  {
    if(numberAbs != 0)
    {
      //calculate next digit
      *--pBuf = hexChars[(numberAbs % base)];
      numberAbs /= base;
    }
    else
      // no more digits left, pad out to desired length
      *--pBuf = pad;
  }

  // apply signed notation if requested
  if(sign)
  {
    if(number < 0)
      *--pBuf = '-';
    else if(number > 0)
       *--pBuf = '+';
    else
       *--pBuf = ' ';
  }

  // print the string right-justified
  uart0SendString(pBuf);
}

/*****************************************************************************
 *
 * Description:
 *    Delay execution by a specified number of microseconds by using
 *    timer #1. A polled implementation.
 *
 * Params:
 *    [in] delayInUs - the number of microseconds to delay.
 *
 ****************************************************************************/
void
delayUs(unsigned int delayInUs)
{
  /*
   * setup timer #1 for delay
   */
  TIMER1_TCR = 0x02;          //stop and reset timer
  TIMER1_PR  = 0x00;          //set prescaler to zero
  TIMER1_MR0 = delayInUs * ((CRYSTAL_FREQUENCY * PLL_FACTOR)/ (1000000 * VPBDIV_FACTOR));
  TIMER1_IR  = 0xff;          //reset all interrrupt flags
  TIMER1_MCR = 0x04;          //stop timer on match
  TIMER1_TCR = 0x01;          //start timer
  
  //wait until delay time has elapsed
  while (TIMER1_TCR & 0x01)
    ;
}

/*****************************************************************************
 *
 * Description:
 *    Initialize timer #0 as real time clock (in milliseconds)
 *           read TIMER0_TC to get current time
 *
 *    NOTE - timer will cycle to zero every 49.71 days
 *
 ****************************************************************************/
void
initRTC(void)
{
  TIMER0_TCR = 0x02;          //stop and reset timer
  TIMER0_IR  = 0xff;          //reset all interrrupt flags
  TIMER0_PR = ((CRYSTAL_FREQUENCY * PLL_FACTOR)/ (1000 * VPBDIV_FACTOR));  // prescale for 1ms tics
  TIMER0_TCR = 0x01;          //start timer
}

/*****************************************************************************
 *
 * Description:
 *    Initialize the PWM unit to generate a variable duty cycle signal on
 *    PWM2 and PWM5. 
 *    The function sets initial frequency. Initial duty cucle is set to 0%.
 *
 * Params:
 *    [in] initialFreqValue - the initial frequency value. Value calculated as:
 *
 *                     (crystal frequency * PLL multiplication factor)
 *                     -----------------------------------------------
 *                           (VPBDIV factor * desired frequency)
 *
 ****************************************************************************/
void
initPwm(unsigned int initialFreqValue)
{
  /*
   * initialize PWM
   */
  PWM_PR  = 0x00000000;             //set prescale to 0
  PWM_MCR = 0x0002;                 //counter resets on MR0 match (period time)
  PWM_MR0 = initialFreqValue;       //MR0 = period cycle time
  PWM_MR2 = 0;                      //MR2 = duty cycle control, initial = 0%
  PWM_MR5 = 0;                      //MR5 = duty cycle control, initial = 0%
  PWM_LER = 0x25;                   //latch new values for MR0 and MR2 and MR5
  PWM_PCR = 0x2400;                 //enable PWM2 and PWM5 in single edge control mode
  PWM_TCR = 0x09;                   //enable PWM and Counter

  /*
   * connect signal PWM2 to pin P0.7
   */
  PINSEL0 &= ~0x0000c000;  //clear bits related to P0.7
  PINSEL0 |=  0x00008000;  //connect signal PWM2 to P0.7 (second alternative function)

  /*
   * connect signal PWM5 to pin P0.21
   */
  PINSEL1 &= ~0x00000c00;  //clear bits related to P0.21
  PINSEL1 |=  0x00000400;  //connect signal PWM5 to P0.21 (second alternative function)

  /*
   * connect motor direction signals to pins p0.22 p0.23
   */
  PINSEL1 &=0xffff0fff;
  IODIR0 |= 0x00c00000;
  IOSET0 |= 0x00c00000;
}

/*****************************************************************************
 *
 * Description:
 *    Initialize the PWM unit to generate a servo control signals on
 *    PWM2 and PWM5. 
 *
 *    pulse width ranges from 1.0ms to 2.0ms, and pulse is updated 50 times/sec
 *
 *    initial pulse is set to 1.5ms
 *
 ****************************************************************************/
void
initServos()
{
  /*
   * initialize dual edge PWM
   */
  PWM_PR  = 0x00000000;             //set prescale to 0
  PWM_MCR = 0x0002;                 //counter resets on MR0 match (period time)
  PWM_MR0 = (CRYSTAL_FREQUENCY * PLL_FACTOR)/ (VPBDIV_FACTOR * 50);  // 50H update rate
  PWM_MR1 = 0;                      //MR2 initial = 1.6ms pulse
  PWM_MR2 = (PWM_MR0 * 160) / 2000;
  PWM_MR4 = 0;                      //MR5 initial = 1.5ms pulse
  PWM_MR2 = (PWM_MR5 * 150) / 2000;
  PWM_LER = 0x37;                   //latch new values for MR0 and MR2 and MR5
  PWM_PCR = 0x2424;                 //enable PWM2 and PWM5 in dual edge control mode
  PWM_TCR = 0x09;                   //enable PWM and Counter

  /*
   * connect signal PWM2 to pin P0.7
   */
  PINSEL0 &= ~0x0000c000;  //clear bits related to P0.7
  PINSEL0 |=  0x00008000;  //connect signal PWM2 to P0.7 (second alternative function)

  /*
   * connect signal PWM5 to pin P0.21
   */
  PINSEL1 &= ~0x00000c00;  //clear bits related to P0.21
  PINSEL1 |=  0x00000400;  //connect signal PWM5 to P0.21 (second alternative function)

  /*
   * connect motor direction signals to pins p0.22 p0.23
   */
  PINSEL1 &=0xffff0fff;
  IODIR0 |= 0x00c00000;
  IOSET0 |= 0x00c00000;
}

/*****************************************************************************
 *
 * Description:
 *    Update the duty cycle value of the PWM signal.
 *
 * Params:
 *    [in] dutyValue - the new duty cycle value in percent in increments
 *                     of 0.01% (i.e., 10% = 1000).
 *
 ****************************************************************************/
void
setPwmDutyPercent(int mleft, int mright)
{
  PWM_MR5 = (PWM_MR0 * abs(mright)) / 100;  //update duty cycle - right motor
  PWM_MR2 = (PWM_MR0 * abs(mleft))  / 100;  //update duty cycle - left motor
  PWM_LER = 0x24;                           //latch new values for MR2 and MR5
  
  if (mleft >= 0)
    IOSET0 |= 0x00400000;
  else
    IOCLR0 |= 0x00400000;

  if (mright >= 0)
    IOSET0 |= 0x00800000;
  else
    IOCLR0 |= 0x00800000;
}

/*****************************************************************************
 *
 * Description:
 *    Update the pulse width of servo control signals.
 *
 * Params:
 *    [in] mleft, mright - between 0 and 100 translates to pulse of 1ms to 2ms
 *                         value of 50 generates 1.5ms pulse
 *
 ****************************************************************************/
void
setServos(int mleft, int mright)
{
  if ((mleft < 0) || (mleft > 100))
    mleft = 50;
  if ((mright < 0) || (mright > 100))
    mright = 50;

  PWM_MR2 = (PWM_MR0 * (100+mleft))  / 2000;  //update duty cycle - left motor
  PWM_MR5 = (PWM_MR0 * (100+mright)) / 2000;  //update duty cycle - right motor
  PWM_LER = 0x2424;                           //latch new values for MR2 and MR5
}

/*****************************************************************************
 *
 * Description:
 *    Initialize p0.24 p0.25 p0.26 p0.27 for IR LEDs
 *
 ****************************************************************************/