Arduino旋转倒立摆-整体方案

lbbook

先来看看题目要求吧：

看 完题目，经过一番思索和百度之后，有了大概的方案，于是乎开始准备材料，设计机械（我负责），同时其他队员开始设计tb6560驱动、电源模块等等，当然 如果你本来就打算选择控制类题目，步进、直流电机及其驱动最好在比赛之前就有充分的准备，我们是使用现成模块的同时开始制作tb6560（比赛的时候其实 评委不看我们做的电路，只看我们能否完成题目要求）

用到的材料大部分都是以前的雕刻机项目剩下的材料，倒是省了不少时间，时间宝贵，当然怎么简单有效怎么来了，编码器连接件等东西是这几天加工出来的，abs材料

来张机械结构渲染图，比赛的时候并没有把全部结果画出来，画出个大概就开始做了，那些细节都是比赛后加上去的：

 
具体的solidworks文件、工程图全部打包在附件中

接下来是电路部分，由于这是控制类题，所以电路比较简单，24v开关电源供电，2596-5v供系统工作，一块arduino nano，一块tb6560，一个ADC键盘用于现场演示，两个mini绝对型编码器1024线

ADC键盘，一个ADC口检测10个按键，我估计检测16个没问题：

tb6560：

2596稳压：

在接下来就是程序了

总共写了14个版本程序，最后用的就是PID_1_3、PID_1_4，所以这里就给出这两个版本的注释吧：

///////////PID_1_3/////////

PID_1_3.ino :主程序，开机自动检测0°并计算出180°，之后进入循环功能演示。平衡采用PID控制，第一个PID，以摆杆角度作为作为输入，步进电机速度作为输出。第二个PID，以步进速度作为输入，平衡点作为输出（180°±8°）

 当摆杆在-110°~-180°或110°~180°范围内时，进行平衡控制，否则关闭步进输出

编码器读数、PID控制放在Timer2中断中，以保证控制周期精准，控制周期5ms
串口命令读取、状态发送放在主函数循环中

起摆思路1：反复震荡摆杆使其摆角越来越大；步进电机往一个方向运动一点距离，停止，等待摆杆达到最高点，再反向运动，停止，等待摆杆达到反向最高点，如此反复，摆杆进入平衡范围后进行平衡控制（起摆时间太长，不采用）

起摆思路2：步进电机突然给一个方向速度，再急刹车，摆杆由于惯性继续圆周运动，当进入平衡范围时，进行平衡控制（时间基本1s以内)

起摆思路3：步进电机突然给一个方向速度，再急刹车，等待摆杆达到最高点，步进电机再反向快速运动，以提高摆杆动能（杆子太重可用此方法)

Stepp.ino :步进电机驱动，使用了Timer1，setStepperSpeed(long myspeed)用于更新速度，StepperEvent()根据速度值驱动步进，如果有加减速效果会更好

Command.ino :接收命令，更新参数，P100代表参数P=100,etc


 Encoder.ino :绝对型编码器读数


Filter.ino :FIR低通滤波、中值滤波

function1~6.ino ：分别实现基本要求1~3，提高要求1~3



 ///////////PID_1_4/////////
 PID_1_4.ino : loop中命令值略有更改


Command.ino : 将读取串口数据改为读取ADC键盘值，ADC键盘优点：仅一个AD口可实现10个按键，缺点：读数略复杂，只能单点按键，适合atmega328管脚少的芯片

 其他函数同PID_1_3

程序比较长，这里就贴出PID_1_3的主函数吧，完整代码附在附件中：

ARDUINO 代码

01.#include

02.

03.

04.

05.#define setBit(val, bitn)    (val |=(1<<(bitn)))

06.#define clearBit(val, bitn)     (val&=~(1<<(bitn)))

07.#define getBit(val, bitn)    (val &(1<<(bitn)) )

08.

09.int functionSpeed=0,functionFlag=0;

10.int PIDSign=true,SpeedPIDSign=true;;

11.int function3Sign=false;

12.

13./////////////Angle Parameter/////////

14.long setPoint=0,setPointOpp=0,originPoint=0;

15.int degree=0,degreeLast=0,setPointDegree=0;

16.

17.

18./////////Arm Speed Parameter/////////

19.#define PULSE_PER_ROUND 1600

20.#define MAX_SP_SPEED (PULSE_PER_ROUND*1)

21.#define MIN_SP_SPEED (-PULSE_PER_ROUND*1)

22.long setPosition=0,position=0,positionLast=0;

23.long absPositionLast=0,absPosition=0;

24.int setSpeed=0,armSpeed=0,lastSpeed=0;

25.int np=500,ni=0,nd=0;

26.long sp_error_p=0,sp_error_i=0,sp_error_d=0;

27.int tempSpSpeed=0,tempPosition;

28.int delta=2,maxDelta=8;

29.

30.int calcAngle(int x)

31.{

32. if(setPointOpp<=x&&x
33.  return (x-setPointOpp)*-0.352;

34. else if(0<=x&&x
35.  return (setPointOpp-x)*0.352;

36. else

37.  return 180-(x-originPoint)*0.352;

38.

39.}

40.

41.

42.void setup()

43.{  

44.

45.  EncoderInit();

46.  StepperInit();

47.

48.  Serial.begin(57600);

49.  Serial.print("test beginndelay 2sn");

50.  int i,num=5,temp;

51.  for(i=0;i
52.  {

53.    temp=SSI(0);

54.    setPointOpp+=temp;//避免编码器0点在摆杆最低点或最高点，简化计算

55.    Serial.println(temp);

56.    delay(100);

57.  }

58.  Serial.println(setPointOpp);

59.  setPointOpp/=num;

60.  Serial.println(setPointOpp);

61.  if(setPointOpp<512)

62.    setPoint=setPointOpp+512;

63.  else

64.    setPoint=setPointOpp-512;

65.   originPoint=setPoint;

66.   setPointDegree=calcAngle(originPoint);

67.   Serial.print("setPointOpp=");

68.   Serial.println(setPointOpp);

69.   Serial.print("setPoint=");

70.   Serial.println(setPoint);

71.  delay(2000);

72.73.

74.  // for Timer2

75.  

76.  // interrupts every 1 ms

77.

78.  TIMSK2 &= ~(1<
79.  TCCR2A &= ~((1<
80.  TCCR2B &= ~(1<
81.  ASSR &= ~(1<
82.  TIMSK2 &= ~(1<
83.  TCCR2B |= (1<
84.  TCCR2B &= ~(1<
85.  TCNT2 = 131;

86.  TIMSK2 |= (1<
87.

88.  Timer1.attachInterrupt( StepperEvent ); // attach the service routine here

89.

90.}

91.

92.

93.int myspeed;

94.unsigned long timer;

95.

96.int angle,angleLast;

97.int error_p,error_i,error_d;

98.int velocity;

99.

100.int kp=15,kd=0,ki=20;//p=15,i=15 works!!

101.

102.unsigned int counter=0;

103.

104.

105.void loop()

106.{    

107.  checkParameter();

108.  switch(functionFlag)

109.  {

110.    case 1:

111.           function1();

112.           break;

113.    case 11:

114.           function1();

115.           break;

116.    case 2:

117.           function2();

118.           break;

119.    case 3:

120.           function3();

121.           break;

122.    case 4:

123.           function4();

124.           break;

125.    case 41:

126.           function41();

127.           break;

128.    case 6:

129.           function6();

130.           break;

131.

132.  }

133.

134.

135.

136.  Serial.print(originPoint);

137.  Serial.print(\',\');

138.  Serial.print(setPoint);

139.  Serial.print(\',\');

140.  Serial.print(angle);

141.  Serial.print(\',\');

142.  Serial.print(degree);

143.  Serial.print(\',\');

144.  Serial.print(armSpeed);//sp_error_i

145.  Serial.print(\',\');

146.  Serial.print(absPosition);

147.  Serial.print(\'n\');

148.

149.  //delay(50);

150.}

151.

152.int calcSpeed(int pos,int posLast)

153.{

154. if(abs(pos-posLast<512))

155.   return pos-posLast;

156. int temp;  

157. if(pos>posLast)//counter clock

158.  temp=1023-pos-posLast;

159. else if(pos
160.  temp=pos+(1023-posLast);

161. if(temp>1000)

162.   return temp-1023;

163. else if(temp<-1000)

164.  return 1023+temp;

165.  return temp;

166.}

167.

168.

169.int controlTimer=0,speedTimer=0;;

170.int tempPos,tempPosLast;

171.int armSpeedArr[3]={0};

172.ISR(TIMER2_OVF_vect) {  

173.  TIMSK2 |= (0<
174.  TCNT2 = 131;  // reload the timer

175.  controlTimer++;

176.  speedTimer++;

177.  // estimate velocity

178.  if (controlTimer == 5){

179.     controlTimer = 0;

180.     ////////////angle PID///////////

181.    angleLast=angle;

182.    angle=SSI(0);

183.    degreeLast=degree;

184.    degree=calcAngle(angle);

185.    //angle=FIR2(angle);

186.    velocity = (angle - angleLast);

187.    if(abs(angle-setPoint)<=4)

188.      error_p = 0;

189.    else

190.      error_p = setPoint-angle;

191.    error_d = velocity;

192.    error_i = error_i + error_p;

193.

194.   if(PIDSign==false||abs(degree)<110)

195.  {

196.   error_p=0;

197.   error_d=0;

198.   error_i=0;

199.  }

200.  myspeed = - (kp*error_p + kd*error_d + ki*error_i/10);

201.  setStepperSpeed(myspeed + tempSpSpeed+functionSpeed);

202.  }

203.

204.

205.  if(speedTimer==5)

206.  {

207.    speedTimer=0;

208.  ////////Speed PID///////////////

209.    positionLast=position;

210.    lastSpeed=armSpeed;

211.    position=SSI(1);

212.

213.    armSpeed=calcSpeed(position,positionLast);

214.    armSpeedArr[0]=armSpeedArr[1];

215.    armSpeedArr[1]=armSpeedArr[2];

216.    armSpeedArr[2]=armSpeed;

217.    armSpeed=mid(armSpeedArr[0],armSpeedArr[1],armSpeedArr[2]);

218.    if(abs(armSpeed)<=1)

219.     armSpeed=0;

220.

221.    absPositionLast=absPosition;

222.    absPosition+=armSpeed;

223.

224.    sp_error_p = setSpeed-armSpeed;

225.    sp_error_d = armSpeed-lastSpeed;

226.    sp_error_i = sp_error_i + sp_error_p;

227.    sp_error_i=constrain(sp_error_i,-1000,1000);

228.

229.

230.    tempSpSpeed = -(np*sp_error_p/10.0 + nd*sp_error_d/10.0 + ni*sp_error_i/100.0);

231.    tempSpSpeed=constrain(tempSpSpeed,-delta,delta);

232.    //if(PIDSign==false||SpeedPIDSign==false||abs(degree)>155)

233.    if(PIDSign==true&&SpeedPIDSign==true&&abs(degree)>155)

234.    {

235.      setPoint+=tempSpSpeed;

236.      if(setPoint>originPoint+maxDelta)

237.       setPoint=originPoint+maxDelta;

238.      else if(setPoint
239.       setPoint=originPoint-maxDelta;

240.    }

241.    else if(SpeedPIDSign==true)

242.    {

243.      setPoint=originPoint;

244.    }

245.    }

246.

247.    //setStepperSpeed(myspeed + tempSpSpeed);

248.

249.  TIMSK2 |= (1<
250.}

附件代码资料下载如上:

游客，如果您要查看本帖隐藏内容请回复

zty615

内容不错，果断支持～

EDC_nijifeng

有点难啊    顶起~~~~~~~~~~~~

bitding

顶一个

从今天开始！

好高端...顶下

杨1008

碉堡了，我想问一下渲染图用哪个软件做的啊

寻觅虚无

去年国赛就是做这个题目 没有搞出来了 就是做好了2问啊 伤心啊

梦红尘

顶一下。。。

EDNC_Will

很好很强大