Keyestudio 4wd BT Car Lesson 17 Bluetooth Multifunctional Car KS0559

sketch.ino

// *******************************************************************************
// Adapted from https://docs.keyestudio.com/projects/KS0559/en/latest/Arduino/arduino.html
/*
Keyestudio 4wd BT Car
Lesson 17
Bluetooth Multifunctional Car
http://www.keyestudio.com
*/

#define HAS_LED // Comment out if the LED module is not attached to D9

const int IIC_DELAY = 3; // Microseconds

typedef byte matrix_t[16];

// Matrix data pattern: (16 width x 8 height, column major, little endian) https://dotmatrixtool.com
const matrix_t START   = {0x00,0x00,0x3C,0x46,0x42,0x52,0x72,0x00,0x3C,0x42,0x42,0x42,0x3C,0x00,0x5E,0x00};
const matrix_t FORWARD = {0x00,0x00,0x00,0x00,0x00,0x24,0x12,0x09,0x12,0x24,0x00,0x00,0x00,0x00,0x00,0x00};
const matrix_t REVERSE = {0x00,0x00,0x00,0x00,0x00,0x24,0x48,0x90,0x48,0x24,0x00,0x00,0x00,0x00,0x00,0x00};
const matrix_t LEFT    = {0x00,0x00,0x00,0x00,0x00,0x00,0x44,0x28,0x10,0x44,0x28,0x10,0x44,0x28,0x10,0x00};
const matrix_t RIGHT   = {0x00,0x10,0x28,0x44,0x10,0x28,0x44,0x10,0x28,0x44,0x00,0x00,0x00,0x00,0x00,0x00};
const matrix_t STOP    = {0x2E,0x2A,0x3A,0x00,0x02,0x3E,0x02,0x00,0x3E,0x22,0x3E,0x00,0x3E,0x0A,0x0E,0x00};
const matrix_t CLEAR   = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
const matrix_t ACCEL   = {0x00,0x40,0x20,0x10,0x08,0x04,0x02,0xFF,0x02,0x04,0x08,0x10,0x20,0x40,0x00,0x00};
const matrix_t DECEL   = {0x00,0x02,0x04,0x08,0x10,0x20,0x40,0xFF,0x40,0x20,0x10,0x08,0x04,0x02,0x00,0x00};
const matrix_t EGG     = {0x3E,0x40,0x3E,0x40,0x3E,0x00,0x7E,0x4A,0x00,0x7E,0x4A,0x00,0x7E,0x4A,0x00,0x5E};
const matrix_t FULL    = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
const matrix_t SEMI    = {0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA};
const matrix_t SEMIINV = {0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55};

const int LEFT_CTRL  = 2; // Define the direction control pins of group B motor
const int LEFT_PWM   = 5; // Define the PWM control pins of group B motor
const int RIGHT_CTRL = 4; // Define the direction control pins of group A motor
const int RIGHT_PWM  = 6; // Define the PWM control pins of group A motor
const int SCL_PIN   = A5; // Clock pin is A5
const int SDA_PIN   = A4; // Data pin is A4
const int SERVO_PIN = A3; // Servo pin is A3
const int L_PIN     = 11; // Left tracking sensor pin is D11
const int M_PIN     =  7; // Middle tracking sensor pin is D7
const int R_PIN     =  8; // Right tracking sensor pin is D8
const int TRIG_PIN  = 12; // TRIG Pin is D12
const int ECHO_PIN  = 13; // ECHO Pin is D13

int speed = 150; // Set the initial speed to 150
char command;

#ifdef HAS_LED
const int LED_PIN   = 9;
const int LED_DELAY = 500; // ms
// https://www.instructables.com/Arduino-Periodic-Interrupt
volatile bool led_event = true;
bool led_state = false;
ISR( TIMER0_COMPA_vect ) {
  static unsigned count = 0;
  if (++count > LED_DELAY) {
    count = 0;
    led_event = true; // set every LED_DELAY ms, must be cleared in loop()
  }                           
}
#endif

void setup() {
  Serial.begin(9600);          // Set baud rate to 9600

  pinMode(LEFT_CTRL, OUTPUT);  // Set direction control pins of group B motor to OUTPUT
  pinMode(LEFT_PWM, OUTPUT);   // Set PWM control pins of group B motor to OUTPUT
  pinMode(RIGHT_CTRL, OUTPUT); // Set direction control pins of group A motor to OUTPUT
  pinMode(RIGHT_PWM, OUTPUT);  // Set PWM control pins of group A motor to OUTPUT
  servo_pulse(90);             // Set angle of the servo to 90 degrees

  delay(300);

  pinMode(L_PIN, INPUT);     // Tracking sensor pins are configured for input mode
  pinMode(M_PIN, INPUT);     // Tracking sensor pins are configured for input mode
  pinMode(R_PIN, INPUT);     // Tracking sensor pins are configured for input mode
  pinMode(TRIG_PIN, OUTPUT); // Define TRIG as the output mode
  pinMode(ECHO_PIN, INPUT);  // Define ECHO as the input mode
  pinMode(SCL_PIN, OUTPUT);  // Set the clock pin to output
  pinMode(SDA_PIN, OUTPUT);  // Set the data pin to output

  matrix_update(START);

#ifdef HAS_LED
  pinMode(LED_PIN, OUTPUT);

  // https://www.instructables.com/Arduino-Periodic-Interrupt
  cli();                 // Turn off all interrupts
  // TIMSK0 = 0;            // Turn off timer0 for lower jitter, which disables delay()
  OCR0A = 0xBB;          // Arbitrary interrupt count
  TIMSK0 |= _BV(OCIE0A); // Piggy back onto interrupt
  sei();                 // Turn interrupts back on  
#endif
}

void loop() {
#ifdef HAS_LED
  if (led_event) {
    led_event = false;
    led_state = !led_state;
    digitalWrite(LED_PIN, led_state);
  }
#endif

  if (Serial.available()) read_command();

  if (!isprint(command)) return;

  switch (command) {
    case 'F':
      car_forward();
      matrix_update(FORWARD);
      break;
    case 'B':
      car_reverse();
      matrix_update(REVERSE);
      break;
    case 'L':
      car_left();
      matrix_update(LEFT);
      break;
    case 'R':
      car_right();
      matrix_update(RIGHT);
      break;
    case 'S':
      car_stop();
      matrix_update(STOP);
      break;
    case 'a':
      accelerate();
      matrix_update(ACCEL);
      break;
    case 'd':
      decelerate();
      matrix_update(DECEL);
      break;
    case 'U': // Enter follow mode
      follow();
      break;
    case 'Y': // Enter obstacle avoidance mode
      avoid();
      break;
    case 'G': // Enter confinement mode
      confinement();
      break;
    case 'X': // Enter tracking mode
      tracking();
      break;
    case 'M': // Easter egg
      easter_egg();
      break;
    case 'T': // Matrix display test
      matrix_test();
      break;
    default:
      Serial.print("Error - unknown command: "); print_char(command); Serial.println();
  }
}

void set_motor_directions(int l_dir, int r_dir, int new_speed) {
  digitalWrite(LEFT_CTRL, l_dir);
  analogWrite(LEFT_PWM, l_dir ? 255-new_speed : new_speed);
  digitalWrite(RIGHT_CTRL, r_dir);
  analogWrite(RIGHT_PWM, r_dir ? 255-new_speed : new_speed);
}

void car_forward() {
  Serial.println("Forward");
  set_motor_directions(HIGH, HIGH, speed);
}

void car_reverse() {
  Serial.println("Reverse");
  set_motor_directions(LOW, LOW, speed);
}

void car_left() {
  Serial.println("Left");
  set_motor_directions(LOW, HIGH, speed);
}

void car_right() {
  Serial.println("Right");
  set_motor_directions(HIGH, LOW, speed);
}

void car_stop() {
  Serial.println("Stop");
  set_motor_directions(LOW, LOW, 0);
}

void accelerate() { // Speed up
  Serial.println("Accelerating");
  do {
    if (speed == 255) continue; // Speed maximum is 255

    matrix_update(ACCEL);
    ++speed;
    Serial.print("Speed: "); Serial.println(speed); // Display new speed
    delay(10);
    // Receive 'S', the car stops accelerating
  } while (read_command() != 'S');
  Serial.println("Done accelerating");
}

void decelerate() { // Slow down
  Serial.println("Slowing");
  do {
    if (speed == 0) continue; // Speed minimum is 0
    
    matrix_update(DECEL);
    --speed;
    Serial.print("Speed: "); Serial.println(speed, DEC); // Display new speed
    delay(10);
    // Receive 'S', the car stops decelerating
  } while (read_command() != 'S');
  Serial.println("Done slowing");
}

int get_distance() {
  digitalWrite(TRIG_PIN, LOW);     // Send pulse through Trig/Pin, trigger HC-SR04 ranging, so that send out ultrasonic signal interface low level 2μs
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);    // Make ultrasonic signal interface high level 10μs, here is at least 10μs
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);     // Keep the ultrasonic signal interface low level
  int distance = pulseIn(ECHO_PIN, HIGH) / 58; // Read the pulse time and convert the pulse time to the distance (unit: cm)
  Serial.print("Distance: "); Serial.println(distance, DEC);

  return distance;
}

void follow() {
  Serial.println("Entered follow mode");
  servo_pulse(90);
  delay(200);
  do {
    int distance = get_distance();
    if (distance < 8) { // distance [0..8)
      car_reverse();
      matrix_update(REVERSE);
    } else if (distance < 13) { // distance [8..13)
      car_stop();
      matrix_update(STOP);
    } else if (distance <= 35) { // distance [13..35]
      car_forward();
      matrix_update(FORWARD);
    } else { // distance > 35
      car_stop();
      matrix_update(STOP);
    }
    // When S is received, the car stops
  } while (read_command() != 'S');
  car_stop();
  Serial.println("Exited follow mode");
}

void avoid() {
  Serial.println("Entering avoid mode");
  do {
    int distance = get_distance(); // Call the ranging function
    if (distance > 0 && distance < 20) {
      car_stop();
      matrix_update(STOP);

      delay(1000);
      servo_pulse(160); // Bring the steering gear to 180 degrees
      delay(500);
      int distance_l = get_distance(); // Get the left distance
      delay(100);
      servo_pulse(20); // Turn the steering gear to 0 degrees
      delay(500);
      int distance_r = get_distance(); // Get the right distance
      delay(100);

      if (distance_l > distance_r) { // Compare the distance
        car_left();
        matrix_update(LEFT);
        servo_pulse(90); // Steering gear returns to 90 degrees
        delay(700);
        matrix_update(FORWARD);
      } else { // right >= left
        car_right();
        matrix_update(RIGHT);
        servo_pulse(90); // Steering gear returns to 90 degrees
        delay(700);
        matrix_update(FORWARD);
      }
    } else { // When the front distance <= 10cm
      car_forward();
      matrix_update(FORWARD);
    }
    // When S is received, the car stops
  } while (read_command() != 'S');
  car_stop();
  Serial.println("Exited avoid mode");
}

void confinement() {
  Serial.println("Entering confinement mode");
  do {
    int l = digitalRead(L_PIN); // Read the value of the left sensor
    int m = digitalRead(M_PIN); // Read the value of the middle sensor
    int r = digitalRead(R_PIN); // Read the value of the right sensor
    if (!l && !m && !r) // The car goes forward when no black line is detected
      car_forward();
    else { // If any of the tracking sensors detect a black line, the goes back and then turns left
      car_reverse();
      delay(500);
      car_left();
      delay(800);
    }
    // When S is received, the car stops
  } while (read_command() != 'S');
  car_stop();
  Serial.println("Exited confinement mode");
}

void tracking() {
  Serial.println("Entering tracking mode");
  do {
    int l = digitalRead(L_PIN); // Read the value of the left sensor
    int m = digitalRead(M_PIN); // Read the value of the middle sensor
    int r = digitalRead(R_PIN); // Read the value of the right sensor
    if (m) { // Black line detected in the middle
      if (l && !r) // If a black line is detected on the left, but not on the right, turn left
        car_left();
      else if (!l && r) // Otherwise, if a black line is detected on the right and not on the left, turn right
        car_right();
      else // Otherwise, the car goes forward
        car_forward();
    } else { // No black lines detected in the middle
      if (l && !r) // If a black line is detected on the left, but not on the right, turn left
        car_right();
      else if (!l && r) // Otherwise, if a black line is detected on the right and not on the left, turn right
        car_right();
      else // Otherwise, stop
        car_stop();
    }
    // When S is received, the car stops
  } while (read_command() != 'S');
  car_stop();
  Serial.println("Exited tracking mode");
}

void easter_egg() {
  Serial.println("Easter egg located!");
  matrix_update(EGG);
}

void matrix_test() {
  Serial.println("Matrix test");
  matrix_update(FULL);
  delay(3000);
  matrix_update(SEMI);
  delay(1000);
  matrix_update(SEMIINV);
  delay(1000);
  matrix_update(CLEAR);
  Serial.println("Matrix test done");
}

void servo_pulse(int angle) { // Steering gear running angle
  int pulse_width = 11*angle + 500;

  for (int i = 0; i < 30; ++i) {
    digitalWrite(SERVO_PIN, HIGH);
    delayMicroseconds(pulse_width);
    digitalWrite(SERVO_PIN, LOW);
    delay(20 - pulse_width/1000);
  }
}

void matrix_update(const matrix_t matrix_values) { // Update dot matrix display
  IIC_start();                  // The function that calls the data transfer start condition
  IIC_send(0xC0);               // Select matrix address
  for (unsigned i = 0; i < sizeof(matrix_t); ++i)
    IIC_send(matrix_values[i]); // Transmit the data of the pattern
  IIC_end();                    // End pattern data transmission

  IIC_start();
  IIC_send(0x8A);               // Display control, select 4/16 pulse width
  IIC_end();
}

void IIC_start() { // Conditions under which data transmission begins
  digitalWrite(SDA_PIN, HIGH);
  digitalWrite(SCL_PIN, HIGH);
  delayMicroseconds(IIC_DELAY);
  digitalWrite(SDA_PIN, LOW);
  delayMicroseconds(IIC_DELAY);
  digitalWrite(SCL_PIN, LOW);
}

void IIC_end() { // Indicates the end of data transmission
  digitalWrite(SCL_PIN, LOW);
  digitalWrite(SDA_PIN, LOW);
  delayMicroseconds(IIC_DELAY);
  digitalWrite(SCL_PIN, HIGH);
  delayMicroseconds(IIC_DELAY);
  digitalWrite(SDA_PIN, HIGH);
  delayMicroseconds(IIC_DELAY);
}

void IIC_send(byte send_data) { // Transmit data
  for (byte mask = 1; mask; mask <<= 1) { // Each byte has 8 bits and is checked bit by bit starting at the lowest level
    // Sets the high and low levels of SDA_Pin depending on whether each bit of the byte is a 1 or a 0
    digitalWrite(SDA_PIN, (send_data & mask) ? HIGH : LOW);
    delayMicroseconds(IIC_DELAY);
    digitalWrite(SCL_PIN, HIGH); // Pull the clock pin SCL high to stop data transmission
    delayMicroseconds(IIC_DELAY);
    digitalWrite(SCL_PIN, LOW); // Pull the clock pin SCL low to change the SIGNAL of SDA
  }
}

void print_char(char c) {
  Serial.print("'");
  Serial.write(c);
  Serial.print("' DEC ");
  Serial.print(c, DEC);
  Serial.print(", HEX ");
  Serial.print(c, HEX);
}

char read_command() {
  command = Serial.read();
  if (!isprint(command)) return command;

  Serial.print("Received command: ");
  print_char(command);
  Serial.println();
  return command;
}

// ********************************* End of File *********************************