下推式磁懸浮

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. }