Purpose:
Balance ball and beam control system.
This project is constructed using the HC-SR04 Ultrasonic Sensor to detect the position of the sphere and using the PID principle to control the Servo motor to move the object to the specified position.
這個專案在建構利用HC-SR04 Ultrasonic Sensor偵測球體位置並利用PID原理, 控制Servo motor移動物體到指定位置.
The control objective is to control the torque applied at the pivot of the beam, such that the ball can roll on the beam and track a desired trajectory. The torque causes thus a change of the beam angle and a movement in the position of the ball.
控制目標是控制施加在橫樑樞軸處的扭矩, 使得球可以在橫樑上滾動並追蹤期望的軌跡. 因此, 扭矩導致承載面角度的變化和球位置的移動.
Fundamental:
網路關於PID控制有很多介紹, 可自行參考.
My Blog artical:
從ESP32 with HC-SR04 Ultrasonic Sensor 這篇文章開始接觸Ultrasonic Sensor
進而利用Ultrasonic Sensor做一些應用專題如:
其中參閱很多網路先進的文章再融入自己的想法, 實際將其做出, 收益頗多!
BOM(Bill of Material):
圖一: Woods
圖二: Servo Motor MG995
圖三:軸承Bearing
圖四: Wood with Bearing
圖五: HC-SR04 Ultrasonic Sensor
圖六: Ruler
圖七: ESP32
圖八: glue 熱熔膠
圖九: PVC Tube
Circuit:
YouTube Demo:
ESP32 Code:
#include <ESP32Servo.h>
#include<PID_v1.h>
//#include <Servo.h>
//--------- Flag structure --------------------------------------
typedef struct _vFlag
{
uint8_t BTFlag = 0;
uint8_t DC_Flag = 0;
uint8_t CANFlag = 0;
uint8_t I2C_Flag = 0;
uint8_t BMP180Flag = 0;
uint8_t HCSR04Flag = 1;
uint8_t LEDFlag = 0;
uint8_t initial_Flag = 0;
uint8_t FunctionFlag = 3;
uint8_t SendFlag = 0;
uint8_t BMPCnt = 0;
} vFlag;
vFlag *flag_Ptr;
vFlag flag;
//------LED------------------
#define LED_BUILTIN 2
//------------------------------------------------
Servo myservo; // create servo object to control a servo
// Recommended PWM GPIO pins on the ESP32 include 2,4,12-19,21-23,25-27,32-33
int servoPin = 13;
//-----hcsr04 sensor------------------
#define TRIGPIN_PIN 12
#define ECHO_PIN 14
long duration;
int distance;
int set=25,neg=5,pos=45,base=67;
double StartAngle = 90; //Angle of servo when beam is parallel to the ground
//-------------PID ------------------------
float Kp = 0.8; //Initial Proportional Gain
float Ki = 0.005; //Initial Integral Gain
float Kd = 0.3; //Intitial Derivative Gain
double Setpoint, Input, Output, ServoOutput;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVERSE);
//--------------------------------------------
unsigned long currentMillis = 0;
unsigned long previousMillis = 0;
int interval = 1000;
//----------uart--------------
#define LINE_BUFFER_LENGTH 64
//--------- uart structure --------------------------------------
typedef struct _vUart
{
char c;
int lineIndex = 0;
int line1Index = 0;
int BTlineIndex = 0;
bool lineIsComment;
bool lineSemiColon;
char line[128];
char BTline[20];
String inputString;
String BTinputString;
String S1inputString;
int V[16];
char ctemp[30];
char I2C_Data[80];
int DC_Spped = 50;
float Voltage[16];
int Buffer[128];
int StartCnt = 0;
int ReadCnt = 0;
int sensorValue = 0;
} vUart;
vUart *Uart_Ptr;
vUart Uart;
//-------------------------------------------------
void setup()
{
Serial.begin(9600);
Serial.println(F("init"));
pinMode(LED_BUILTIN, OUTPUT);
pinMode(TRIGPIN_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
pinMode(servoPin, OUTPUT);
//myservo.setPeriodHertz(50); // standard 50 hz servo
//myservo.attach(servoPin, 500, 2400); // attaches the servo on pin 13 to the servo object
myservo.attach(servoPin);
myservo.write(StartAngle);
Input = readPosition();
myPID.SetMode(AUTOMATIC); //Set PID object myPID to AUTOMATIC
myPID.SetOutputLimits(-25,25); //Set Output limits to -80 and 80 degrees.
}
//-----------------------------------------
void loop()
{
Serial.print(F("Main at core:"));
Serial.println(xPortGetCoreID());
while(1)
{
if(flag.HCSR04Flag==1)
{
Setpoint = 25;
Input = readPosition();
myPID.Compute();
//ServoOutput=base+Output;
//myservo.write(ServoOutput);
Serial.print(Input); Serial.print(" "); Serial.println(Output);
if (Output>0)
{
ServoOutput=92+Output;
myservo.write(ServoOutput);
}
else if(Output<0)
{
ServoOutput=60+Output;
myservo.write(ServoOutput);
}
}
if(flag.LEDFlag == 1)
{
digitalWrite(LED_BUILTIN, HIGH);
vTaskDelay(300);
digitalWrite(LED_BUILTIN, LOW);
vTaskDelay(300);
}
while (Serial.available() > 0)
{
Uart.c = Serial.read();
if ((Uart.c == '\n') || (Uart.c == '\r'))
{ // End of line reached
if (Uart.lineIndex > 0)
{ // Line is complete. Then execute!
Uart.line[Uart.lineIndex] = '\0'; // Terminate string
//Serial.println( F("Debug") );
//Serial.println( Uart.inputString );
processCommand(Uart.line); // do something with the command
Uart.lineIndex = 0;
Uart.inputString = "";
}
else
{
// Empty or comment line. Skip block.
}
Uart.lineIsComment = false;
Uart.lineSemiColon = false;
Serial.println(F("ok>"));
}
else
{
//Serial.println( c );
if ((Uart.lineIsComment) || (Uart.lineSemiColon))
{
if (Uart.c == ')')
Uart.lineIsComment = false; // End of comment. Resume line.
}
else
{
if (Uart.c == '/')
{ // Block delete not supported. Ignore character.
}
else if (Uart.c == '~')
{ // Enable comments flag and ignore all characters until ')' or EOL.
Uart.lineIsComment = true;
}
else if (Uart.c == ';')
{
Uart.lineSemiColon = true;
}
else if (Uart.lineIndex >= LINE_BUFFER_LENGTH - 1)
{
Serial.println("ERROR - lineBuffer overflow");
Uart.lineIsComment = false;
Uart.lineSemiColon = false;
}
else if (Uart.c >= 'a' && Uart.c <= 'z')
{ // Upcase lowercase
Uart.line[Uart.lineIndex] = Uart.c - 'a' + 'A';
Uart.lineIndex = Uart.lineIndex + 1;
Uart.inputString += (char)(Uart.c - 'a' + 'A');
}
else
{
Uart.line[Uart.lineIndex] = Uart.c;
Uart.lineIndex = Uart.lineIndex + 1;
Uart.inputString += Uart.c;
}
}
}
} //while (Serial.available() > 0)
}
}
//---------------------------------------
float readPosition()
{
if(flag.HCSR04Flag==1)
{
pinMode(TRIGPIN_PIN, OUTPUT);
digitalWrite(TRIGPIN_PIN, LOW);
delayMicroseconds(5);
digitalWrite(TRIGPIN_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGPIN_PIN, LOW);
pinMode(ECHO_PIN, INPUT);
duration= pulseIn(ECHO_PIN, HIGH);
distance= duration/29/2;
return (distance);
}
}
//----------------------------------------
void processCommand(char *data)
{
int len, xlen, ylen, zlen, alen;
int tempDIO;
String stemp;
len = Uart.inputString.length();
//---------------------------------------
if (strstr(data, "VER") != NULL)
{
Serial.println(F("ESP32_20231031"));
}
if (strstr(data, "HCSR04_ON") != NULL)
{
flag.HCSR04Flag = 1;
flag.LEDFlag = 0;
Serial.println(F("HCSR04_ON"));
}
if (strstr(data, "HCSR04_OFF") != NULL)
{
flag.HCSR04Flag = 0;
flag.LEDFlag = 1;
Serial.println(F("HCSR04_OFF"));
}
if (strstr(data, "SERVO_90")!= NULL)
{
Serial.println(F("SERVO_90"));
myservo.write(90);
}
if (strstr(data, "SERVO_50")!= NULL)
{
Serial.println(F("SERVO_50"));
myservo.write(50);
}
if (strstr(data, "SERVO_70")!= NULL)
{
Serial.println(F("SERVO_70"));
myservo.write(70);
}
if (strstr(data, "SERVO_100")!= NULL)
{
Serial.println(F("SERVO_100"));
myservo.write(100);
}
if (strstr(data, "SERVO_120")!= NULL)
{
Serial.println(F("SERVO_120"));
myservo.write(120);
}
if (strstr(data, "SERVO_140")!= NULL)
{
Serial.println(F("SERVO_140"));
myservo.write(140);
}
}
//-----------------------------------------
