基于Arduino的小车碰触转弯代码 -

标题: 基于Arduino的小车碰触转弯代码 [打印本页]

作者: xinRo 时间: 2018-11-27 16:04
标题: 基于Arduino的小车碰触转弯代码
写了一个简单的，基于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;
  }
}

欢迎光临 (http://www.51hei.com/bbs/)