#include <LedControl.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
// 8x8 Matrix
#define DIN_PIN 11
#define CS_PIN 10
#define CLK_PIN 13
// L298N driver
#define IN1 8
#define IN2 7
#define IN3 5
#define IN4 4
#define ENA 6
#define ENB 3
#define SERVO 9
#define W 0
#define NW 45
#define N 90
#define NE 135
#define E 180
#define TRIG 14
#define ECHO 15
// Serial output constants
#define FORWARD ">F<" // Robot platform moving forward
#define RIGHT ">R<" // Platform rotates RIGHT in-place ~90 deg
#define LEFT ">L<" // Platform rotates LEFT in-place ~90 deg
#define SCAN_FRONT_CLEAR ">SF0<"
#define SCAN_FRONT_OBSTACLE ">SF1<"
#define SCAN_RIGHT_CLEAR ">SR0<"
#define SCAN_RIGHT_OBSTACLE ">SR1<"
#define SCAN_LEFT_CLEAR ">SL0<"
#define SCAN_LEFT_OBSTACLE ">SL1<"
// 8x8 Matrix images as 64-bit long integer
#define FRONT_0 0x18187e7e1818817e
#define FRONT_1 0x00007e7e0000817e
#define RIGHT_0 0x408c8cbfbf8c8c40
#define RIGHT_1 0x408080bfbf808040
#define LEFT_0 0x023131fdfd313102
#define LEFT_1 0x020101fdfd010102
#define SCANING_LEFT 0x0209259595250902
#define SCANING_FRONT 0x18002418423c817e
#define SCANING_RIGHT 0x4090a4a9a9a49040
#define NE_1 0x00003fbf80808078
#define NE_0 0x0c0c3fbf8c8c8078
#define NW_1 0x0000fcfd0101011e
#define NW_0 0x3030fcfd3131011e
#define FORWARD 0x0042663c5a663c18
// #define FORWARD2 0x42663c5a663c1800
#define R_LEFT 0xc0c0e4763f1f0604
// #define R_LEFT2 0xc0c0e87838780000
#define R_RIGHT 0x0303276efcf86020
// #define R_RIGHT2 0x0303171e1c1e0000
SoftwareSerial btSerial(16, 17);
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo;
LedControl display = LedControl(DIN_PIN, CLK_PIN, CS_PIN);
bool goingForward = 0;
bool goingBack = 0;
bool standing = 1;
bool rotatingLeft = 0;
bool rotatingRight = 0;
bool scaning = 0;
bool goodToGo = 0;
bool nextStep = 0;
bool servoIsRotating = 0;
bool servoHasRotated = 0;
int rotation_time = 2000;
int driving_time = 2000;
int servo_rotation = 500;
int pause = 500;
int obstacle_treshold = 30;
byte driving_speed = 200;
byte rotation_speed = 200;
int movements_count = 0;
int rotations = 0;
unsigned long time;
unsigned long pause_start;
unsigned long movement_start;
unsigned long rotation_start;
unsigned long servo_start;
byte rotationAngles[] = {W, N, E};
byte scanResults[] = {3,3,3};
byte scaningStep = 0;
// 0 - scaning LEFT
// 1 - scaning FRONT
// 2 - scaning RIGHT
byte step = 1;
// 1 - scaning left
// 2 - scaning front
// 3 - scaning right
// 4 - movement
byte movements[5] = {0,0,0,0,0};
// 0 - no movement
// 1 - forward
// 2 - right
// 3 - left
void setup() {
Serial.begin(9600);
btSerial.begin(9600);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(TRIG, OUTPUT);
pinMode(ECHO, INPUT);
servo.attach(SERVO);
servo.write(N);
lcd.init();
lcd.backlight();
display.clearDisplay(0);
display.shutdown(0, false);
display.setIntensity(0, 10);
delay(1000);
Serial.println(F("^_^"));
for (int i = 0; i < 3; i++){
Serial.println(rotationAngles[i]);
}
}
void displayImage(uint64_t image) {
for (int i = 0; i < 8; i++) {
byte row = (image >> i * 8) & 0xFF;
for (int j = 0; j < 8; j++) {
display.setLed(0, i, j, bitRead(row, j));
}
}
}
void lcdWrite() {
lcd.setCursor(0, 0);
lcd.print(F("Mov.: Rotat.:"));
lcd.setCursor(0, 1);
lcd.print(movements_count);
lcd.setCursor(9, 1);
lcd.print(rotations);
// 0123456789ABCDEF
// 0 Mov.: Rotat.:
// 1 1 1
}
int ultrasonic() {
//Signala izsutisana no Trig pin
digitalWrite(TRIG, LOW); // Pirms impulsa sutisanas, parliecinamies, ka Trig pin ir izslegts
delayMicroseconds(2);
digitalWrite(TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG, LOW);
//Atbildes impulsa salasisana no Sensora
long ilgums = pulseIn(ECHO, HIGH);
//Attaluma noteiksana
//Izmantojam skanas atrumu 343 metri sekunde
int attalums = (ilgums / 2) * 0.0343;
return attalums;
}
void forward() {
analogWrite(ENA, driving_speed);
analogWrite(ENB, driving_speed);
digitalWrite(IN1, 1);
digitalWrite(IN2, 0);
digitalWrite(IN3, 1);
digitalWrite(IN4, 0);
standing = 0;
}
void backward() {
digitalWrite(IN1, 0);
digitalWrite(IN2, 1);
digitalWrite(IN3, 0);
digitalWrite(IN4, 1);
}
void motorsStop() {
digitalWrite(IN1, 0);
digitalWrite(IN2, 0);
digitalWrite(IN3, 0);
digitalWrite(IN4, 0);
}
void left() {
analogWrite(ENA, rotation_speed);
analogWrite(ENB, rotation_speed);
digitalWrite(IN1, 0);
digitalWrite(IN2, 1);
digitalWrite(IN3, 1);
digitalWrite(IN4, 0);
}
void right() {
analogWrite(ENA, rotation_speed);
analogWrite(ENB, rotation_speed);
digitalWrite(IN1, 1);
digitalWrite(IN2, 0);
digitalWrite(IN3, 0);
digitalWrite(IN4, 1);
}
void changeStep() {
if (++step > 5) step = 0;
nextStep = 0;
}
int scaningDirection() {
int scaningResults[5];
for (int i = 0; i < 5; i++){
scaningResults[i] = ultrasonic();
delay(50);
}
int smallest = scaningResults[0];
for (int i = 1; i < 5; i++){
smallest = min(smallest, scaningResults[i]);
}
return smallest;
}
void scaningProcess() {
if (!servoIsRotating) {
// Serial.print("servoIsRotating? "); Serial.println(servoIsRotating);
if (servoHasRotated) {
servoHasRotated = 0;
int distance = scaningDirection();
// Serial.print(F("Scaned dist: ")); Serial.println(distance);
if (distance >= obstacle_treshold) {
scanResults[scaningStep] = 0;
} else {
scanResults[scaningStep] = 1;
}
if (++scaningStep > 2) {
scaningStep = 0;
scaning = 0;
changeStep();
}
} else {
servo.write(rotationAngles[scaningStep]);
// Serial.print("servo rotates to: "); Serial.println(rotationAngles[scaningStep]);
servo_start = time;
servoIsRotating = 1;
}
}
}
void displayMatrixInfo() {
if (scaning) {
switch (scaningStep) {
case 0:
if (scanResults[scaningStep] > 1) {
displayImage(SCANING_LEFT);
} else if (scanResults[scaningStep] == 0) {
displayImage(LEFT_0);
} else if (scanResults[scaningStep] == 1) {
displayImage(LEFT_1);
}
break;
case 1:
if (scanResults[scaningStep] > 1) {
displayImage(SCANING_FRONT);
} else if (scanResults[scaningStep] == 0) {
displayImage(FRONT_0);
} else if (scanResults[scaningStep] == 1) {
displayImage(FRONT_1);
}
break;
case 2:
if (scanResults[scaningStep] > 1) {
displayImage(SCANING_RIGHT);
} else if (scanResults[scaningStep] == 0) {
displayImage(RIGHT_0);
} else if (scanResults[scaningStep] == 1) {
displayImage(RIGHT_1);
}
break;
default:
break;
}
}
if (goingForward) { displayImage(FORWARD); }
if (rotatingLeft) { displayImage(R_LEFT); }
if (rotatingRight) { displayImage(R_RIGHT); }
}
char previous_rotation = ' ';
void chooseMovement() {
// if going STRAIGHT is not an option,
// default rotation will be RIGHT, then FORWARD, then RIGHT again
// if RIGHT is not available, but LEFT is clear -> turning LEFT, than
// if RIGHT is not available -> turnin LEFT
if (scanResults[1] == 0) {
movements[0] = 1; // Front
goingForward = 1;
} else if (scanResults[2] == 0) {
movements[0] = 2; // RIGHT
movements[1] = 1; // FRONT
movements[2] = 2; // RIGHT
}
// else if () {
// }
}
unsigned long debugTime;
unsigned long lcdRefreshTime;
bool first = 1;
void loop() {
time = millis();
// 1. Scaning LEFT, FRONT and RIGHT
// 2. Choosing, where to go
if (goodToGo) {
switch (step) {
case 1:
scaning = 1;
break;
case 2:
// scaning = 1;
chooseMovement();
break;
default:
break;
}
}
if (Serial.available() > 0) {
char input = Serial.read();
if (input != 10 or input != 13){
switch (input) {
case 'r':
displayImage(RIGHT_0);
break;
default:
displayImage(0);
}
}
}
if (nextStep) {
changeStep();
}
if (scaning) {
scaningProcess();
}
if (pause_start + pause <= time) {
goodToGo = 1;
}
if (servoIsRotating && servo_start + servo_rotation <= time) {
servoIsRotating = 0;
servoHasRotated = 1;
// Serial.println("Servo has stoped!");
}
if (debugTime + 500 <= time) {
debugTime = time;
printDebugInfo();
}
if (lcdRefreshTime + 1000 <= time) {
lcdWrite();
lcdRefreshTime = time;
}
displayMatrixInfo();
// if (time > 10000) {exit(1);}
// if (first) {
// // displayImage(FORWARD);
// displayImage(SCANING_RIGHT);
// first = 0;
// }
}
void printDebugInfo() {
// Serial.print(F("step: ")); Serial.print(step);
// Serial.print(F(" scaning: ")); Serial.print(scaning);
// Serial.print(F("\t")); Serial.println(time);
Serial.print(F("L: ")); Serial.print(scanResults[0]);
Serial.print(F("\tF: ")); Serial.print(scanResults[1]);
Serial.print(F("\tR: ")); Serial.print(scanResults[2]);
Serial.println();
}