下推式磁懸浮

float kpx=1 ; //比例

float kpy=1 ; //比例

float kdx=2 ; //微分

float kdy=2 ; //微分

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

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

int nowx, nowy ;

int px,dx,errx,oldx,outputx ;

int py,dy,erry,oldy,outputy ;

int erx,ery;

unsigned long t2,t1,t4,t3;

void setup() {

Serial.begin(115200);

pinMode(5,OUTPUT);

pinMode(6,OUTPUT);

pinMode(9,OUTPUT);

pinMode(10,OUTPUT);

}

void loop(){

nowx = analogRead(A1);

Serial.print("nowx=");

Serial.print(nowx);

Serial.print(" , ");

errx = nowx - setx ;

erx=abs(errx);

px = kpx * erx ;

Serial.print("px=");

Serial.print(px);

Serial.print(" , ");

t2=micros();

dx = kdx * (nowx - oldx)*1000/(t2-t1) ;

Serial.print("dx=");

Serial.print(dx);

Serial.print(" , ");

oldx = nowx ;

t1=t2;

outputx=int(constrain(px + dx ,0,255));

if(errx>0){

analogWrite(5,outputx);

analogWrite(6,0);

Serial.print("outputx=");

Serial.print(outputx);

Serial.print(" , ");

}

if(errx==0){

analogWrite(5,0);

analogWrite(6,0);

Serial.print("outputx=");

Serial.print(outputx);

Serial.print(" , ");

}

if(errx<0){

analogWrite(6,outputx);

analogWrite(5,0);

Serial.print("outputx=");

Serial.print(-1*outputx);

Serial.print(" , ");

}

delay(1);

nowy = analogRead(A0);

Serial.print("nowy=");

Serial.print(nowy);

Serial.print(" , ");

erry = nowy - sety ;

ery=abs(erry);

py = kpy * ery ;

Serial.print("py=");

Serial.print(py);

Serial.print(" , ");

t4=micros();

dy = kdy * (nowy - oldy)*1000/(t4-t3) ;

Serial.print("dy=");

Serial.print(dy);

Serial.print(" , ");

oldy = nowy ;

t3=t4;

outputy=int(constrain(py + dy ,0,255));

if(erry>0){

analogWrite(9,outputy);

analogWrite(10,0);

Serial.print("outputy=");

Serial.println(outputy);

}

if(erry==0){

analogWrite(9,0);

analogWrite(10,0);

Serial.print("outputy=");

Serial.println(outputy);

}

if(erry<0){

analogWrite(10,outputy);

analogWrite(9,0);

Serial.print("outputy=");

Serial.println(-1*outputy);

}

delay(1);

}

view plaincopy to clipboardprint?

  1. //PINs setting

  2. int adjust1Pin = 1; //用来调节A的电位器

  3. int adjust2Pin = 2; //用来调节B的电位器

  4. int read1Pin = 4; //用来连接输入A传感器

  5. int read2Pin = 3; //用来连接输入B传感器

  6. int i1Pin = 36; //连接电机驱动板的I1接口

  7. int i2Pin = 37; //连接电机驱动板的I2接口

  8. int i3Pin = 39; //连接电机驱动板的I3接口

  9. int i4Pin = 38; //连接电机驱动板的I4接口

  10. int power1Pin = 5; //连接电机驱动板的EA接口

  11. int power2Pin = 6; //连接电机驱动板的EB接口

  12. int rotatePin = 3; //用来控制磁场旋转的PMW接口

  14. boolean debug = false;

  15. boolean writeLog = false;

  16. double setKd1 = 0.55;

  17. double setKd2 = 0.55;

  18. double setKp = 22;

  19. int offset = 70;

  20. int delayMs = 1;

  21. int tick = 0;

  22. int myLog[3500];

  24. //PID structure

  25. typedef struct {

  26. double target;

  27. double aver;

  28. double Kp;

  29. double Kd;

  30. int preError;

  31. int power;

  32. boolean flag;

  33. double v;

  34. } PID;

  36. PID Pid1, Pid2;

  38. void setup()

  39. {

  40. pinMode(i1Pin, OUTPUT); //I1和I2都是数字信号

  41. pinMode(i2Pin, OUTPUT); //通过设置I1和I2来控制电流方向

  42. pinMode(i3Pin, OUTPUT); //I1和I2都是数字信号

  43. pinMode(i4Pin, OUTPUT); //通过设置I1和I2来控制电流方向

  44. pinMode(power1Pin, OUTPUT); //按占空比方式输出的模拟信号

  45. pinMode(power2Pin, OUTPUT); //按占空比方式输出的模拟信号

  46. pinMode(rotatePin, OUTPUT); //按占空比方式输出的模拟信号

  48. //analogWrite(rotatePin, 128);

  50. Serial.begin(9600); //设置波特率

  51. TCCR0B = 0x01; // Timer 0: PWM 5 & 6 @ 16 kHz

  52. TCCR1B = 0x01; // Timer 1: PWM 9 & 10 @ 32 kHz

  53. TCCR2B = 0x01; // Timer 2: PWM 3 & 11 @ 32 kHz

  54. Pid1.Kp = setKp;

  55. Pid1.preError = 0;

  56. Pid1.Kd = setKd1;

  57. Pid1.power = 0;

  58. Pid1.flag = true;

  59. Pid1.target = 300;

  60. Pid1.aver = 0;

  61. Pid1.v = 0;

  62. Pid2.Kp = setKp;

  63. Pid2.preError = 0;

  64. Pid2.Kd = setKd2;

  65. Pid2.power = 0;

  66. Pid2.flag = true;

  67. Pid2.target = 300;

  68. Pid2.aver = 0;

  69. Pid2.v = 0;

  70. tick = 0;

  71. }

  73. int tick2 = 0;

  74. //boolean rotateFlag = true;

  75. void loop()

  76. {

  77. //digitalWrite(rotatePin, rotateFlag);

  78. //rotateFlag = ! rotateFlag;

  79. //delay(16000);

  80. //return;

  82. if(debug) tick = 0;

  83. tick++;

  84. if(tick==500)

  85. {

  86. tick2++;

  87. if(tick2<50) {tick = 0;return;}

  88. tick2 = 0;

  89. if(writeLog)

  90. {

  91. for(int i=0;i<500;i++)

  92. {

  93. Serial.print(myLog[i*7 + 0]);

  94. Serial.print(" ");

  95. Serial.print(myLog[i*7 + 1]);

  96. Serial.print(" ");

  97. Serial.print(myLog[i*7 + 2]);

  98. Serial.print(" ");

  99. Serial.print(myLog[i*7 + 3]);

  100. Serial.print(" ");

  101. Serial.print(myLog[i*7 + 4]);

  102. Serial.print(" ");

  103. Serial.print(myLog[i*7 + 5]);

  104. Serial.print(" ");

  105. Serial.print(myLog[i*7 + 6]);

  106. Serial.println(" ");

  107. }

  108. Serial.println(Pid1.target);

  109. Serial.println(Pid1.preError);

  110. Serial.println(Pid2.target);

  111. Serial.println(Pid2.preError);

  112. }

  113. return;

  114. }

  115. else if(tick>500)

  116. {

  117. tick = 0;

  118. //delay(990000);

  119. return;

  120. };

  122. //=======第一组电位器和传感器========

  123. int readValue1 = 0;

  124. for(int i = 0; i < 4; i++) readValue1 += analogRead(read1Pin);

  125. readValue1 >>= 2;

  126. //readValue1 += (Pid1.flag ? 1 : -1) * Pid1.power / 17;

  127. int adjustValue1 = analogRead(adjust1Pin); //410 analogRead(adjust1Pin);

  128. Pid1.aver = Pid1.aver * 0.9995 + readValue1 * 0.0005;

  129. Pid1.target = Pid1.target + (Pid1.target - Pid1.aver) / 100.0;

  130. Pid1.target = max(0, max(adjustValue1 - offset, Pid1.target));

  131. Pid1.target = min(755, min(adjustValue1 + offset, Pid1.target));

  133. //=======第二组电位器和传感器=======

  134. int readValue2 = 0;

  135. for(int i = 0; i < 4; i++) readValue2 += analogRead(read2Pin);

  136. readValue2 >>= 2;

  137. //readValue2 += (Pid2.flag ? 1 : -1) * Pid2.power / 6;

  138. int adjustValue2 = analogRead(adjust2Pin); //240 analogRead(adjust2Pin);

  139. Pid2.aver = Pid2.aver * 0.9995 + readValue2 * 0.0005;

  140. Pid2.target = Pid2.target + (Pid2.target - Pid2.aver) / 1000.0;

  141. Pid2.target = max(0, max(adjustValue2 - offset, Pid2.target));

  142. Pid2.target = min(755, min(adjustValue2 + offset, Pid2.target));

  144. if(debug)

  145. {

  146. Serial.println(adjustValue1);

  147. Serial.println(adjustValue2);

  148. Serial.println(readValue1);

  149. Serial.println(readValue2);

  150. Pid1.flag = adjustValue1 > 512;

  151. Pid1.power = abs(adjustValue1 - 512) / 2;

  152. if(Pid1.power > 255) Pid1.power = 255;

  153. digitalWrite(i1Pin, Pid1.flag);

  154. digitalWrite(i2Pin, !Pid1.flag);

  155. analogWrite(power1Pin, Pid1.power);

  156. Pid2.flag = adjustValue2 > 512;

  157. Pid2.power = abs(adjustValue2 - 512) / 2;

  158. if(Pid2.power > 255) Pid2.power = 255;

  159. digitalWrite(i3Pin, Pid2.flag);

  160. digitalWrite(i4Pin, !Pid2.flag);

  161. analogWrite(power2Pin, Pid2.power);

  162. delay(32000);

  163. return;

  164. }

  166. //Calculate power values

  167. double v, error;

  168. error = readValue1 - Pid1.target;

  169. v = error - Pid1.preError;

  170. Pid1.v = (Pid1.v * 6 + v) / 7;

  171. Pid1.power = (int)error * Pid1.Kd + Pid1.v * Pid1.Kp;

  172. Pid1.flag = Pid1.power > 0;

  173. Pid1.power = abs(Pid1.power);

  174. if(Pid1.power>255) Pid1.power = 255;

  175. Pid1.preError = error;

  177. error = readValue2 - Pid2.target;

  178. v = error - Pid2.preError;

  179. Pid2.v = (Pid2.v * 6 + v) / 7;

  180. Pid2.power = (int)error * Pid2.Kd + Pid2.v * Pid2.Kp;

  181. Pid2.flag = Pid2.power < 0;

  182. Pid2.power = abs(Pid2.power);

  183. if(Pid2.power>255) Pid2.power = 255;

  184. Pid2.preError = error;

  186. //Write PMW to control the floa

  187. digitalWrite(i1Pin, Pid1.flag);

  188. digitalWrite(i2Pin, !Pid1.flag);

  189. analogWrite(power1Pin, Pid1.power);

  191. digitalWrite(i3Pin, Pid2.flag);

  192. digitalWrite(i4Pin, !Pid2.flag);

  193. analogWrite(power2Pin, Pid2.power);

  195. myLog[tick * 7 + 0] = tick;

  196. myLog[tick * 7 + 1] = (int)Pid1.target;

  197. myLog[tick * 7 + 2] = readValue1;

  198. myLog[tick * 7 + 3] = Pid1.power;

  199. myLog[tick * 7 + 4] = (int)Pid2.target;

  200. myLog[tick * 7 + 5] = readValue2;

  201. myLog[tick * 7 + 6] = Pid2.power;

  203. /*

  204. for(int i=0;i<8;i++)

  205. {

  206. digitalWrite(rotatePins[i] , 0);

  207. digitalWrite(rotatePins[(i + 1) % 8] ,1);

  208. delay(1);

  209. }

  210. digitalWrite(rotatePins[0] , 0);

  211. */

  213. delay(delayMs);

  214. }