I have an idea, I'd like to make a table that could be leveled by a car around one axis. Actually it would be very similar to a balancing robot.
I already build both the table and car but now I am stuck with the leveling part of my project.
I though it is working like the balancing robot, but in my project I should detect when the table started to tilt, because it always starts from 5 or -5 degree, and when the car reaches the deadlock then the table tilts. So the only difference between my project and balancing robot is that in my project the car should move in reverse when the car that balance reaches the deadlock.
My question would be that how could I detect that moment when the car reached the deadlock? I have already tried to compare the last angle with the corrent angle but it has not really worked because my gyroscope gives the angle values very fast.
Here is my code that I wrote already:
#include <Arduino.h>
#include <PID_v1.h>
#include <espnow.h>
#include <ESP8266WiFi.h>
#include <Ticker.h>
#define FORWARD 1
#define BACKWARD 2
#define STOP 3
void motor_stop();
void motor_forward();
void motor_backward();
void print_debuginformation();
double Angle_to_send, Angle;
int roundedAngle, lastAngle;
boolean run = false;
//Motor A,L
const int A_motor_PWM = 16;//D0
const int A_motor_forwardpin = 13;//D7
const int A_motor_backwardpin = 15;//D8
//Motor B,R
const int B_motor_PWM = 12;//D6
const int B_motor_forwardpin = 5;//D1
const int B_motor_backwardpin = 0;//D3
int direction;
//ESP-NOW
void OnDataRecv(uint8_t * mac, uint8_t *incomingData, uint8_t len) {
memcpy(&Angle_to_send, incomingData, sizeof(Angle_to_send));
//Serial.print("Fogadott "); Serial.println(Angle_to_send);
}
//PID
//double kp =5.3, ki = 0.1, kd = 0.25; //kp=5.3 ki=0.1 kd=0.25
//PID deltaPWM_PID(&error, &deltaPWM, &desired, kp, ki, kd, DIRECT);
double input_1 = 0, setpoint_1 = 0, output_1 = 0, output;
double kp_1 = 5, ki_1 = 0, kd_1 = 0.5;
PID angle_PID(&input_1, &output_1, &setpoint_1, kp_1, ki_1, kd_1, DIRECT);
void setup() {
Serial.begin(115200);
WiFi.mode(WIFI_STA); //ESP-NOW, Wi-Fi Station
pinMode(A_motor_forwardpin,OUTPUT);
pinMode(A_motor_backwardpin,OUTPUT);
pinMode(A_motor_PWM,OUTPUT);
pinMode(B_motor_forwardpin,OUTPUT);
pinMode(B_motor_backwardpin,OUTPUT);
pinMode(B_motor_PWM,OUTPUT);
motor_stop();
//PID
angle_PID.SetMode(AUTOMATIC);
angle_PID.SetOutputLimits(480, 800);
angle_PID.SetSampleTime(10);
//ESP-NOW
if (esp_now_init() != 0) {
Serial.println("Error initializing ESP-NOW");
return;
}
esp_now_set_self_role(ESP_NOW_ROLE_SLAVE);
esp_now_register_recv_cb(OnDataRecv);
}
void loop() {
Angle = Angle_to_send;
roundedAngle = Angle_to_send;
lastAngle = roundedAngle;
if(Angle > 0) {Angle *= -1;}
input_1 = Angle;
angle_PID.Compute();
print_debuginformation();
if(Angle_to_send > -20 && Angle_to_send < 20){
if(Angle_to_send > 0){
motor_forward();
direction = FORWARD;
if (roundedAngle < lastAngle){
motor_backward();
direction = BACKWARD;
}
}
else if(Angle_to_send < 0){
motor_backward();
direction = BACKWARD;
if (roundedAngle > lastAngle){
motor_forward();
direction = FORWARD;
}
}
else{
motor_stop();
direction = STOP;
}
}
else{
motor_stop();
direction = STOP;
}
}
void motor_forward(){
analogWrite(A_motor_PWM, 0.98*output_1);
analogWrite(B_motor_PWM, output_1);
digitalWrite(A_motor_forwardpin, HIGH);
digitalWrite(A_motor_backwardpin, LOW);
digitalWrite(B_motor_forwardpin, HIGH);
digitalWrite(B_motor_backwardpin, LOW);
run = true;
}
void motor_backward(){
analogWrite(A_motor_PWM, 0.98*output_1);
analogWrite(B_motor_PWM, output_1);
digitalWrite(A_motor_forwardpin, LOW);
digitalWrite(A_motor_backwardpin, HIGH);
digitalWrite(B_motor_forwardpin, LOW);
digitalWrite(B_motor_backwardpin, HIGH);
run = true;
}
void motor_stop(){
digitalWrite(A_motor_forwardpin, HIGH);
digitalWrite(A_motor_backwardpin, HIGH);
digitalWrite(B_motor_forwardpin, HIGH);
digitalWrite(B_motor_backwardpin, HIGH);
digitalWrite(A_motor_PWM, HIGH);
digitalWrite(B_motor_PWM, HIGH);
run = false;
}
void print_debuginformation(){
Serial.print("PIDinput");Serial.print(input_1);Serial.print(" -->");
Serial.print(" PIDoutput: ");Serial.print(output_1);Serial.print("******");
Serial.print(" Fogadott ");Serial.print(Angle_to_send);Serial.print(" Irány: ");Serial.print(direction);
Serial.print(" 1=F 2=B 3=S ");Serial.print(roundedAngle);Serial.println(lastAngle);
}
Thanks in advance!
