用Arduino制作简易磁悬浮装置

#include <PID_v1.h>



#include <LCD5110_CN.h>



#define IN1 4



#define IN2 3



#define IN3 8



#define IN4 7



#define ENA 6



#define ENB 5



#define BL 2



LCD5110 myGLCD(9,10,11,13,12);



extern uint8_t SmallFont[];



extern uint8_t MediumNumbers[];



extern uint8_t BigNumbers[];



double Setpoint_X, Input_X, Output_X,X_plus;



double p_X = 1,i_X = 0,d_X = 0.01;



double Setpoint_Y, Input_Y, Output_Y,Y_plus;



double p_Y = 1,i_Y = 0,d_Y = 0.01;



int i,on_put;



unsigned long time;



PID PID_X(&Input_X, &Output_X, &Setpoint_X,p_X,i_X,d_X, DIRECT);



PID PID_Y(&Input_Y, &Output_Y, &Setpoint_Y,p_Y,i_Y,d_Y, DIRECT);



char inByte='9',nullByte,run_flag,run_dirict;



float go_step;



void turn_X(int a)



{



if(a>=0)



{



digitalWrite(IN1,1);



digitalWrite(IN2,0);



analogWrite(ENA,a);



}



else



{



a=-a;



digitalWrite(IN1,0);



digitalWrite(IN2,1);



analogWrite(ENA,a);



}



}



void turn_Y(int a)



{



if(a>=0)



{



digitalWrite(IN3,0);



digitalWrite(IN4,1);



analogWrite(ENB,a);



}



else



{



a=-a;



digitalWrite(IN3,1);



digitalWrite(IN4,0);



analogWrite(ENB,a);



}



}



void setup()



{



myGLCD.InitLCD();



myGLCD.setFont(BigNumbers);



pinMode(IN1,OUTPUT);



pinMode(IN2,OUTPUT);



pinMode(IN3,OUTPUT);



pinMode(IN4,OUTPUT);



pinMode(ENA,OUTPUT);



pinMode(ENB,OUTPUT);



pinMode(BL,OUTPUT);



digitalWrite(IN1,0);



digitalWrite(IN2,0);



digitalWrite(IN3,0);



digitalWrite(IN4,0);



analogWrite(ENA,0);



analogWrite(ENB,0);



Serial.begin(115200);



Setpoint_X = 560;//560;



Setpoint_Y = 560;//560;



PID_X.SetTunings(p_X,i_X,d_X);



PID_Y.SetTunings(p_Y,i_Y,d_Y);



PID_X.SetOutputLimits(-255,255);



PID_Y.SetOutputLimits(-255,255);



PID_X.SetSampleTime(5);



PID_Y.SetSampleTime(5);



PID_X.SetMode(AUTOMATIC);



PID_Y.SetMode(AUTOMATIC);



}



void loop()



{



while (Serial.available() > 0)



{



nullByte= char(Serial.read());



if(nullByte == 'w')



{



Setpoint_X+=10;



//inByte =Serial.read();



}



else if(nullByte == 'q')



{



Setpoint_X-=10;



}



else if(nullByte == 's')



{



Setpoint_Y+=10;



}



else if(nullByte == 'a')



{



Setpoint_Y-=10;



}



else if(nullByte == 'o')



{



run_flag=!run_flag;



run_dirict = 1;



Setpoint_X=560;



Setpoint_Y=560;



}



else if(nullByte =='p')



{



run_flag=!run_flag;



run_dirict = 0;



Setpoint_X=560;



Setpoint_Y=560;



}



else if(nullByte =='x')



{



nullByte=char(Serial.read());



if(nullByte>20)



{inByte=nullByte;



Setpoint_X = 10*(inByte-'0')+480;



}



nullByte=char(Serial.read());



if(nullByte>20)



{inByte=nullByte;



Setpoint_Y = 10*(inByte-'0')+480;



}



}



if(Setpoint_X>575)



Setpoint_X=575;



if(Setpoint_Y>575)



Setpoint_Y=575;



if(Setpoint_X<480)



Setpoint_X=480;



if(Setpoint_Y<480)



Setpoint_Y=480;



nullByte ='?';



}



Input_X = analogRead(A1);



Input_Y = analogRead(A0);



if(analogRead(A2)<450)



{



digitalWrite(BL,1);



on_put=1;



}



else



{



digitalWrite(BL,0);



on_put=0;



}



i++;



/*if(i==500)



{



Serial.print(inByte);



Serial.print(",");



Serial.println(Setpoint_Y);



}*/



if(i==1000)



{



myGLCD.printNumI(Setpoint_X, RIGHT, 0);



myGLCD.printNumI(Setpoint_Y, RIGHT, 24);



i=0;



}



if(on_put)



{



PID_X.Compute();



PID_Y.Compute();



turn_X(Output_X+X_plus);



turn_Y(Output_Y+Y_plus);



if(run_flag)



{



if(millis()-time>2)



{



time = millis();



if(run_dirict)



{



X_plus = 25*cos(go_step);



Y_plus = 25*sin(go_step);



}



else



{



X_plus = 25*sin(go_step);



Y_plus = 25*cos(go_step);



}



go_step+=0.07;



if(go_step>6.3)



go_step=0;



}



}



}



else



{



turn_X(0);



turn_Y(0);



}



// myGLCD.printNumI(Input_X, RIGHT, 0);



// myGLCD.printNumI(Input_Y, RIGHT, 24);



// myPID.SetTunings(kp,ki,kd);



}