写了一个简单的，基于Arduino的小车碰触转弯代码，思路：碰触，后退，转向，前进，，仅供学习讨论
#include <AFMotor.h>
AF_Stepper motorG(200,1);
AF_Stepper motorD(200,2);
int impactPinG =0 ;
int impactPinD =2 ;
int seuilimpactG= 10;
int seuilimpactD= 10;
bool murG=false,murD=false;
int marka=0;
void rotation(int degree)
{
int rota;
int num;
if (degree>=0)
{
rota=degree*75*3.14/180/0.65;
for(num=0;num<=rota;num++)
{
motorG.step(1, FORWARD, INTERLEAVE);
motorD.step(1, FORWARD, INTERLEAVE);
}
}
else
{
rota=degree*75*3.14/180/0.65;
for(num=0;num>=rota;num--)
{
motorG.step(1, BACKWARD, INTERLEAVE);
motorD.step(1, BACKWARD, INTERLEAVE);
}
}
}
int FinDeCourse()
{
int impactG,impactD;
impactG=analogRead(impactPinG);
impactD=analogRead(impactPinD);
if (impactG<seuilimpactG&&murG==false)
{
Serial.println("Contact gauche actionne");
murG=true;
return 1;
}
else if (impactD<seuilimpactD&&murD==false)
{
Serial.println("Contact droit actionne");
murD=true;
return 2;
}
else if (impactD>seuilimpactD&&impactG>seuilimpactG)
{
murD=false;
murG=false;
return 0;
}
}
void ligne(int pas)
{
int num=0;
if (pas>=0)
{
for(num; num<pas;num++)
{
motorG.step(1, FORWARD, INTERLEAVE);
motorD.step(1, BACKWARD, INTERLEAVE);
}
}
else
{
for(num; num>pas;num--)
{
motorG.step(1, BACKWARD, INTERLEAVE);
motorD.step(1, FORWARD, INTERLEAVE);
}
}
}
void setup()
{
motorG.setSpeed(30);
motorD.setSpeed(30);
Serial.begin(9600);
pinMode(impactPinD, INPUT);
pinMode(impactPinG, INPUT);
}
void loop()
{
Serial.println(FinDeCourse());
switch (FinDeCourse())
{
case 0:
ligne(2);
break;
case 1:
ligne(-40);
rotation(45);
break;
case 2:
ligne(-40);
rotation(-45);
break;
}
}
|
