智伴卫士——360°智能盲人避障腰带

#include <SoftPWM.h>
#define motorA 11
#define motorB 10
#define motorC 9
#define motorD 8
#define motorE 2
#define motorF 3
void setup(){
  Serial.begin(9600);
  pinMode(A4, OUTPUT);
  pinMode(A0, INPUT);
  pinMode(12, INPUT);
  pinMode(13, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
 
  SoftPWMBegin();
  SoftPWMSet(11, 0);
  SoftPWMSet(10, 0);
  SoftPWMSet(9, 0);
  SoftPWMSet(8, 0);
  SoftPWMSet(2, 0);
  SoftPWMSet(3, 0);
  SoftPWMSetFadeTime(11, 100, 100);
  SoftPWMSetFadeTime(10, 100, 100);
  SoftPWMSetFadeTime(9, 100, 100);
  SoftPWMSetFadeTime(8, 100, 100);
  SoftPWMSetFadeTime(2, 100, 100);
  SoftPWMSetFadeTime(3, 100, 100);
}

void loop(){
  int distanA,distanB,distanC,distanD,distanE,distanF;
  distanA = checkdistanceA();
  checkmotor(distanA,motorA);
  Serial.print("A");
  Serial.print(distanA);
  
  distanB = checkdistanceB();
  checkmotor(distanB,motorB);
  Serial.print("    B");
  Serial.print(distanB);

  distanC = checkdistanceC();
  checkmotor(distanC,motorC);
  Serial.print("    C");
  Serial.print(distanC);

  distanD = checkdistanceD();
  checkmotor(distanD,motorD);
  Serial.print("     D");
  Serial.print(distanD);

  distanE = checkdistanceE();
  checkmotor(distanE,motorE);
  Serial.print("     E");
  Serial.print(distanE);

  distanF = checkdistanceF(); 
  checkmotor(distanF,motorF);
  Serial.print("     F");
  Serial.println(distanF);

  
}

void checkmotor(int distan,int motor)
{
    if(distan<40)
  {
     distan =  map(distan,5,4,255,0);
     SoftPWMSet(motor, distan);
    }
  else SoftPWMSet(motor, 0);
  delay(10);
}
  

float checkdistanceA() {
  digitalWrite(A4, LOW);
  delayMicroseconds(2);
  digitalWrite(A4, HIGH);
  delayMicroseconds(10);
  digitalWrite(A4, LOW);
  float distance = pulseIn(A0, HIGH) / 58.00;
  delay(10);
  return distance;
}

float checkdistanceB() {
  digitalWrite(A4, LOW);
  delayMicroseconds(2);
  digitalWrite(A4, HIGH);
  delayMicroseconds(10);
  digitalWrite(A4, LOW);
  float distance = pulseIn(12, HIGH) / 58.00;
  delay(10);
  return distance;
}

float checkdistanceC() {
  digitalWrite(A4, LOW);
  delayMicroseconds(2);
  digitalWrite(A4, HIGH);
  delayMicroseconds(10);
  digitalWrite(A4, LOW);
  float distance = pulseIn(13, HIGH) / 58.00;
  delay(10);
  return distance;
}

float checkdistanceD() {
  digitalWrite(A4, LOW);
  delayMicroseconds(2);
  digitalWrite(A4, HIGH);
  delayMicroseconds(10);
  digitalWrite(A4, LOW);
  float distance = pulseIn(A1, HIGH) / 58.00;
  delay(10);
  return distance;
}

float checkdistanceE() {
  digitalWrite(A4, LOW);
  delayMicroseconds(2);
  digitalWrite(A4, HIGH);
  delayMicroseconds(10);
  digitalWrite(A4, LOW);
  float distance = pulseIn(A2, HIGH) / 58.00;
  delay(10);
  return distance;
}

float checkdistanceF() {
  digitalWrite(A4, LOW);
  delayMicroseconds(2);
  digitalWrite(A4, HIGH);
  delayMicroseconds(10);
  digitalWrite(A4, LOW);
  float distance = pulseIn(A3, HIGH) / 58.00;
  delay(10);
  return distance;
}