基于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;
   }
}