Help with programming. Autonomous Car

[cnfoul]

Hello, I have written this code as I am simulating an autonomous car(on TINKERCAD) that will stop when it detects a person crossing the road with the HC-SR04 Ultrasonic sensor. I expected this code to stop the car when a duration of 1164 was registered and to start moving again once the person had moved out of the way. I am unsure of how to add hysteresis so when the duration is close to the 1164 value the car doesn't keep braking and accelerating. I was also wondering if there is a way to add a range of values of duration for the car so for example if the duration was between 1164 and 2328 the car would reduce speed instead of moving. I have tried but failed to do this so any pointers in the right direction would be greatly appreciated.

// Motor driver with encoder readings
//

#define motorPinA 11
#define motorPinB 6
#define speedPin A0
#define encPinA 2
#define encPinB 4
#define SCALING 35128.6  //Motor speed scaling

//For braking lights (attached to same pin as they turn on at the same time)
#define brakePinLeds 10

//For HC-SRO4 Ultrasonic Sensor
#define trigPin 13
#define echoPin 12

bool motorDir=HIGH;
long pulseCount=0;
int reqSpeed=0;
float actSpeed=0;
unsigned long tA;
unsigned long tB;

//Define PID variables
float Kp=0.87;  //Ku=1.45, Tu=100
float Kd=11.278;
float Ki=0.0168;
float setSpeed=0;
float curError=0;
float oldError=0;
float difError=0;
float intError=0;

void setup()
{
  // Open COMMS channel
  Serial.begin(9600);
  
  
  // Set digital pins used for motor control
  pinMode(motorPinA, OUTPUT);
  pinMode(motorPinB, OUTPUT);
  
  //Set digital pin for braking leds
  pinMode(brakePinLeds, OUTPUT);
  
  //Set pins for HC-SRO4 Ultrasonic Sensor
  pinMode(trigPin,OUTPUT);
  pinMode(echoPin,INPUT);
  
  // Set up interrupts
  pulseCount=0;
  motorDir=HIGH;
  attachInterrupt(digitalPinToInterrupt(encPinA),WheelInterrupt, CHANGE);
  pinMode(encPinB, INPUT);
  
  //Start pulse counter
  pulseCount=0;
  tA=micros();
  delay(40);
}

void loop()
{
  //Ultrasonic sensor
  long duration;
  digitalWrite(trigPin,LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin,HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin,LOW);
  duration=pulseIn(echoPin,HIGH);
  
   //Read Potentiometer
int x = analogRead(speedPin);
reqSpeed= map(x,0,1023,-255,255);

  
  //Calculate speed
  tB=micros();
  actSpeed= SCALING*pulseCount/(tB-tA);
  
  //Calculate errors
  oldError=curError;
  curError=reqSpeed - actSpeed;
  difError=((curError-oldError)*1E3)/(tB-tA);
  intError+=(((curError+oldError)/2)*(tB-tA)/1E3);
  
  //Use PID controller
  setSpeed= Kp*curError+ Kd*difError+ Ki*intError;
  
  // Display results
  PrintValues();
  
  //Set speed limits
  if(setSpeed>255)
    setSpeed=255;
  if (setSpeed<-255)
    setSpeed=-255;
  
 // Set motor speed 
 if (reqSpeed<0)
   motorDir=LOW;
  else
    motorDir=HIGH;
  if((motorDir==HIGH&&actSpeed<-10)||(motorDir==LOW&&actSpeed>10))
  {
    //Apply brakes before changing direction
    Serial.println(" Brakes applied...");
    analogWrite(motorPinA,1);
    digitalWrite(motorPinB,LOW);
    digitalWrite(brakePinLeds,HIGH);
  }
  else if(motorDir==LOW&&reqSpeed<0)
  {
    //Set motor running clockwise
    Serial.println(" Motor running clockwise...");
    digitalWrite(motorPinA,LOW);
    analogWrite(motorPinB,-setSpeed);
    digitalWrite(brakePinLeds,HIGH);
  }
  else if(motorDir==HIGH&&reqSpeed>0&&duration<=1164)//Duration value=20cm 
  {
    //Apply brakes as object detected in path
    Serial.println("Braking due to object in path...");
    analogWrite(motorPinA,1); //Shoul be digitalWrite,LOW but problem with simulator
    digitalWrite(motorPinB,LOW);
    digitalWrite(brakePinLeds,HIGH);
  }
  else if(motorDir==HIGH&&reqSpeed>0&&duration>1164)//Duration value=300cm
  {
    //Set motor running anticlockwise
    Serial.println(" Motor running anticlockwise...");
    analogWrite(motorPinA, setSpeed);
    digitalWrite(motorPinB,LOW);
    digitalWrite(brakePinLeds,LOW);
    
  }
  else
  {
    // Unexpected scenario occurs
    Serial.println(" Unexpected motor control scenario...");
  }
  
  
  // Reset counters
  pulseCount=0;
  tA=micros();
  
  delay(100);
}

// Function checks the state of Pin A when it changes and compares this to Pin B
// to determine if motor is running clockwise or anticlockwise
  void WheelInterrupt()
  {
    int pinStateA= digitalRead(encPinA);
    int pinStateB= digitalRead(encPinB);
    
    if(pinStateA==pinStateB)
      pulseCount++;
    else
      pulseCount--;
  }
void PrintValues()
{
  Serial.print(tB/1000.0);
  Serial.print(", ");
  Serial.print(reqSpeed);
  Serial.print(", ");
  Serial.println(actSpeed);
}

[vaj4088]

Thank you for properly posting your code !

if the duration was between 1164 and 2328 the car would reduce speed instead of moving.

I don't understand this. Maybe you intended "stopping" instead of "moving".

If 1164 is your threshold, then brake if the car is accelerating AND the duration is under, say 1154 (1164 - 10). Don't accelerate unless you are braking AND the duration is over, say 1174 (1164 + 10). There are some details to work out (such as how to get started) but this is the general idea behind hysteresis. The threshold to be used varies depending upon what the car is doing.

I don't want to provide too much detail in case this is homework or a class project.

[cnfoul]

Thank you for your reply. I have sorted this problem now, I was adding a much larger difference between the values which resulted in the simulation giving me some error when the object was placed between the thresholds. Thank you again, I really appreciate your help.