相關例程
#include <Servo.h>
#define FORW 0//前進
#define BACK 1//後退
Servo servo1;
Servo servo2;
#define E1 5//速度端口
#define E2 6
#define M1 4//方向端口
#define M2 7
#define GP2D12_r 0 //右 接模擬口PIN0
#define GP2D12_c 1 //中
#define GP2D12_l 2 //左
#define R 5 //環境光傳感器 接模擬口PIN5
#define LED 13//LED燈 接數字口PIN13
void MOTO_control(int M1_S,int M2_S,int M1_DIR,int M2_DIR) //電機控制
{
//////////M1////////////////////////
if(FORW == M1_DIR)
digitalWrite(M1,LOW);
else
digitalWrite(M1,HIGH);
if(0 == M1_S)
digitalWrite(E1,LOW);
else
analogWrite(E1,M1_S);
///////////M2//////////////////////
if(FORW == M2_DIR)
digitalWrite(M2,LOW);
else
digitalWrite(M2,HIGH);
if(0 == M2_S)
digitalWrite(E2,LOW);
else
analogWrite(E2,M2_S);
}
void setup()
{
int i;
for(i=4;i<=7;i++)
pinMode(i, OUTPUT);
servo1.attach(9);//定義舵機控制口數字口PIN9
servo2.attach(10);
Serial.begin(19200);//設置波特率 APC220波特率需要和這個一致
}
void loop()
{
int dataLen = 0; //數據長度
int start_flag = 0; //起動標誌位
int data_index=0;
int com_index=0; //索引初始化
int temp_char1;
int temp_char0;
int i,unm,x,y;
int state; //幀狀態
int SUM1,SUM2; //效驗和
int MOTO[7],RC[9]; //電機控制寄存器和舵機控制寄存器
int L_ir,C_ir,R_ir; //紅外線寄存器
int Serial_flag,Serial_unm;
while(1)
{
int val;
L_ir=analogRead(GP2D12_l);
C_ir=analogRead(GP2D12_c);
R_ir=analogRead(GP2D12_r);
val=analogRead(R);
if(val<600)digitalWrite(LED,HIGH);//LED模塊< 食人魚>
else digitalWrite(LED,LOW);
temp_char1 = Serial.read(); //讀取串口數據
if(temp_char1!=-1)
{
Serial_flag=1;
Serial_unm=0;
if(start_flag == 0) //等待數據頭 0X55,0XAA
{
if( temp_char1 == 170 ) // 0XAA
{
if(temp_char0 == 85) //0X55
{
start_flag = 1; //收到數據頭,表示一個數據包傳送開始
MOTO[0]=85;
MOTO[1]=170;
RC[0]=85;
RC[1]=170;
data_index = 0;
com_index = 0;
}
}
else temp_char0 = temp_char1;
}
else if( com_index < 2)
{
switch(com_index)
{
case 0 : MOTO[2] = temp_char1;
RC[2] = temp_char1;
dataLen = temp_char1;
break;
case 1 : MOTO[3] = temp_char1;
RC[3] = temp_char1; //第2個數據,讀取命令字
break;
}
com_index++;
}
else if(data_index < dataLen && MOTO[3]==2) //存儲數據
{
MOTO[data_index+4] = temp_char1;
data_index ++;
}
else if(data_index < dataLen && RC[3]==1) //存儲數據
{
RC[data_index+4] = temp_char1;
data_index ++;
}
else
{
MOTO[6] = temp_char1;
RC[8] = temp_char1;
state=1; // 一幀接收完畢
start_flag = 0;
}
}
else
{
Serial_unm++;
if(Serial_unm==2000){Serial_flag=0;Serial_unm=0;}
if(Serial_flag==0)
{
if(C_ir<250&& L_ir<250 && R_ir<250)
{
MOTO_control(150,150,FORW,FORW);
delay(200);
}
else if((C_ir<250||C_ir>250)&& L_ir>300 && R_ir<250)
{
MOTO_control(150,150,BACK,FORW);
delay(600);
}
else if((C_ir<250||C_ir>250)&& L_ir<250 && R_ir>300)
{
MOTO_control(150,150,FORW,BACK);
delay(800);
}
else if(C_ir>250&& L_ir>250 && R_ir>250)
{
MOTO_control(150,150,BACK,BACK);
delay(1000);
}
}
}
if(state==1)
{
state=0;
if(MOTO[3]==2)
{
SUM1=0;
for(i=0; i<6; i++)SUM1+=MOTO[i];
SUM1%=256;
if(SUM1==MOTO[6])
{
if(MOTO[4]==128&&MOTO[5]==128){MOTO_control(0,0,FORW,FORW);delay(100);}
else if(MOTO[4]<128&&MOTO[5]>128)MOTO_control(150- MOTO[4],MOTO[5],BACK,FORW);//監控軟件上的控制球發來的速度小，所以用150減了一下
else if(MOTO[4]<128&&MOTO[5]<128)MOTO_control(150-MOTO[4],150-MOTO[5],FORW,FORW);
else if(MOTO[4]>128&&MOTO[5]<128)MOTO_control(MOTO[4],150-MOTO[5],FORW,BACK);
else if(MOTO[4]>128&&MOTO[5]>128)MOTO_control(MOTO[4],MOTO[5],BACK,BACK);
delay(40);
}
}
if(RC[3]==1)
{
SUM2=0;
for(i=0; i<8; i++)SUM2+=RC[i];
SUM2%=256;
if(SUM2==RC[8])
{
if(RC[4]>=150)x=150;
else if(RC[4]<=30)x=30;
else x=RC[4];
if(RC[6]>=150)y=150;
else if(RC[6]<=30)x=30;
else y=RC[6];
servo1.write(x);
servo2.write(y);
}
}
}
}
}
相關視頻
http://v.youku.com/v_show/id_XNjQxMzc3MDQ=.html
