上拉式磁懸浮

float kp=0.25 ; //比例

float kd=700 ; //微分

int c = 20 ; //常數

int set = 512 ; //設定點 (介於AnalogRead讀數範圍 0~1023之間)

int now ;

int err,old,output ;

unsigned long t1,t2;

float p,d;

void setup() {

//Serial.begin(9600);

}

void loop() {

now = analogRead(A0);

if(now<20){

analogWrite(3,0);

}else{

t2=micros();

err = now - set ;

p = kp * err ;

d = kd *1000* (now - old)/(t2-t1) ;

output=constrain(p + d ,-127,127);

analogWrite(3,output + 127);

old = now ;

t1=t2;

Serial.print("now=");

Serial.print(now);

Serial.print(" , ");

Serial.print("err=");

Serial.print(err);

Serial.print(" , ");

Serial.print("p=");

Serial.print(p);

Serial.print(" , ");

Serial.print("d=");

Serial.print(d);

Serial.print(" , ");

Serial.println(output+128);

}

}

//Hall effect sensor

int x=21;

int i,j,p1[21],p2[21],timer=0,temp,now,old,error,output,setpoint = 382,total;

unsigned long t,oldt;

float p,d,kp = -0.6,kd = -25;

void setup() {

//Serial.begin(9600);

}

void loop() {

timer =timer % x;

old = now;

p1[timer] = analogRead(A0);

now = p1[timer];

for(i=0;i<x;i++)

{

p2[i] = p1[i];

}

timer++;

for(i=0;i<(x-1);i++)

{

for(j=0;j<10;j++)

{

if(p2[j+1] >= p2[j])

{

temp = p2[j];

p2[j] = p2[j+1];

p2[j+1] = temp;

}

}

}

t = micros(); //紀錄時間(單位:微秒)

error = p2[10] - setpoint ; //設誤差值(error)=現在感應的光度(now)-預設光度(setpoint)

p =kp*error; //設函數(p)=誤差值*對應比例(kp)

d =kd*(now - old)*1000/ (t - oldt); //注三

output = constrain(p+100+d,0,255); //注四

if(now>=762)

output = 0;

analogWrite(3,output); //注五 //更新過去時間(oldt)

/* Serial.print(p2[0]);

Serial.print(" , ");

Serial.print(p2[1]);

Serial.print(" , ");

Serial.print(p2[2]);

Serial.print(" , ");

Serial.print(p2[3]);

Serial.print(" , ");

Serial.print(p2[4]);

Serial.print(" , ");

Serial.print(p2[5]);

Serial.print(" , ");

Serial.print(p2[6]);

Serial.print(" , ");

Serial.print(p2[7]);

Serial.print(" , ");

Serial.print(p2[8]);

Serial.print(" , ");

Serial.print(p2[9]);

Serial.print(" , ");

Serial.print(p2[10]);

Serial.print(" , ");*/

Serial.print( p1[timer]);

Serial.print(" , ");

Serial.println(error);

oldt = t;

total = 0;

}