#include <Arduino.h>
#include <iostream>
#include <cmath>
#include <cstring>
#include <chrono>
#include <thread>
#include <WiFi.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include "esp_camera.h"
#include "SPI.h"
#include "driver/rtc_io.h"
#include <FS.h>
#include <SPIFFS.h>
#include "Arduino.h"
#include "FS.h" // SD Card ESP32
#include "SD.h"
#include "SPI.h"
#include "SD_MMC.h" // SD Card ESP32
#include "soc/soc.h" // Disable brownour problems
#include "soc/rtc_cntl_reg.h" // Disable brownour problems
#include "driver/rtc_io.h"
#include "driver/ledc.h"
#include <EEPROM.h>
#include <SD.h>
using namespace std;
const char* ssid = "Yohai wifi";
const char* password = "yohai123";
const char* host = "192.168.84.67";
const uint16_t port = 11000;
const uint16_t buff_size = 1024;
#define CONFIG_FREERTOS_TASK_STACK_SIZE 65536
// camera config :
#define CAMERA_MODEL_AI_THINKER
#if defined(CAMERA_MODEL_AI_THINKER)
#define PWDN_GPIO_NUM 32
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 0
#define SIOD_GPIO_NUM 26
#define SIOC_GPIO_NUM 27
#define Y9_GPIO_NUM 35
#define Y8_GPIO_NUM 34
#define Y7_GPIO_NUM 39
#define Y6_GPIO_NUM 36
#define Y5_GPIO_NUM 21
#define Y4_GPIO_NUM 19
#define Y3_GPIO_NUM 18
#define Y2_GPIO_NUM 5
#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22
#else
#error "Camera model not selected"
#endif
#define FILE_PHOTO "/photo.png"
camera_config_t config;
esp_err_t err;
class stepper
{
private:
const int high = 1, low = 0;
const int right = 0, left = 1;
const int ms_val = 1;
int sleep_timer = 10;
const int wheels[2][2] = {{2, 4}, {13, 14}};
const float wheel_size = 2 * 2.54; // diameter in inches to cm
const float car_dim = 10;
int right_pin[2], left_pin[2];
public:
stepper()
{
right_pin[0] = wheels[right][0];
right_pin[1] = wheels[right][1];
left_pin[0] = wheels[left][0];
left_pin[1] = wheels[left][1];
}
void move(int r[], int l[] , int mv, float degree)
{
Serial.println(degree);
int times = round(abs(degree) * mv);
if (degree >= 0)
{
if (r != NULL)
digitalWrite(r[1], HIGH);
if (l != NULL)
digitalWrite(l[1], LOW);
}
else{
if (r != NULL)
digitalWrite(r[1], LOW);
if (l != NULL)
digitalWrite(l[1], HIGH);
}
for (int i = 0; i < times ; i++)
{
update(r, l);
delay(sleep_timer);
}
}
void update(int r[], int l[])
{
if (r != NULL)
update_right(r);
if (l != NULL)
update_left(l);
}
void update_right(int r[])
{
digitalWrite(r[0], HIGH);
delayMicroseconds(2);
digitalWrite(r[0], LOW);
}
void update_left(int l[])
{
digitalWrite(l[0], HIGH);
delayMicroseconds(2);
digitalWrite(l[0], LOW);
}
void move_meters(int r[], int l[], int m, float length)
{
float wheel_per = 2 * wheel_size / 2 * M_PI;
float rounds_degree = length / wheel_per * 360;
move(r, l, m, rounds_degree/ 1.8);
}
void rotate_car(int r[], int l[], int m, float degree)
{
float wheel_per = 2 * wheel_size / 2 * M_PI;
float car_per = 2 * car_dim * M_PI;
float length = car_per / 360 * degree;
if (degree >= 0)
{
move_meters(NULL, l , m, length);
}
else{
move_meters(r, NULL, m, -1*length);
}
}
bool in_arr(char arr[], char c, int n)
{
for (int i = 0; i< n; i++)
{
if (arr[i] == c)
return true;
}
return false;
}
char* do_commend(char* data)
{
char* commend = strtok(data, " ");
data = strtok(NULL, " ");
char commend_types[5] = {'d', 'r', 't', 'm'};
char ret[24];
if (!in_arr(commend_types, commend[0], 5))
{
strcpy(ret, "wrong input");
return ret;
}
//std::string str_data(data);
//int data_i = 0;
float data_i=atof(data);
switch (commend[0]){
case 'd':
move_meters(right_pin, left_pin, ms_val, data_i);
Serial.print("finished driving ");
Serial.println(data_i);
strcpy(ret,"moved ");
break;
case 'r':
rotate_car(right_pin, left_pin, ms_val, data_i);
strcpy(ret, "rotated ");
break;
case 't':
sleep_timer = data_i;
strcpy(ret,"changed sleep timer");
break;
}
return ret;
}
};
void connect_to_wifi()
{
WiFi.begin(ssid, password);
Serial.print("Connecting to Wi-Fi");
while (WiFi.status() != WL_CONNECTED) {
delay(1000);
Serial.print(".");
}
Serial.println();
Serial.print("Connected to Wi-Fi. IP address: ");
Serial.println(WiFi.localIP());
}
WiFiClient connect_to_server()
{
Serial.print("trying to connect : ( ");
Serial.print(host);
WiFiClient client;
if (!client.connect(host, port)) {
Serial.println("Failed to connect to server");
return connect_to_server();
}
return client;
}
void input(char* source)
{
string data = "";
int x = 0;
do{
if(Serial.available() > 0)
{
x = Serial.read();
data += (char)x;
Serial.print((char)x);
}
}
while((char)x != '\n');
strcpy(source, data.c_str());
}
void initMicroSDCard() {
// Start the MicroSD card
Serial.println("Mounting MicroSD Card");
if (!SD_MMC.begin()) {
Serial.println("MicroSD Card Mount Failed");
return;
}
uint8_t cardType = SD_MMC.cardType();
if (cardType == CARD_NONE) {
Serial.println("No MicroSD Card found");
return;
}
}
void run_motor()
{
for(int i = 0 ; i < 1000;i++)
{
Serial.println("running motor");
digitalWrite(2, HIGH);
delayMicroseconds(5);
digitalWrite(2, LOW);
delay(10);
}
}
/*void txt_file()
{
capturePhotoSave(FILE_PHOTO);
fs::FS &fs = SD_MMC;
File newfile = fs.open("/newtext.txt", FILE_WRITE);
char buffer[1024] = "this is my text i eant to see what happens";
newfile.write((uint8_t*)buffer, 1024);
newfile.close();
}
void send_image(WiFiClient client)
{
Serial.println("handaling photo");
capturePhotoSave(FILE_PHOTO);
fs::FS &fs = SD_MMC;
File myfile = fs.open(FILE_PHOTO, FILE_READ);
uint8_t buffer[4096];
char temp[1024];
String response;
int size = myfile.size();
size_t count = 0;
Serial.println(size);
try{
client.print(size);
}catch(exception e){
Serial.println("cant send to server");
}
while (myfile.available())
{
size_t n = myfile.read(buffer, sizeof(buffer));
//Serial.print("sizeof(buffer) : ");
//Serial.println(sizeof(buffer));
if (n > 0)
{
try{
client.write(buffer, n);
}catch(exception e){
Serial.println("cant send to server");
}
}
//Serial.print("count : ");
//Serial.println(count);
count+=n;
delay(200);
}
Serial.println("done sending");;
myfile.close();
}*/
stepper car;
void setup()
{
Serial.begin(115200);
pinMode(2, OUTPUT);
pinMode(4, OUTPUT);
pinMode(13, OUTPUT);
pinMode(14, OUTPUT);
digitalWrite(13, LOW);
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
if(psramFound()){
config.frame_size = FRAMESIZE_UXGA; // FRAMESIZE_ + QVGA|CIF|VGA|SVGA|XGA|SXGA|UXGA
config.jpeg_quality = 10;
config.fb_count = 2;
} else {
config.frame_size = FRAMESIZE_SVGA;
config.jpeg_quality = 12;
config.fb_count = 1;
}
if(!SD_MMC.begin()){
Serial.println("SD Card Mount Failed");
return;
}
config.ledc_channel = LEDC_CHANNEL_0;
config.ledc_timer = LEDC_TIMER_0;
config.pin_d0 = Y2_GPIO_NUM;
config.pin_d1 = Y3_GPIO_NUM;
config.pin_d2 = Y4_GPIO_NUM;
config.pin_d3 = Y5_GPIO_NUM;
config.pin_d4 = Y6_GPIO_NUM;
config.pin_d5 = Y7_GPIO_NUM;
config.pin_d6 = Y8_GPIO_NUM;
config.pin_d7 = Y9_GPIO_NUM;
config.pin_xclk = XCLK_GPIO_NUM;
config.pin_pclk = PCLK_GPIO_NUM;
config.pin_vsync = VSYNC_GPIO_NUM;
config.pin_href = HREF_GPIO_NUM;
config.pin_sscb_sda = SIOD_GPIO_NUM;
config.pin_sscb_scl = SIOC_GPIO_NUM;
config.pin_pwdn = PWDN_GPIO_NUM;
config.pin_reset = RESET_GPIO_NUM;
config.xclk_freq_hz = 20000000;
config.pixel_format = PIXFORMAT_JPEG;
err = esp_camera_init(&config);
initMicroSDCard();
SD_MMC.begin("/sdcard", true);
//txt_file();
}
void loop()
{
run_motor();
/*connect_to_wifi();
WiFiClient client = connect_to_server();
while (client.connected())
{
if (client.available())
{
/*String response = client.readStringUntil('\n');
Serial.print("Server response: ");
Serial.println(response);
char temp[1024];
strcpy(temp, response.c_str());
char send[1024] = "done";
if (temp[0] == 'p')
{
send_image(client);
}
else{
char* send = car.do_commend(temp);
client.println(send);
}
// Send response to server
}*/
//}
string commend;
string format = "commend format: (d, r) (number)\npls enter commend , ex : d 10\n>>>";
while (true)
{
Serial.printf(format.c_str());
char temp[1024];
input(temp);
Serial.println(temp);
char* check = car.do_commend(temp);
Serial.println(check);
}
}
/*void capturePhotoSave(char* path ) {
// Take Picture with Camera
// Setup frame buffer
camera_fb_t * fb = esp_camera_fb_get();
if (!fb) {
Serial.println("Camera capture failed");
return;
}
// Save picture to microSD card
fs::FS &fs = SD_MMC;
File file = fs.open(path, FILE_WRITE);
if (!file) {
Serial.println("Failed to open file in write mode");
}
else {
file.write(fb->buf, fb->len); // payload (image), payload length
Serial.printf("Saved file to path: %s\n", path);
}
// Close the file
file.close();
// Return the frame buffer back to the driver for reuse
esp_camera_fb_return(fb);
}*/