參考網(wǎng)上arduino下推式磁懸浮制作(源碼+電路圖)

ID:330820 · 發(fā)表于 2018-12-17 11:22

參考網(wǎng)上arduino下推磁懸浮制作

1.有曾加一些功能LCD顯示PID,霍爾輸入及其它輸出,

2.PID叄數(shù)可從arduino serial Monitor,更改輸入kp1.4 按ENTER,ki0.01按ENTER, kd7.8按ENTER.

4.SW可微調(diào)X-,X+,Y-,Y+位置,

5.L298N輸入用74LS04,

6.霍爾要安裝的高度在線圈高度一半的高度.

7.缐圈頭要接在一起,兩個缐圈尾接到L298N OUT位置.

8.線圈及霍爾都是跟網(wǎng)上一樣安裝,

安裝完成試機

1.L298N不要接12V.

2.arduino接上電源5V調(diào)教OFFSET R5,R6 到約2.15V.

3.把浮子放到霍爾上XY方向左右移動LCD Hx,Hy數(shù)字會變化,

4.先把L298N,IN1,IN2不要接,只接X,IN3,IN4,然后接12V電源,試X缐圈是否對X霍爾,

如果接缐正確會有力拉動浮子,可調(diào)OFFSET ,R5 調(diào)到浮子在兩個缐圈中心位置.

然后接上IN1,IN2,調(diào)Y線圏跟X線圈一樣方法調(diào),但調(diào)OFFSET,R6.

5.如果XY接缐正確浮子會浮動的,然后調(diào)教R5,R6使浮子在四個缐圈中心位置.

如果浮子沒有拉力拉向中心,檢查有無接錯缐.

6.調(diào)教PID參數(shù)會使浮子隱定.

7.PID參數(shù)可從arduino serial Monitor,

更改輸入

kp1.43 按ENTER

ki0.018按ENTER

kd7.5按ENTER

8.4個SW可微調(diào)浮子位置.

這個程式都是從網(wǎng)上arduino的磁浮程式更攺過來,如有錯請自行更正.

大家玩得開心.

制作出來的實物圖如下：

電路原理圖如下：

下面是arduino程序源碼

網(wǎng)上資料

www點torresballard.co點uk/levitron/

www點amobbs點com/thread-3752191-2-1.html

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
String mySt = "";
boolean stringComplete = false; // whether the string is complete
int anaPin_y = 0; //Arduino Analogic Pin 0
int anaPin_x = 1; // Arduino Analogic Pin 1
//int pin3_x = 3;
//int pin4_x = 4;
int pin_pwm5_x = 5; // Arduino Digital Pin 5
int pin_pwm6_y = 6; // Arduino Digital Pin 6
//int pin7_y = 7;
//int pin8_y = 8;
int subPin_x = 9; // Arduino Digital Pin 7
int addPin_x = 10; // Arduino Digital Pin 8
int subPin_y = 11; // Arduino Digital Pin 11
int addPin_y = 12; // Arduino Digital Pin 12
int timer1_counter; //for timer
//---------------------------------------------------------
int HallX[5], HallY[5]; //Hall effect reading arrays
int MaxX, MaxY, MinX, MinY; //Error contol variables
int AveHallX=0, AveHallY=0; //Average hall sensor readings
int setPtX=380; //430; // Hall sensor value for equilibrium state
int setPtY=380; //430;
int errorX; // midValue - current X hall sensor reading
int prevErrX=0; // Previous error used to calc derivatve
int dErrorX; // Derivative of error in X
int errorY; // midValue - current Y hall sensor reading
int prevErrY=0; // previous error for calculating dirivative
int dErrorY; // derivative of error in Y
int powerX; //PWM value driving X coil
int powerY; //PWM value driving Y coil
float Kp=1.43;//1.58; // Proportional weighting
float Ki=0.18; // Derivative weighting
float Kd=7.5; //9.79; // Integral weighting
float varKp = 0; // Tuning variables (external)
float varKi = 0;
float varKd = 0;
float SumX = 0; // Sum of XY readings
float SumY = 0;
float integralX = 0; // Set intial integrals to be zero
float integralY = 0;
//---------------------------------------------------------
void setup()
{
lcd.begin(20, 4);
// Levitator initialization Begin;
Serial.begin(9600);
Serial.println("Starting...");
// Digital Pins Work Mode Setup;
pinMode(pin3_x,OUTPUT);
pinMode(pin4_x,OUTPUT);
pinMode(pin_pwm5_x, OUTPUT);
pinMode(pin_pwm6_y, OUTPUT);
pinMode(pin7_y,OUTPUT);
pinMode(pin8_y,OUTPUT);
pinMode(addPin_x, INPUT_PULLUP);
pinMode(subPin_x, INPUT_PULLUP);
pinMode(addPin_y, INPUT_PULLUP);
pinMode(subPin_y, INPUT_PULLUP);
//--------------------------timer setup
noInterrupts(); // disable all interrupts
TCCR1A = 0;
TCCR1B = 0;
timer1_counter = 34286; // preload timer 65536-16MHz/256/2Hz (34286 for 2ms)
TCNT1 = timer1_counter; // preload timer
TCCR1B |= (1 << CS10); // no prescaler
TIMSK1 |= (1 << TOIE1); // enable timer overflow interrupt
interrupts(); // enable all interrupts
//------------------------------------------------------------------------
lcd.backlight();
lcd.setCursor(0, 0);
lcd.print("Kp:");
lcd.setCursor(0, 1);
lcd.print("Ki:");
lcd.setCursor(0, 2);
lcd.print("Kd:");
lcd.setCursor(14, 0);
lcd.print("Hx:");
lcd.setCursor(14, 1);
lcd.print("Hy:");
lcd.setCursor(14, 2);
lcd.print("Px:");
lcd.setCursor(14, 3);
lcd.print("Py:");
lcd.setCursor(8, 0);
lcd.print("X:");
lcd.setCursor(8, 1);
lcd.print("Y:");
}
//-----------------------------------------------------------------------
void loop()
{
if (stringComplete)
{
// clear the string when COM receiving is completed
stringComplete = false;
}
//receive command from serial Monitor
if (mySt.substring(0, 2) == "kp")
{
Kp = mySt.substring(2, mySt.length()).toFloat(); //get string after kp
}
if (mySt.substring(0, 2) == "ki")
{
Ki = mySt.substring(2, mySt.length()).toFloat(); //get string after ki
}
if (mySt.substring(0, 2) == "kd")
{
Kd = mySt.substring(2, mySt.length()).toFloat(); //get string after kd
}
mySt = "";
//----------------------------------------------------------------------------------
Serial.println(powerX);
// delay(1);
//------------------------------------------------------------------------------------
lcd.setCursor(3, 0);
lcd.print(" ");
lcd.setCursor(3, 0);
lcd.print(Kp);
lcd.setCursor(3, 1);
lcd.print(" ");
lcd.setCursor(3, 1);
lcd.print(Ki);
lcd.setCursor(3, 2);
lcd.print(" ");
lcd.setCursor(3, 2);
lcd.print(Kd);
lcd.setCursor(17, 0);
lcd.print(" ");
lcd.setCursor(17, 0);
lcd.print(AveHallX);
lcd.setCursor(17, 1);
lcd.print(" ");
lcd.setCursor(17, 1);
lcd.print(AveHallY);
lcd.setCursor(17, 2);
lcd.print(" ");
lcd.setCursor(17, 2);
lcd.print(powerX);
lcd.setCursor(17, 3);
lcd.print(" ");
lcd.setCursor(17, 3);
lcd.print(powerY);
lcd.setCursor(10,0);
lcd.print(" ");
lcd.setCursor(10,0);
lcd.print(setPtX);
lcd.setCursor(10,1);
lcd.print(" ");
lcd.setCursor(10,1);
lcd.print(setPtY);
//----------------------------------------------------------------------
// Increase The Value Of Levitation Point;
if (digitalRead(addPin_x) == LOW)
{
setPtX++;
delay(1);
}
if (digitalRead(subPin_x) == LOW)
{
setPtX--;
delay(1);
}
if (digitalRead(addPin_y) == LOW)
{
setPtY++;
delay(1);
}
if (digitalRead(subPin_y) == LOW)
{
setPtY--;
delay(1);
}
}
//--------------------------------------------------------------
ISR(TIMER1_OVF_vect) // interrupt service routine - tick every 0.1sec
{
// PORTB ^= _BV(5); //test Toggle LED, PB5 = Arduino pin 13
digitalWrite(13,HIGH);
TCNT1 = timer1_counter; // set timer
//--------------------------------------------------------------------------
HallX[0] = analogRead(anaPin_x);
MaxX=HallX[0];
MinX=MaxX;
HallY[0] = analogRead(anaPin_y);
MaxY=HallY[0];
MinY=MaxY;
HallX[1] = analogRead(anaPin_x);
if (HallX[1]>MaxX){MaxX=HallX[1];}
else if (HallX[1]<MinX){MinX=HallX[1];}
HallY[1] = analogRead(anaPin_y);
if (HallY[1]>MaxY){MaxY=HallY[1];}
else if (HallY[1]<MinY){MinY=HallY[1];}
HallX[2]=analogRead(anaPin_x);
if (HallX[2]>MaxX){MaxX=HallX[2];}
else if (HallX[2]<MinX){MinX=HallX[2];}
HallY[2] = analogRead(anaPin_y);
if (HallY[2]>MaxY){MaxY=HallY[2];}
else if (HallY[2]<MinY){MinY=HallY[2];}
HallX[3] = analogRead(anaPin_x);
if (HallX[3]>MaxX){MaxX=HallX[3];}
else if (HallX[3]<MinX){MinX=HallX[3];}
HallY[3] = analogRead(anaPin_y);
if (HallY[3]>MaxY){MaxY=HallY[3];}
else if (HallY[3]<MinY){MinY=HallY[3];}
HallX[4] = analogRead(anaPin_x);
if (HallX[4]>MaxX){MaxX=HallX[4];}
else if (HallX[4]<MinX){MinX=HallX[4];}
HallY[4] = analogRead(anaPin_y);
if (HallY[4]>MaxY){MaxY=HallY[4];}
else if (HallY[4]<MinY){MinY=HallY[4];}
SumX=HallX[0]+HallX[1]+HallX[2]+HallX[3]+HallX[4];
SumY=HallY[0]+HallY[1]+HallY[2]+HallY[3]+HallY[4];
AveHallX = (SumX-MinX-MaxX)/3; // Average x reading
AveHallY = (SumY-MinY-MaxY)/3; // Average y reading
errorX = setPtX - AveHallX; // X position error
errorY = setPtY - AveHallY; // Y position error
dErrorX = errorX - prevErrX; // X position differential error
dErrorY = errorY - prevErrY; // Y position differential error
integralX = integralX + errorX*0.015; // X integral
integralY = integralY + errorY*0.015; // Y integral
if (integralX > 500) integralX = 500;
if (integralX < -500) integralX = -500;
if (integralY > 500) integralY = 500;
if (integralY < -500) integralY = -500;
powerX = errorX*Kp + integralX*Ki + dErrorX*Kd; // X power
powerY = errorY*Kp + integralY*Ki + dErrorY*Kd; // Y power
prevErrX=errorX; // Assign new previous x error value
prevErrY=errorY; // Assign new previous y error value
//----------------------------------------------------------------------------
// Check the value for levitation point;
// if (powerX < 0) powerX = 0;
if (powerX < 0) powerX= 0;
if (powerX > 255) powerX = 255;
if (powerY < 0) powerY = 0;
if (powerY > 255) powerY = 255;
//----------------------------------------------------------------------------
analogWrite(pin_pwm5_x, powerX);
analogWrite(pin_pwm6_y, powerY);
prevErrX=errorX;
prevErrY=errorY; // Assign new previous y error
//--------------------------------------------------------------------------------
// PORTB ^= _BV(5); //測試LED，PB5 = Arduino腳13
digitalWrite(13,LOW);
}
void serialEvent() {
while (Serial.available()) {
// get the new byte:得到新的字節(jié)：
char inChar = (char)Serial.read();
// 將它添加到inputString：
if (inChar != '\n') {
mySt += inChar;
}
// 如果傳入的字符是換行符，請設(shè)置標誌
if (inChar == '\n') {
stringComplete = true;
}
}
}