Line follower code improvement

[dark_charfi]

Hello guys, I've came here to ask if anyone could help me improve my line tracking code, cuz I've bought a freenove tracking line car 4WD, and when it comes to angles that are 90° or less, it doesn't follow the line anymore, so if anyone coul help me, I'll be so thankful

Here's the code I work with

#define PIN_SERVO      2
#define MOTOR_DIRECTION     0 //If the direction is reversed, change 0 to 1
#define PIN_DIRECTION_LEFT  4
#define PIN_DIRECTION_RIGHT 3
#define PIN_MOTOR_PWM_LEFT  6
#define PIN_MOTOR_PWM_RIGHT 5
#define PIN_SONIC_TRIG    7
#define PIN_SONIC_ECHO    8
#define PIN_IRREMOTE_RECV 9
#define PIN_SPI_CE      9
#define PIN_SPI_CSN     10
#define PIN_SPI_MOSI    11
#define PIN_SPI_MISO    12
#define PIN_SPI_SCK     13
#define PIN_BATTERY     A0
#define PIN_BUZZER      A0
#define PIN_TRACKING_LEFT A1
#define PIN_TRACKING_CENTER A2
#define PIN_TRACKING_RIGHT  A3
#define MOTOR_PWM_DEAD    10

#define TK_STOP_SPEED          1
#define TK_FORWARD_SPEED        (90 + tk_VoltageCompensationToSpeed    )

//define different speed levels
int tk_VoltageCompensationToSpeed;  //define Voltage Speed Compensation
#define TK_TURN_SPEED_LV4       (160 + tk_VoltageCompensationToSpeed   )
#define TK_TURN_SPEED_LV3       (130 + tk_VoltageCompensationToSpeed   )
#define TK_TURN_SPEED_LV2       (-120 + tk_VoltageCompensationToSpeed  )
#define TK_TURN_SPEED_LV1       (-140 + tk_VoltageCompensationToSpeed  )

float batteryVoltage = 0;
bool isBuzzered = false;

void setup() {
  pinsSetup(); //set up pins
  getTrackingSensorVal();//Calculate Voltage speed Compensation
}

void loop() {
  u8 trackingSensorVal = 0;
  trackingSensorVal = getTrackingSensorVal(); //get sensor value

  switch (trackingSensorVal)
  {
    case 0:   //000
      motorRun(TK_FORWARD_SPEED, TK_FORWARD_SPEED); //car move forward
      break;
    case 7:   //111
      motorRun(TK_STOP_SPEED, TK_STOP_SPEED); //car stop
      break;
    case 1:   //001
      motorRun(TK_TURN_SPEED_LV4, TK_TURN_SPEED_LV1); //car turn
      break;
    case 3:   //011
      motorRun(TK_TURN_SPEED_LV3, TK_TURN_SPEED_LV2); //car turn right
      break;
    case 2:   //010
    case 5:   //101
      motorRun(TK_FORWARD_SPEED, TK_FORWARD_SPEED);  //car move forward
      break;
    case 6:   //110
      motorRun(TK_TURN_SPEED_LV2, TK_TURN_SPEED_LV3); //car turn left
      break;
    case 4:   //100
      motorRun(TK_TURN_SPEED_LV1, TK_TURN_SPEED_LV4); //car turn right
      break;
    default:
      break;
  }
}

void tk_CalculateVoltageCompensation() {
  getBatteryVoltage();
  float voltageOffset = 7 - batteryVoltage;
  tk_VoltageCompensationToSpeed = 30 * voltageOffset;
}

//when black line on one side is detected, the value of the side will be 0, or the value is 1
u8 getTrackingSensorVal() {
  u8 trackingSensorVal = 0;
  trackingSensorVal = (digitalRead(PIN_TRACKING_LEFT) == 1 ? 1 : 0) << 2 | (digitalRead(PIN_TRACKING_CENTER) == 1 ? 1 : 0) << 1 | (digitalRead(PIN_TRACKING_RIGHT) == 1 ? 1 : 0) << 0;
  return trackingSensorVal;
}

void pinsSetup() {
  //define motor pin
  pinMode(PIN_DIRECTION_LEFT, OUTPUT);
  pinMode(PIN_MOTOR_PWM_LEFT, OUTPUT);
  pinMode(PIN_DIRECTION_RIGHT, OUTPUT);
  pinMode(PIN_MOTOR_PWM_RIGHT, OUTPUT);
  //define ultrasonic moduel pin
  pinMode(PIN_SONIC_TRIG, OUTPUT);
  pinMode(PIN_SONIC_ECHO, INPUT);
  //define tracking sensor pin
  pinMode(PIN_TRACKING_LEFT, INPUT);
  pinMode(PIN_TRACKING_RIGHT, INPUT);
  pinMode(PIN_TRACKING_CENTER, INPUT);
  setBuzzer(false);
}

void motorRun(int speedl, int speedr) {
  int dirL = 0, dirR = 0;
  if (speedl > 0) {
    dirL = 0 ^ MOTOR_DIRECTION;
  } else {
    dirL = 1 ^ MOTOR_DIRECTION;
    speedl = -speedl;
  }

  if (speedr > 0) {
    dirR = 1 ^ MOTOR_DIRECTION;
  } else {
    dirR = 0 ^ MOTOR_DIRECTION;
    speedr = -speedr;
  }
  speedl = constrain(speedl, 0, 255); // speedl absolute value should be within 0~255
  speedr = constrain(speedr, 0, 255); // speedr absolute value should be within 0~255
  digitalWrite(PIN_DIRECTION_LEFT, dirL);
  digitalWrite(PIN_DIRECTION_RIGHT, dirR);
  analogWrite(PIN_MOTOR_PWM_LEFT, speedl);
  analogWrite(PIN_MOTOR_PWM_RIGHT, speedr);
}

bool getBatteryVoltage() {
  if (!isBuzzered) {
    pinMode(PIN_BATTERY, INPUT);
    int batteryADC = analogRead(PIN_BATTERY);
    if (batteryADC < 614)    // 3V/12V ,Voltage read: <2.1V/8.4V
    {
      batteryVoltage = batteryADC / 1023.0 * 5.0 * 4;
      return true;
    }
  }
  return false;
}

void setBuzzer(bool flag) {
  isBuzzered = flag;
  pinMode(PIN_BUZZER, flag);
  digitalWrite(PIN_BUZZER, flag);
}

void alarm(u8 beat, u8 repeat) {
  beat = constrain(beat, 1, 9);
  repeat = constrain(repeat, 1, 255);
  for (int j = 0; j < repeat; j++) {
    for (int i = 0; i < beat; i++) {
      setBuzzer(true);
      delay(100);
      setBuzzer(false);
      delay(100);
    }
    delay(500);
  }
}

void resetCarAction() {
  motorRun(0, 0);
  setBuzzer(false);
}

[J-M-L]

@dark_charfi

#define PIN_TRACKING_LEFT A1
#define PIN_TRACKING_CENTER A2
#define PIN_TRACKING_RIGHT  A3

Have you confirmed the 3 sensors do work as expected?

[dark_charfi]

yup cuz when it comes to curved lines and angles between 180 and 90 he follows, but the problem is in the 90° angles and less

[PaulRB]

@dark_charfi

angles that are 90° or less, it doesn't follow the line anymore

@dark_charfi

the problem is in the 90° angles and less

Ok, you have told us what it doesn't do when the angle is less than 90° but what does it do?

Also you said "anymore". So it did follow the line before? What changed?

[dark_charfi]

it creates a new path , like if it has a 90° to the right, it turns a little (let's say makes with the original line 135°) then continues forward

[PaulRB]

Is that because none of the 3 sensors are triggered, so trackingSensorVal == 0 and the car continues moving forward?

After a time, won't the line be detected by either the left or right sensors, causing the car to correct it's direction back towards the line?

[dark_charfi]

no it corrects its direction only after going into another straight line that is normal to the one she was 1st on
that's what I remarked after tryin' from several positions

[J-M-L]

How far apart are the sensors and how far from the line?

[dark_charfi]

1.5cm
and the road is 2cm

[PaulRB]

@dark_charfi

corrects its direction only after going into another straight line that is normal to the one she was 1st on

Why does not correct it's direction when the line is detected by the left or right sensors? That's what your code seems to be designed to do. If it can't detect the line at 135° why can it detect at 90°?

I think a diagram would help us understand the situation with more clarity.

[dark_charfi]

the black line is the trajectory and the red one is the movement of the robot

[J-M-L]

Is that what you call your angle (compared to direction of travel)?

and is that what you say is failing? (when the angle is less than 90°)

if so this is what happens when it works

now try to think what happens with the sharp angle

[dark_charfi]

no what I said is that he follows the line if it's straight but when he comes to a 90° or less he doesn't follow anymore (the angles are based on the trajectory)

[dark_charfi]

like that picture

[J-M-L]

so that what should be handled

[dark_charfi]

u can say sth like that

[J-M-L]

The code does this upon the double hit

@dark_charfi

    case 3:   //011
      motorRun(TK_TURN_SPEED_LV3, TK_TURN_SPEED_LV2); //car turn right
      break;

Your decision tree should be smarter

[dark_charfi]

it does it in normal ways, in curves or angles between 180 and 90
but not in the 90 angles and less

[J-M-L]

@dark_charfi

but not in the 90 angles and less

The decisions are very clear in the switch case based on what the sensors reads

Try to manually simulate what the sensors sees and what the motors do and you’ll understand what happens.

[PaulRB]

  case 0:   //000
      motorRun(TK_FORWARD_SPEED, TK_FORWARD_SPEED); //car move forward
      break;

Maybe this part is a mistake. Maybe it is better not to adjust the motor speed if no sensor is triggered. That way, if the car is turning, it will continue to turn and hopefully find the line again.