
#include <Servo.h>
#include <Metro.h>
Metro measureDistance = Metro(50)
Metro sweepServo = Metro(20)
unsigned long actualDistance = 0
Servo myservo //创建舵机
int pos = 60
int sweepFlag = 1
int URPWM = 3//PWM输出0-25000us,每50us代表1cm
int URTRIG= 10// PWM trigger pin PWM串口为10
uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01} // distance measure command 距离测量命令
void setup(){ // Serial initialization 串行初始化
myservo.attach(9) //舵机串口为9
Serial.begin(9600)// Sets the baud rate to 9600
SensorSetup()
}
void loop(){
if(measureDistance.check() == 1){
actualDistance = MeasureDistance()
// Serial.println(actualDistance)
// delay(100)
}
if(sweepServo.check() == 1){
servoSweep()
}
}
void SensorSetup(){
pinMode(URTRIG,OUTPUT)// A low pull on pin COMP/TRIG
digitalWrite(URTRIG,HIGH) // Set to HIGH
pinMode(URPWM, INPUT) // Sending Enable PWM mode command 发送使能控制模式命令
for(int i=0i<4i++){
Serial.write(EnPwmCmd[i])
}
}
int MeasureDistance(){// a low pull on pin COMP/TRIG triggering a sensor reading 触发传感器读数
digitalWrite(URTRIG, LOW)
digitalWrite(URTRIG, HIGH) // reading Pin PWM will output pulses读引脚脉宽调制将输出脉冲
unsigned long distance=pulseIn(URPWM,LOW)
if(distance==50000){ // the reading is invalid.阅读无效
Serial.print("Invalid")
}else{
distance=distance/50 // every 50us low level stands for 1cm
}
return distance
}
void servoSweep(){
if(sweepFlag ){
if(pos>=60 &&pos<=120){
pos=pos+1 // in steps of 1 degree 1度角度的转动
myservo.write(pos)// tell servo to go to position in variable 'pos' 告诉舵机转动的角度
}
if(pos>119) sweepFlag = false // assign the variable again 重新分配变量
}else {
if(pos>=60 &&pos<=120){
pos=pos-1
myservo.write(pos)
}
if(pos<61) sweepFlag = true
}
}
////////////////////////////////////////////////////////////
需要加载一个Metro.h的库,这只是调试机器,余下的完全看你的发挥了,加上电机
这是一个超声波避障小车的源程序,可以参考下,用的89C52单片机,舵机控制转角避障。#include<AT89x51.H>
#include <intrins.h>
#define Sevro_moto_pwm P2_7 //接舵机信号端输入PWM信号调节速度
#define ECHO P2_4 //超声波接口定义
#define TRIG P2_5 //超声波接口定义
#define Left_moto_go {P1_0=1,P1_1=0,P1_2=1,P1_3=0} //左边两个电机向前走
#define Left_moto_back {P1_0=0,P1_1=1,P1_2=0,P1_3=1} //左边两个电机向后转
#define Left_moto_Stop {P1_0=0,P1_1=0,P1_2=0,P1_3=0} //左边两个电机停转
#define Right_moto_go {P1_4=1,P1_5=0,P1_6=1,P1_7=0} //右边两个电机向前走
#define Right_moto_back {P1_4=0,P1_5=1,P1_6=0,P1_7=1} //右边两个电机向前走
#define Right_moto_Stop {P1_4=0,P1_5=0,P1_6=0,P1_7=0} //右边两个电机停转
unsigned char const discode[] ={ 0xC0,0xF9,0xA4,0xB0,0x99,0x92,0x82,0xF8,0x80,0x90,0xBF,0xff/*-*/}
unsigned char const positon[3]={ 0xfe,0xfd,0xfb}
unsigned char disbuff[4] ={ 0,0,0,0,}
unsigned char posit=0
unsigned char pwm_val_left = 0//变量定义
unsigned char push_val_left =14//舵机归中,产生约,1.5MS 信号
unsigned long S=0
unsigned long S1=0
unsigned long S2=0
unsigned long S3=0
unsigned long S4=0
unsigned int time=0//时间变量
unsigned int timer=0//延时基准变量
unsigned char timer1=0//扫描时间变量
/************************************************************************/
void delay(unsigned int k) //延时函数
{
unsigned int x,y
for(x=0x<kx++)
for(y=0y<2000y++)
}
/************************************************************************/
void Display(void) //扫描数码管
{
if(posit==0)
{P0=(discode[disbuff[posit]])&0x7f}//产生点
else
{P0=discode[disbuff[posit]]} if(posit==0)
{ P2_1=0P2_2=1P2_3=1}
if(posit==1)
{P2_1=1P2_2=0P2_3=1}
if(posit==2)
{P2_1=1P2_2=1P2_3=0}
if(++posit>=3)
posit=0
}
/************************************************************************/
void StartModule() //启动测距信号
{
TRIG=1
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
_nop_()
TRIG=0
}
/***************************************************/
void Conut(void) //计算距离
{
while(!ECHO)//当RX为零时等待
TR0=1//开启计数
while(ECHO)//当RX为1计数并等待
TR0=0//关闭计数
time=TH0*256+TL0//读取脉宽长度
TH0=0
TL0=0
S=(time*1.7)/100//算出来是CM
disbuff[0]=S%1000/100//更新显示
disbuff[1]=S%1000%100/10
disbuff[2]=S%1000%10 %10
}
/************************************************************************/
//前速前进
void run(void)
{
Left_moto_go //左电机往前走
Right_moto_go //右电机往前走
}
/************************************************************************/
//前速后退
void backrun(void)
{
Left_moto_back //左电机往前走
Right_moto_back //右电机往前走
}
/************************************************************************/
//左转
void leftrun(void)
{
Left_moto_back //左电机往前走
Right_moto_go //右电机往前走
}
/************************************************************************/
//右转
void rightrun(void)
{
Left_moto_go //左电机往前走
Right_moto_back //右电机往前走
}
/************************************************************************/
//STOP
void stoprun(void)
{
Left_moto_Stop //左电机停走
Right_moto_Stop //右电机停走
}
/************************************************************************/
void COMM( void )
{
push_val_left=5//舵机向左转90度
timer=0
while(timer<=4000)//延时400MS让舵机转到其位置
StartModule()//启动超声波测距
Conut()//计算距离
S2=S
push_val_left=23//舵机向右转90度
timer=0
while(timer<=4000)//延时400MS让舵机转到其位置
StartModule()//启动超声波测距
Conut()//计算距离
S4=S
push_val_left=14//舵机归中
timer=0
while(timer<=4000)//延时400MS让舵机转到其位置 StartModule()//启动超声波测距
Conut()//计算距离
S1=Sif((S2<20)||(S4<20)) //只要左右各有距离小于20CM小车后退
{
backrun()//后退
timer=0
while(timer<=4000)
}
if(S2>S4)
{
rightrun()//车的左边比车的右边距离小 右转
timer=0
while(timer<=4000)
}
else
{
leftrun()//车的左边比车的右边距离大 左转
timer=0
while(timer<=4000)
}
} /************************************************************************/
/* PWM调制电机转速 */
/************************************************************************/
/* 左电机调速 */
/*调节push_val_left的值改变电机转速,占空比 */
void pwm_Servomoto(void)
{
if(pwm_val_left<=push_val_left)
Sevro_moto_pwm=1
else
Sevro_moto_pwm=0
if(pwm_val_left>=200)
pwm_val_left=0
}
/***************************************************/
///*TIMER1中断服务子函数产生PWM信号*/
void time1()interrupt 3 using 2
{
TH1=(65536-100)/256//100US定时
TL1=(65536-100)%256
timer++//定时器100US为准。在这个基础上延时
pwm_val_left++
pwm_Servomoto()timer1++//2MS扫一次数码管
if(timer1>=20)
{
timer1=0
Display()
}
}
/***************************************************/
///*TIMER0中断服务子函数产生PWM信号*/
void timer0()interrupt 1 using 0
{
} /***************************************************/ void main(void)
{ TMOD=0X11
TH1=(65536-100)/256//100US定时
TL1=(65536-100)%256
TH0=0
TL0=0
TR1= 1
ET1= 1
ET0= 1
EA = 1delay(100)
push_val_left=14//舵机归中
while(1) /*无限循环*/
{ if(timer>=1000) //100MS检测启动检测一次
{
timer=0
StartModule()//启动检测
Conut()//计算距离
if(S<30) //距离小于20CM
{
stoprun()//小车停止
COMM()//方向函数
}
else
if(S>30) //距离大于,30CM往前走
run()
}
}
}
/**************************************************************************/
下面是头文件:
头文件(一)
/*--------------------------------------------------------------------------
AT89X51.H Header file for the low voltage Flash Atmel AT89C51 and AT89LV51.
Copyright (c) 1988-2002 Keil Elektronik GmbH and Keil Software, Inc.
All rights reserved.
--------------------------------------------------------------------------*/
#ifndef __AT89X51_H__
#define __AT89X51_H__
/*------------------------------------------------
Byte Registers
------------------------------------------------*/
sfr P0 = 0x80
sfr SP = 0x81
sfr DPL = 0x82
sfr DPH = 0x83
sfr PCON = 0x87
sfr TCON = 0x88
sfr TMOD = 0x89
sfr TL0 = 0x8A
sfr TL1 = 0x8B
sfr TH0 = 0x8C
sfr TH1 = 0x8D
sfr P1 = 0x90
sfr SCON = 0x98
sfr SBUF = 0x99
sfr P2 = 0xA0
sfr IE = 0xA8
sfr P3 = 0xB0
sfr IP = 0xB8
sfr PSW = 0xD0
sfr ACC = 0xE0
sfr B = 0xF0
/*------------------------------------------------
P0 Bit Registers
------------------------------------------------*/
sbit P0_0 = 0x80
sbit P0_1 = 0x81
sbit P0_2 = 0x82
sbit P0_3 = 0x83
sbit P0_4 = 0x84
sbit P0_5 = 0x85
sbit P0_6 = 0x86
sbit P0_7 = 0x87
/*------------------------------------------------
PCON Bit Values
------------------------------------------------*/
#define IDL_ 0x01
#define STOP_ 0x02
#define PD_ 0x02 /* Alternate definition */
#define GF0_ 0x04
#define GF1_ 0x08 #define SMOD_ 0x80 /
*------------------------------------------------
TCON Bit Registers
------------------------------------------------*/
sbit IT0 = 0x88
sbit IE0 = 0x89
sbit IT1 = 0x8A
sbit IE1 = 0x8B
sbit TR0 = 0x8C
sbit TF0 = 0x8D
sbit TR1 = 0x8E
sbit TF1 = 0x8F
/*------------------------------------------------
TMOD Bit Values
------------------------------------------------*/
#define T0_M0_ 0x01
#define T0_M1_ 0x02
#define T0_CT_ 0x04
#define T0_GATE_ 0x08
#define T1_M0_ 0x10
#define T1_M1_ 0x20
#define T1_CT_ 0x40
#define T1_GATE_ 0x80
#define T1_MASK_ 0xF0
#define T0_MASK_ 0x0F
/*------------------------------------------------
P1 Bit Registers
------------------------------------------------*/
sbit P1_0 = 0x90
sbit P1_1 = 0x91
sbit P1_2 = 0x92
sbit P1_3 = 0x93
sbit P1_4 = 0x94
sbit P1_5 = 0x95
sbit P1_6 = 0x96
sbit P1_7 = 0x97/
*------------------------------------------------
SCON Bit Registers
------------------------------------------------*/
sbit RI = 0x98
sbit TI = 0x99
sbit RB8 = 0x9A
sbit TB8 = 0x9B
sbit REN = 0x9C
sbit SM2 = 0x9D
sbit SM1 = 0x9E
sbit SM0 = 0x9F
/*------------------------------------------------
P2 Bit Registers
------------------------------------------------*/
sbit P2_0 = 0xA0
sbit P2_1 = 0xA1
sbit P2_2 = 0xA2
sbit P2_3 = 0xA3
sbit P2_4 = 0xA4
sbit P2_5 = 0xA5
sbit P2_6 = 0xA6
sbit P2_7 = 0xA7
/*------------------------------------------------
IE Bit Registers
------------------------------------------------*/
sbit EX0 = 0xA8/* 1=Enable External interrupt 0 */
sbit ET0 = 0xA9/* 1=Enable Timer 0 interrupt */
sbit EX1 = 0xAA/* 1=Enable External interrupt 1 */
sbit ET1 = 0xAB/* 1=Enable Timer 1 interrupt */
sbit ES = 0xAC/* 1=Enable Serial port interrupt */
sbit ET2 = 0xAD/* 1=Enable Timer 2 interrupt */ sbit EA = 0xAF/* 0=Disable all interrupts */
/*------------------------------------------------
P3 Bit Registers (Mnemonics &Ports)
------------------------------------------------*/
sbit P3_0 = 0xB0
sbit P3_1 = 0xB1
sbit P3_2 = 0xB2
sbit P3_3 = 0xB3
sbit P3_4 = 0xB4
sbit P3_5 = 0xB5
sbit P3_6 = 0xB6
sbit P3_7 = 0xB7sbit RXD = 0xB0/* Serial data input */
sbit TXD = 0xB1/* Serial data output */
sbit INT0 = 0xB2/* External interrupt 0 */
sbit INT1 = 0xB3/* External interrupt 1 */
sbit T0 = 0xB4/* Timer 0 external input */
sbit T1 = 0xB5/* Timer 1 external input */
sbit WR = 0xB6/* External data memory write strobe */
sbit RD = 0xB7/* External data memory read strobe */
/*------------------------------------------------
IP Bit Registers
------------------------------------------------*/
sbit PX0 = 0xB8
sbit PT0 = 0xB9
sbit PX1 = 0xBA
sbit PT1 = 0xBB
sbit PS = 0xBC
sbit PT2 = 0xBD
/*------------------------------------------------
PSW Bit Registers
------------------------------------------------*/
sbit P = 0xD0
sbit FL = 0xD1
sbit OV = 0xD2
sbit RS0 = 0xD3
sbit RS1 = 0xD4
sbit F0 = 0xD5
sbit AC = 0xD6
sbit CY = 0xD7
/*------------------------------------------------
Interrupt Vectors:
Interrupt Address = (Number * 8) + 3
------------------------------------------------*/
#define IE0_VECTOR 0 /* 0x03 External Interrupt 0 */
#define TF0_VECTOR 1 /* 0x0B Timer 0 */
#define IE1_VECTOR 2 /* 0x13 External Interrupt 1 */
#define TF1_VECTOR 3 /* 0x1B Timer 1 */
#define SIO_VECTOR 4 /* 0x23 Serial port */ #endif
头文件(二)
/*--------------------------------------------------------------------------
INTRINS.H Intrinsic functions for C51.
Copyright (c) 1988-2002 Keil Elektronik GmbH and Keil Software, Inc.
All rights reserved.
--------------------------------------------------------------------------*/
#ifndef __INTRINS_H__
#define __INTRINS_H__ extern void _nop_ (void)
extern bit _testbit_ (bit)
extern unsigned char _cror_ (unsigned char, unsigned char)
extern unsigned int _iror_ (unsigned int, unsigned char)
extern unsigned long _lror_ (unsigned long, unsigned char)
extern unsigned char _crol_ (unsigned char, unsigned char)
extern unsigned int _irol_ (unsigned int, unsigned char)
extern unsigned long _lrol_ (unsigned long, unsigned char)
extern unsigned char _chkfloat_(float)#endif
这个应该是通过串口发送数据信息的,发送和接收在一根信号线上,手上没有现成的程序,你看看这个在其他网上的行不行,最好根据手册自己写
#include <avr/io.h>
#include <util/delay.h>
void InitUart0(void)
{
UCSR0A = 0x02// 设置为倍速模式
UBRR0H = 0
UBRR0L = 1
UCSR0B = (1<<RXEN)|(1<<TXEN)// 接收器与发送器使能
UCSR0C = (3<<UCSZ0)
DDRE &= ~_BV(PE0)// 初始化RX 端口默认方向为输入
PORTE &= ~_BV(PE0)// 初始化RX 端口默认状态为高阻
DDRE |= _BV(PE1)// 初始化TX 端口默认方向为输出
PORTE |= _BV(PE1)// 初始化TX 端口默认状态为高电平
DDRA |= _BV(PA0)// 初始化使能端口状态方向为输出
PORTA &= ~_BV(PA0)// 初始化使能端口状态为RX 状态
DDRA |= _BV(PA1)// 初始化使能端口状态方向为输出
PORTA |= _BV(PA1)// 初始化使能端口状态方为TX 状态
}
void SendUart0Byte(unsigned char data)
{
while ( !( UCSR0A &(1<<UDRE)) )// 等待发送缓冲器为空
UDR0 = data/* 将数据放入缓冲器,发送数据*/
}
void SetServoLimit(unsigned char id, unsigned short int cw_limit, unsigned short int ccw_limit)
{
unsigned short int temp_ccw = 0// 临时速度,用于进行方向判别
unsigned short int temp_cw = 0
unsigned char temp_ccw_h = 0// 待发送数据h 位
unsigned char temp_ccw_l = 0// 待发送数据l 位
unsigned char temp_cw_h = 0
unsigned char temp_cw_l = 0
unsigned char temp_sum = 0// 校验和寄存变量
if (ccw_limit >1023)
{
temp_ccw = 1023// 限制速度值在可用范围内
}
else
{
temp_ccw = ccw_limit
}
if (cw_limit >1023)
{
temp_cw = 1023
}
else
{
temp_cw = cw_limit
}
temp_ccw_h = (unsigned char)(temp_ccw >>8)
temp_ccw_l = (unsigned char)temp_ccw// 将16bit 数据拆为2个8bit 数据
temp_cw_h = (unsigned char)(temp_cw >>8)
temp_cw_l = (unsigned char)temp_cw// 将16bit 数据拆为2个8bit 数据
PORTA &= ~_BV(PA1)
PORTA |= _BV(PA0)// 使总线处于主机发送状态
UCSR0A |= (1<<TXC0)// 清除UART0写完成标志
SendUart0Byte(0xFF)// 发送启动符号0xFF
SendUart0Byte(0xFF)// 发送启动符号0xFF
SendUart0Byte(id)// 发送id
SendUart0Byte(7)// 发送数据长度为参数长度+2,参数长度为3
SendUart0Byte(0x03)// 命令数据为“WRITE DATA”
SendUart0Byte(0x06)// 舵机控制寄存器首地址
SendUart0Byte(temp_cw_l)// 发送顺时针位置限制低位
SendUart0Byte(temp_cw_h)// 发送顺时针位置限制高位
SendUart0Byte(temp_ccw_l)// 发送逆时针位置限制低位
SendUart0Byte(temp_ccw_h)// 发送逆时针位置限制高位
temp_sum = id + 7 + 0x03 + 0x06 + temp_cw_l + temp_cw_h + temp_ccw_l + temp_ccw_h
temp_sum = ~temp_sum// 计算校验和
SendUart0Byte(temp_sum)// 发送校验和
while ( !( UCSR0A &(1<<TXC0)) ) // 等待发送完成
{ // (Waiting for finishing sending)
}
PORTA |= _BV(PA1)
PORTA &= ~_BV(PA0)// 使总线处于主机接收状态
_delay_ms(2)//送完成后,总线会被从机占用,反馈应答数据,所以进行延时
}
void SetServoPosition(unsigned char id, unsigned short int position, unsigned short int
velocity)
{
unsigned short int temp_velocity = 0// 临时速度,用于进行方向判别
unsigned short int temp_position = 0
unsigned char temp_velocity_h = 0// 待发送数据h 位
unsigned char temp_velocity_l = 0// 待发送数据l 位
unsigned char temp_position_h = 0
unsigned char temp_position_l = 0
unsigned char temp_sum = 0// 校验和寄存变量
if (velocity >1023)
{
temp_velocity = 1023// 限制速度值在可用范围内
}
else
{
temp_velocity = velocity
}
if (position >1023)
{
temp_position = 1023
}
else
{
temp_position = position
}
temp_velocity_h = (unsigned char)(temp_velocity >>8)
// 将16bit 数据拆为2个8bit 数据
temp_velocity_l = (unsigned char)temp_velocity
temp_position_h = (unsigned char)(temp_position >>8)
// 将16bit 数据拆为2个8bit 数据
temp_position_l = (unsigned char)temp_position
PORTA &= ~_BV(PA1)
PORTA |= _BV(PA0)// 使总线处于主机发送状态
UCSR0A |= (1<<TXC0)// 清除UART0写完成标志
SendUart0Byte(0xFF)// 发送启动符号0xFF
SendUart0Byte(0xFF)
SendUart0Byte(id)// 发送id
SendUart0Byte(7)// 发送数据长度为参数长度+2,参数长度为3
SendUart0Byte(0x03)// 命令数据为“WRITE DATA”
SendUart0Byte(0x1E)// 舵机控制寄存器首地址
SendUart0Byte(temp_position_l)// 发送速度数据低位
SendUart0Byte(temp_position_h)// 发送速度数据高位
SendUart0Byte(temp_velocity_l)//发送位置低字节
SendUart0Byte(temp_velocity_h)// 发送位置高字节
temp_sum = id + 7 + 0x03 + 0x1E + temp_position_l + temp_position_h + temp_velocity_l +
temp_velocity_h
temp_sum = ~temp_sum// 计算校验和
SendUart0Byte(temp_sum)// 发送校验和 (Send the checksum)
while ( !( UCSR0A &(1<<TXC0)) ) // 等待发送完成
{ // (Waiting for finishing sending)
}
PORTA |= _BV(PA1)
PORTA &= ~_BV(PA0)// 使总线处于主机接收状态
_delay_ms(2)// 发送完成后,总线会被从机占用,反馈应答数据,所以进行延时
}
void SetServoVelocity(unsigned char id, signed short int velocity)
{
unsigned char temp_sign = 0// 临时符号,用于进行方向判别
unsigned short int temp_velocity = 0// 临时速度,用于进行方向判别
unsigned char temp_value_h = 0// 待发送数据h 位
unsigned char temp_value_l = 0// 待发送数据l 位
unsigned char temp_sum = 0// 校验和寄存变量
if (velocity <0)
{
temp_velocity = -velocity// 如果为负数,则取绝对值
temp_sign = 1// 设置负数符号标志
}
else
{
temp_velocity = velocity
temp_sign = 0// 设置正数符号标志
}
if (temp_velocity >1023)
{
temp_velocity = 1023// 限制速度值在可用范围内
}
temp_velocity |= (temp_sign <<10)
temp_value_h = (unsigned char)(temp_velocity >>8)
// 将16bit 数据拆为2个8bit 数据
temp_value_l = (unsigned char)temp_velocity
PORTA &= ~_BV(PA1)
PORTA |= _BV(PA0)// 使总线处于主机发送状态
UCSR0A |= (1<<TXC0)// 清除UART0写完成标志
SendUart0Byte(0xFF)// 发送启动符号0xFF
SendUart0Byte(0xFF)// 发送启动符号0xFF
SendUart0Byte(id)// 发送id
SendUart0Byte(5)// 发送数据长度为参数长度+2,参数长度为3
SendUart0Byte(0x03)// 命令数据为“WRITE DATA”
SendUart0Byte(0x20)// 舵机控制寄存器首地址
SendUart0Byte(temp_value_l)// 发送速度数据低位
SendUart0Byte(temp_value_h)// 发送速度数据高位
temp_sum = id + 5 + 0x03 + 0x20 + temp_value_l + temp_value_h
temp_sum = ~temp_sum// 计算校验和
SendUart0Byte(temp_sum)// 发送校验和
while ( !( UCSR0A &(1<<TXC0)) ) // 等待发送完成
{
}
PORTA |= _BV(PA1)
PORTA &= ~_BV(PA0)// 使总线处于主机接收状态
_delay_ms(2)// 发送完成后,总线会被从机占用,反馈应答数据,所以进行延时
}
int main(void)
{
InitUart0()
SetServoLimit(2,0,1023)
while(1)
{
_delay_ms(1000)//延时1s
SetServoPosition(2, 1000, 500)//控制舵机以500的速度运动到1000的位置
_delay_ms(1000)//延时1s
SetServoPosition(2, 200, 100)//控制舵机以100的速度运动到200的位置
}
}
欢迎分享,转载请注明来源:内存溢出
微信扫一扫
支付宝扫一扫
评论列表(0条)