///////// วงจรขับ TB6612 drive pin ////
#define PWM 9 //ขาส่งสัญญาณ PWM ไปยังวงจรขับ
#define IN2 10 //ขาควบคุมทิศทางการหมุน ไปยังวงจรขับ
#define IN1 11 //ขาควบคุมทิศทางการหมุน ไปยังวงจรขับ
///////// ขาสัญญาณที่อ่านเอ็นโค้ดเดอร์ /////////
#define ENCODER_A 2// Motor encoder pin เฟส A ที่ขา 2
#define ENCODER_B 4// Motor encoder pin เฟส B ที่ขา 4
///////// ขาสัญญาณจากโพเทนชิโอมิเตอร์ที่ขา Analog /////////
#define Analogin 1
#include <PinChangeInt.h> // ไลเบอรี่สำหรับใช้งานอินเตอร์รัปที่ขาดิจิตอลอินพุต ในที่นี้ใช้สำหรับอ่านพัลส์เอ็นโค้ดเดอร์
#include <MsTimer2.h> // ไลเบอรี่สำหรับใช้งานไทเมอร์
int Setpoint = 512; // ค่าตรงกลางของตัวโพเทนชิโอมิเตอร์ ที่ทำให้แกนตั้งฉากกับพื้น
float Position = 1000; // ตัวแปร Position ค่าจะวิ่งอยู่ในช่วง +/- 500
float Sensor; // Angle displacement sensor parameters
float Motor_PWM; // Motor PWM parameters อยู่ในชว่ง -255 ถึง +255
float Balance_Pwm; // Pendulum uplight loop PWM parameters ตัวแปรเอาต์พุตสัญญาณ PWM ของการควบคุมแกนเพนดูลัม
float Position_Pwm; // Position loop PWM parameters
int Target_Position = 1000; // Target value
static float Position_Int;
int EnablePendulum = 0; // Start/Stop control pendulum
float Balance_KP = 300; // Balance Gain
float Balance_KI = 0; // Balance Gain
float Balance_KD = 200; // Balance Gain
float Position_KP = 100; // Position Gain
float Position_KI = 0; // Position Gain
float Position_KD = 100; // Position Gain
/**************************************************************************
Function: PD control
Entry parameters: Angle
Return Value: Pendulum uplight Tilt Control PWM
**************************************************************************/
float Balance_Control(float sensor){
float Error; // ตัวแปรเก็บค่า Error
static float Last_Error; // ตัวแปรที่เกี่ยวข้องกับ PD control
static float D_Error; // ตัวแปรที่เกี่ยวข้องกับ PD control
int PD_Output; // ตัวแปรเอาต์พุตของ PD control
Error = sensor - Setpoint; // หาค่า Error
D_Error = Error - Last_Error; // หาค่าการเปลี่ยนแปลงชั่วขณะ(เทอม D)
PD_Output = Balance_KP/10 * Error + D_Error * Balance_KD/1000; // PWM PD control for motors that calculate tilt control
Last_Error = Error; // ทำให้ Errorใหม่ เป็น Error เก่า
return PD_Output; // ส่งค่าเอาต์พุต
}
/**************************************************************************
Function: Position PD control
Entry parameters: Encoder
Return Value: Position control PWM
**************************************************************************/
float Position_Control(int Encoder){
static float Position_PWM; //Define related variables
static float Last_Position; //Define related variables
static float Position_Bias; //Define related variables
static float Position_Differential; //Define related variables
static float Position_Least; //Define related variables
Position_Least = Target_Position - Encoder; // Get deviation value
Position_Bias *= 0.8; // first-order low-pass filter
Position_Bias += Position_Least*0.2; // first order difference
Position_Int = Position_Int + Position_Least;
if(Position_Int > 500)Position_Int = 500;
if(Position_Int < -500)Position_Int = -500;
Position_Differential = Position_Bias - Last_Position; // Keep the last deviation
Last_Position = Position_Bias; // เปลี่ยนค่าล่าสุดเป็นค่าเก่า สำหรับการคำนวณรอบหน้า
Position_PWM = (Position_Bias*Position_KP/1000) + (Position_Int*Position_KI/100) + (Position_Differential*Position_KD/10); // Position control
return Position_PWM; //Return control
}
/**************************************************************************
Function: ส่งค่า PWM ออกที่ขา 9 และกำหนดทิศทางการหมุนด้วยขา 10 และ 11
Entry parameters: PWM
Return Value: None
**************************************************************************/
void Set_PWM(int motor){
if (motor< 0) digitalWrite(IN1, LOW), digitalWrite(IN2, HIGH); // ค่าติดลบ
else digitalWrite(IN1, HIGH), digitalWrite(IN2, LOW); // ค่าบวก
analogWrite(PWM, abs(motor)); // ส่งสัญญาณ PWM ค่ามาก มอเตอร์หมุนเร็ว ค่าน้อย มอเตอร์หมุนช้า
}
/**************************************************************************
Function: หาค่าเฉลี่ยของค่าที่อ่านได้จากเซนเซอร์ตรวจจับมุมเอียง
Entry parameters: None
Return Value: None
**************************************************************************/
u16 Get_Adc_Average(u8 ch,u8 times){
unsigned int temp_val=0;
unsigned char t;
for(t=0;t<times;t++){
temp_val+=analogRead(ch);//อ่านค่าตำแหน่งแกนจาก Adc แล้วบวกในตัวเอง
}
return temp_val/times;//ครบแล้วหารด้วยค่า time
}
/**************************************************************************
Function: ทำฟังชัน ทุกๆ 5ms control จากไทเมอร์2
Entry parameters: None
Return Value: None
**************************************************************************/
void control(){
static unsigned char Position_Count; // Variable for position control frequency division
sei(); // เปิดให้อินเตอร์รัปต์ทำงาน
Sensor= 1023 - Get_Adc_Average(1,5); // หาค่าเฉลี่ยของแรงดันที่อ่านได้จาก Adc ต่อเข้าที่ขา A1 หาค่าเฉลี่ย 5 ครั้ง
Balance_Pwm = Balance_Control(Sensor); // เรียกฟังก์ชัน PD control เพื่อเลี้ยงแกนให้ตรง
if(++Position_Count>4)
{
Position_Pwm = Position_Control(Position); // Position PD control
Position_Count = 0;
}
Motor_PWM = Balance_Pwm - Position_Pwm; // Calculate motor final PWM
if(Motor_PWM>255) Motor_PWM = +255; //ลิมิตค่า PWM มากสุด, ถ้าค่าเกิน 255 มอเตอร์จะหยุด
if(Motor_PWM<-255)Motor_PWM = -255; //ลิมิตค่า PWM น้อยสุด
if(Sensor<(Setpoint-200)||Sensor>(Setpoint+200)) // ถ้ามุมการเอียงมากไปจะส่งให้มอเตอร์หยุดทำงาน
{
Motor_PWM = 0;
digitalWrite(IN1, LOW); //TB6612 level control
digitalWrite(IN2, LOW); //TB6612 level control
}
if(EnablePendulum == 1)
{
Set_PWM(Motor_PWM); //Output motor control
}
else
{
Set_PWM(0); //Stop motor control
}
}
/**************************************************************************
Function: คอนฟิกพอร์ตและกำหนดค่าเริ่มต้นต่างๆ
Entry parameters: None
Return Value: None
**************************************************************************/
voidsetup() {
int fff = 1;
TCCR1B =(TCCR1B & 0xF8) | fff;
pinMode(IN1, OUTPUT); //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต
pinMode(IN2, OUTPUT); //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต
pinMode(PWM, OUTPUT); //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต
digitalWrite(IN1, 0); //ส่งลอจิก 0
digitalWrite(IN2, 0); //ส่งลอจิก 0
digitalWrite(PWM, 0); //ส่งลอจิก 0
pinMode(ENCODER_A, INPUT); // กำหนดขาอ่านสัญญาณเอ็นโค้ดเดอร์ให้เป็น อินพุต
pinMode(ENCODER_B, INPUT); // กำหนดขาอ่านสัญญาณเอ็นโค้ดเดอร์ให้เป็น อินพุต
Serial.begin(115200); // กำหนดบอตเรตการสื่อสาร serial เพื่อส่งข้อมูลไปยังคอมพิวเตอร์
delay(200); // หน่วงเวลาเล็กน้อยเพื่อให้แน่ใจว่าการตั้งค่าต่างๆ เสร็จเรียบร้อยแล้ว
MsTimer2::set(5, control); // Use Timer 2 to set 5ms timer interrupt
MsTimer2::start(); // เปิดการทำงานไทเมอร์
attachInterrupt(0, READ_ENCODER_A, CHANGE); //เปิดใช้งานอินเตอร์รัปต์สำหรับอ่านค่าเอ็นโค้ดเดอร์ ทุกครั้งที่มีการเปลี่ยนแปลง จาก 0 เป็น 1 หรือ จาก 1 เป็น 0
}
/**************************************************************************
Function function: ส่งค่าไปที่ serial port เพื่อพล็อตกราฟ
Entry parameters: None
Return Value: None
**************************************************************************/
void loop(){ // แสดงผลกราฟ
Serial.print("\n");
//1
Serial.print(Motor_PWM);
Serial.print("\t");
//2
Serial.print(Sensor);
Serial.print("\t");
//3
Serial.print(Setpoint);
Serial.print("\t");
//4
Serial.print(Position);
Serial.print("\t");
//5
Serial.print(Target_Position);
Serial.print("\t");
//6
Serial.print(Balance_KP);
Serial.print("\t");
//7
Serial.print(Position_KP);
Serial.print("\t");
//8
Serial.print(Position_KI);
Serial.print("\t");
//9
Serial.print(Position_KD);
Serial.print("\t");
//10
Serial.print(Balance_KI);
Serial.print("\t");
//11
Serial.print(Balance_KD);
Serial.print("\t");
//delay(5);
switch (Serial.read()) {
//************ Balance ****************
case 'c':
Setpoint = Setpoint +1;
break;
case 'd':
Setpoint = Setpoint -1;
break;
case 'a':
Balance_KP = Balance_KP+1;
break;
case 'b':
Balance_KP = Balance_KP-1;
break;
case 'q':
Balance_KI = Balance_KI+1;
break;
case 'r':
Balance_KI = Balance_KI-1;
break;
case 's':
Balance_KD = Balance_KD+1;
break;
case 't':
Balance_KD = Balance_KD-1;
break;
//************ Position **************
case 'e':
Target_Position = Target_Position +1;
break;
case 'f':
Target_Position = Target_Position -1;
break;
case 'g':
Position_KP = Position_KP +1;
break;
case 'h':
Position_KP = Position_KP -1;
break;
case 'i':
Position_KI = Position_KI +1;
break;
case 'j':
Position_KI = Position_KI -1;
break;
case 'k':
Position_KD = Position_KD +1;
break;
case 'l':
Position_KD = Position_KD -1;
break;
//*************** Additional *****************
case 'x':
EnablePendulum = 1;
break;
case 'y':
EnablePendulum = 0;
break;
case 'u':
Position = 1000;
break;
}
}
/**************************************************************************
Function: อ่านสัญญาณเอ็นโค้ดเดอร์ตตามการอินเตอร์รัฟต์ เป็นกรณีที่สัญญาณเฟส A นำเฟส B
Entry parameters: None
Return Value: None
**************************************************************************/
voidREAD_ENCODER_A(){
if(digitalRead(ENCODER_A) == HIGH)
{ //เกิดอินเตอร์รัปต์ที่ขอบขาลง
if (digitalRead(ENCODER_B) == LOW) Position--; //เพิ่มค่า ตัวแปร Position
else Position++;
}
else
{ //เกิดอินเตอร์รัปต์ที่ขอบขาขึ้น
if (digitalRead(ENCODER_B) == LOW) Position++; //ลดค่า ตัวแปร Position
else Position--;
}
}
/**************************************************************************
Function: อ่านสัญญาณเอ็นโค้ดเดอร์ตตามการอินเตอร์รัฟต์ เป็นกรณีที่สัญญาณเฟส B นำเฟส A
Entry parameters: None
Return Value: None
**************************************************************************/
voidREAD_ENCODER_B(){
if(digitalRead(ENCODER_B) == LOW)
{//เกิดอินเตอร์รัปต์ที่ขอบขาลง
if (digitalRead(ENCODER_A) == LOW) Position--;//เพิ่มค่า ตัวแปร Position
else Position++;
}
else
{ //เกิดอินเตอร์รัปต์ที่ขอบขาขึ้น
if (digitalRead(ENCODER_A) == LOW) Position++; //ลดค่า ตัวแปร Position
else Position--;
}
}
รีวิว
ยังไม่มีบทวิจารณ์