两个电机的循迹小车的程序求改成四个电机的

maruirui

我这有一个两个电机的循迹小车的程序，有没有大佬能指点我一下将其改成四个电机的？？？多谢！！！



int Left_motor = 8;     //
int Left_motor_pwm = 9;     //
int Right_motor_pwm = 10;     //
int Right_motor = 11;     //
int key = A2;     //
int SL;     //
int SR;     //
int SM;     //
int SB;     //
const int SensorRight = 3;     //
const int SensorLeft = 4;      //
const int SensorMin = 44;      //
const int SensorBack = 5;      //
const int SensorBackLeft = 45;      //
void Motorinit()
{
  //brake(1,0);
  pinMode(Left_motor,OUTPUT);     //
  pinMode(Left_motor_pwm,OUTPUT);     //
  pinMode(Right_motor_pwm,OUTPUT);      //
  pinMode(Right_motor,OUTPUT);      //
  pinMode(key,INPUT);     //
  pinMode(beep,OUTPUT);
  pinMode(47,OUTPUT);
  digitalWrite(47,HIGH);

  pinMode(SensorRight, INPUT);      //
  pinMode(SensorLeft, INPUT);     //
  pinMode(SensorMin, INPUT);      //
  pinMode(SensorBack, INPUT);     //
}
void motor_run(int Delay_time = 0,unsigned char _speed = 120)     //
{
  digitalWrite(Right_motor,LOW);      //
  digitalWrite(Right_motor_pwm,HIGH);     //     
  analogWrite(Right_motor_pwm,_speed);      //

  digitalWrite(Left_motor,LOW);     //
  digitalWrite(Left_motor_pwm,HIGH);      //     
  analogWrite(Left_motor_pwm,_speed);     //
  if(Delay_time != 0)
  {
    delay(Delay_time);
    brake(1,0);
  }
}

void spin_left_first()      //
{
  digitalWrite(Right_motor,LOW);      //
  digitalWrite(Right_motor_pwm,HIGH);  //
  analogWrite(Right_motor_pwm,220);     //

  digitalWrite(Left_motor,HIGH);  //
  digitalWrite(Left_motor_pwm,LOW);  //
  analogWrite(Left_motor_pwm,200);//
}
void spin_right()        //
{
  digitalWrite(Right_motor,HIGH);  //
  digitalWrite(Right_motor_pwm,HIGH);  //
  analogWrite(Right_motor_pwm,170);//  

  digitalWrite(Left_motor,LOW);  //
  digitalWrite(Left_motor_pwm,HIGH);  //
  analogWrite(Left_motor_pwm,190);//
}
void Go_straight(int Speed = 100)
{

    SR = digitalRead(SensorRight);//
    SL = digitalRead(SensorLeft);//

    if (SL == LOW && SR==LOW)
      motor_run(0,Speed);   //
    else if (SL == HIGH & SR == LOW)//
      spin_left_first();
    else if (SL == LOW & SR == HIGH) //  
      spin_right();
    else
    {
        motor_run(0,Speed);  
    }
}
void brake(int flag,int delay_time)         //2  0   1   100
{
  if(flag != 1)
  {
    digitalWrite(Left_motor,HIGH);
    digitalWrite(Right_motor,HIGH);
  }
  else
  {
    digitalWrite(Left_motor,LOW);
    digitalWrite(Right_motor,LOW);
  }
  if(delay_time != 0)
    delay(delay_time);
  if(flag == 1)
  {
    digitalWrite(Left_motor,HIGH);
    digitalWrite(Right_motor,HIGH);
  }
  else
  {
    digitalWrite(Left_motor,LOW);
    digitalWrite(Right_motor,LOW);
  }
  digitalWrite(Right_motor_pwm,255);   
  digitalWrite(Left_motor_pwm,255);  //  
  delay(30);
  digitalWrite(Right_motor_pwm,LOW);  //
  analogWrite(Right_motor_pwm,0);//

  digitalWrite(Left_motor_pwm,LOW);  //
  analogWrite(Left_motor_pwm,0);//
}
//int _Speed = 100
void back(int _Speed = 80)         
{
  digitalWrite(Right_motor,HIGH);  
  digitalWrite(Right_motor_pwm,HIGH);   
  analogWrite(Right_motor_pwm,_Speed);


  digitalWrite(Left_motor,HIGH);  
  digitalWrite(Left_motor_pwm,HIGH);  //  
  analogWrite(Left_motor_pwm,_Speed);//
}
void back1()         
{
  digitalWrite(Right_motor,HIGH);  
  digitalWrite(Right_motor_pwm,HIGH);   
  analogWrite(Right_motor_pwm,70);


  digitalWrite(Left_motor,HIGH);  
  digitalWrite(Left_motor_pwm,HIGH);  //  
  analogWrite(Left_motor_pwm,90);//
}
void back_straight()
{
    SR = digitalRead(SensorBack);//
    SL = digitalRead(SensorBackLeft);//

    if (SL == LOW && SR==LOW)
      back();   //
    else if (SL == HIGH & SR == LOW)//
      spin_right();
    else if (SL == LOW & SR == HIGH) //  
      spin_left_first();
    else
    {
        back();
        delay(100);
    }
}


void keysacn()//
{
  int val;
  val=digitalRead(key);//
  while(!digitalRead(key))//
  {
    val=digitalRead(key);//
  }
  while(digitalRead(key))//
  {
    delay(10);  //
    val=digitalRead(key);//
    if(val==HIGH)  //
    {
      digitalWrite(beep,HIGH);
      while(!digitalRead(key))
      digitalWrite(beep,LOW);
    }
    else
      digitalWrite(beep,LOW);//
  }
}