walkspeed.c.svn-base

来自「AVR单片机下」· SVN-BASE 代码 · 共 105 行

/*                ****ROBOCON 2009 | BUPT TEAM*******
 * ------------------------------------------------------------------------
 * FileName   : walkspeed.c
 * Version    : 1.0
 * Brief      : 控制机器人行走速度，基于promotion驱动器
 * Code by    : leaf
 * Date       : Mar 31, 2009
 * Note       : 使用inline是因为主要都是代理性的调用
 *
 *
 * ------------------------------------------------------------------------
 */
#include "lib_bupt.h"

#ifdef WALKSPEED_H_INCLUDED

/*两个驱动器的端口信息由内部的表态变量来保存*/
static SpeedPair curSpeed;
static Promotion promLeft;
static Promotion promRight;
static Bool      revLeft = False;
static Bool      revRight = False;

/* 左轮反速度 */
inline void setLeftRev( Bool rev )
{
	revLeft = rev;
}
/* 右轮反速度 */
inline void setRightRev( Bool rev )
{
	revRight = rev;
}

/*初始化两个驱动器的设置，指定端口，指定波特率，指定ID*/
inline void walkInit(PromPort pl, PromPort pr, PromId leftId, PromId rightId){
	promInit(&promLeft, pl, leftId);
	promInit(&promRight, pr, rightId);
}

/*获得上次设定的速度*/
inline SpeedPair getCurSpeed(void){
	return curSpeed;
}

/*同时设置两轮速度*/
inline void      setCurSpeed(const SpeedPair *p){
	setLeftSpeed(p->left);
	setRightSpeed(p->right);
}

/*设置左速度*/
inline void      setLeftSpeed(SPEED_T speed){
	curSpeed.left = speed;
	promSpeed(&promLeft, revLeft ? -speed : speed );
}

/*设置右速度*/
inline void      setRightSpeed(SPEED_T speed){
	curSpeed.right = speed;
	promSpeed(&promRight, revRight ? -speed : speed );
}

/*两边用同样的速度*/
inline void      setSameSpeed(SPEED_T speed){
	setRightSpeed(speed);
	setLeftSpeed(speed);
}

/*左位移*/
inline void      moveLeft(DIST_T dist){
	promSendCommand(&promLeft, "HO");
	promPosition( &promLeft, revLeft ? -dist : dist );
}

/*右位移*/
inline void      moveRight(DIST_T dist){
	promSendCommand(&promRight, "HO");
	promPosition( &promRight, revRight ? -dist : dist );
}

/*两边使用相同位移*/
inline void      moveSame(DIST_T dist){
	moveLeft(dist);
	moveRight(dist);
}

/*保持当前位置*/
inline void      holdPosition(void){
	setSameSpeed( 0 );
	setSameSpeed( 0 );
}

/*获取左边的promotion对象*/
inline Promotion* getPromLeft(void){
	return &promLeft;
}

/*获取右边的promotion对象*/
inline Promotion* getPromRight(void){
	return &promRight;
}

#endif