2013电子设计竞赛-旋转倒立摆-整体方案-2014.10.26更新
本帖最后由 Malc 于 2015-6-10 13:15 编辑电赛结束了一段时间了，趁这段时间，总结、整理一下自己的方案
------------------------------------------------------------------------------------------------------------
先来看看题目要求吧：
看完题目，经过一番思索和百度之后，有了大概的方案，于是乎开始准备材料，设计机械（我负责），同时其他队员开始设计tb6560驱动、电源模块等等，当然如果你本来就打算选择控制类题目，步进、直流电机及其驱动最好在比赛之前就有充分的准备，我们是使用现成模块的同时开始制作tb6560（比赛的时候其实评委不看我们做的电路，只看我们能否完成题目要求）
用到的材料大部分都是以前的雕刻机项目剩下的材料，倒是省了不少时间，时间宝贵，当然怎么简单有效怎么来了，编码器连接件等东西是这几天加工出来的，abs材料
来张机械结构渲染图，比赛的时候并没有把全部结果画出来，画出个大概就开始做了，那些细节都是比赛后加上去的：
具体的solidworks文件、工程图全部打包在附件中
接下来是电路部分，由于这是控制类题，所以电路比较简单，24v开关电源供电，2596-5v供系统工作，一块arduino nano，一块tb6560，一个ADC键盘用于现场演示，两个mini绝对型编码器1024线
ADC键盘，一个ADC口检测10个按键，我估计检测16个没问题：
tb6560：
2596稳压：
2014.10.26更新：
系统函数图如下：
在接下来就是程序了
总共写了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的主函数吧，完整代码附在附件中：
#include <TimerOne.h>
#define setBit(val, bitn) (val |=(1<<(bitn)))
#define clearBit(val, bitn) (val&=~(1<<(bitn)))
#define getBit(val, bitn) (val &(1<<(bitn)) )
int functionSpeed=0,functionFlag=0;
int PIDSign=true,SpeedPIDSign=true;;
int function3Sign=false;
/////////////Angle Parameter/////////
long setPoint=0,setPointOpp=0,originPoint=0;
int degree=0,degreeLast=0,setPointDegree=0;
/////////Arm Speed Parameter/////////
#define PULSE_PER_ROUND 1600
#define MAX_SP_SPEED (PULSE_PER_ROUND*1)
#define MIN_SP_SPEED (-PULSE_PER_ROUND*1)
long setPosition=0,position=0,positionLast=0;
long absPositionLast=0,absPosition=0;
int setSpeed=0,armSpeed=0,lastSpeed=0;
int np=500,ni=0,nd=0;
long sp_error_p=0,sp_error_i=0,sp_error_d=0;
int tempSpSpeed=0,tempPosition;
int delta=2,maxDelta=8;
int calcAngle(int x)
{
if(setPointOpp<=x&&x<originPoint)
return (x-setPointOpp)*-0.352;
else if(0<=x&&x<setPointOpp)
return (setPointOpp-x)*0.352;
else
return 180-(x-originPoint)*0.352;
}
void setup()
{
EncoderInit();
StepperInit();
Serial.begin(57600);
Serial.print("test begin\ndelay 2s\n");
int i,num=5,temp;
for(i=0;i<num;i++)
{
temp=SSI(0);
setPointOpp+=temp;//避免编码器0点在摆杆最低点或最高点，简化计算
Serial.println(temp);
delay(100);
}
Serial.println(setPointOpp);
setPointOpp/=num;
Serial.println(setPointOpp);
if(setPointOpp<512)
setPoint=setPointOpp+512;
else
setPoint=setPointOpp-512;
originPoint=setPoint;
setPointDegree=calcAngle(originPoint);
Serial.print("setPointOpp=");
Serial.println(setPointOpp);
Serial.print("setPoint=");
Serial.println(setPoint);
delay(2000);
/*--------------------*/
// for Timer2
/*--------------------*/
// interrupts every 1 ms
TIMSK2 &= ~(1<<TOIE2);
TCCR2A &= ~((1<<WGM21) | (1<<WGM20));
TCCR2B &= ~(1<<WGM22);
ASSR &= ~(1<<AS2);
TIMSK2 &= ~(1<<OCIE2A);
TCCR2B |= (1<<CS22)| (1<<CS20);
TCCR2B &= ~(1<<CS21);
TCNT2 = 131;
TIMSK2 |= (1<<TOIE2);
Timer1.attachInterrupt( StepperEvent ); // attach the service routine here
}
int myspeed;
unsigned long timer;
int angle,angleLast;
int error_p,error_i,error_d;
int velocity;
int kp=15,kd=0,ki=20;//p=15,i=15 works!!
unsigned int counter=0;
void loop()
{
checkParameter();
switch(functionFlag)
{
case 1:
function1();
break;
case 11:
function1();
break;
case 2:
function2();
break;
case 3:
function3();
break;
case 4:
function4();
break;
case 41:
function41();
break;
case 6:
function6();
break;
}
Serial.print(originPoint);
Serial.print(',');
Serial.print(setPoint);
Serial.print(',');
Serial.print(angle);
Serial.print(',');
Serial.print(degree);
Serial.print(',');
Serial.print(armSpeed);//sp_error_i
Serial.print(',');
Serial.print(absPosition);
Serial.print('\n');
//delay(50);
}
int calcSpeed(int pos,int posLast)
{
if(abs(pos-posLast<512))
return pos-posLast;
int temp;
if(pos>posLast)//counter clock
temp=1023-pos-posLast;
else if(pos<posLast)//clock
temp=pos+(1023-posLast);
if(temp>1000)
return temp-1023;
else if(temp<-1000)
return 1023+temp;
return temp;
}
int controlTimer=0,speedTimer=0;;
int tempPos,tempPosLast;
int armSpeedArr={0};
ISR(TIMER2_OVF_vect) {
TIMSK2 |= (0<<TOIE2);
TCNT2 = 131;// reload the timer
controlTimer++;
speedTimer++;
// estimate velocity
if (controlTimer == 5){
controlTimer = 0;
////////////angle PID///////////
angleLast=angle;
angle=SSI(0);
degreeLast=degree;
degree=calcAngle(angle);
//angle=FIR2(angle);
velocity = (angle - angleLast);
if(abs(angle-setPoint)<=4)
error_p = 0;
else
error_p = setPoint-angle;
error_d = velocity;
error_i = error_i + error_p;
if(PIDSign==false||abs(degree)<110)
{
error_p=0;
error_d=0;
error_i=0;
}
myspeed = - (kp*error_p + kd*error_d + ki*error_i/10);
setStepperSpeed(myspeed + tempSpSpeed+functionSpeed);
}
if(speedTimer==5)
{
speedTimer=0;
////////Speed PID///////////////
positionLast=position;
lastSpeed=armSpeed;
position=SSI(1);
armSpeed=calcSpeed(position,positionLast);
armSpeedArr=armSpeedArr;
armSpeedArr=armSpeedArr;
armSpeedArr=armSpeed;
armSpeed=mid(armSpeedArr,armSpeedArr,armSpeedArr);
if(abs(armSpeed)<=1)
armSpeed=0;
absPositionLast=absPosition;
absPosition+=armSpeed;
sp_error_p = setSpeed-armSpeed;
sp_error_d = armSpeed-lastSpeed;
sp_error_i = sp_error_i + sp_error_p;
sp_error_i=constrain(sp_error_i,-1000,1000);
tempSpSpeed = -(np*sp_error_p/10.0 + nd*sp_error_d/10.0 + ni*sp_error_i/100.0);
tempSpSpeed=constrain(tempSpSpeed,-delta,delta);
//if(PIDSign==false||SpeedPIDSign==false||abs(degree)>155)
if(PIDSign==true&&SpeedPIDSign==true&&abs(degree)>155)
{
setPoint+=tempSpSpeed;
if(setPoint>originPoint+maxDelta)
setPoint=originPoint+maxDelta;
else if(setPoint<originPoint-maxDelta)
setPoint=originPoint-maxDelta;
}
else if(SpeedPIDSign==true)
{
setPoint=originPoint;
}
}
//setStepperSpeed(myspeed + tempSpSpeed);
TIMSK2 |= (1<<TOIE2);
}
视频、图片在这里：
http://v.youku.com/v_show/id_XNjA2MzY3NTQw.html
附件：
115：
http://115.com/lb/5lbawy4ygq2#
倒立摆1.rar
115网盘礼包码：5lbawy4ygq2
TB6560 资料：
gyq 发表于 2014-8-4 10:40 static/image/common/back.gif
你说的用502固定在步进电机的轴上么，还是底座上，要是在底座上的话，编码器怎么采集转轴的速度数据呀，这 ...
用502来粘合步进电机的轴与编码器的轴，用胶带固定编码器，防止自转 {:soso_e102:}终于看到源码了。。。仔细学习 这效果图让我想起了那个小蜘蛛 弘毅 发表于 2013-10-7 21:04 static/image/common/back.gif
终于看到源码了。。。仔细学习
时间紧迫，代码写的比较乱，算法有待改进。。
ps：那两个nano偷偷拆下来自己用了{:soso_e113:} 幻生幻灭 发表于 2013-10-7 21:15 static/image/common/back.gif
这效果图让我想起了那个小蜘蛛
不玩蜘蛛了，雕刻机更好玩~ 关注你很久了，好喜欢你的文章，终于知道你的倒立摆是咋做的的啦~学习，谢谢~~~ Uno的话程序能用么 我们做的也是这个，不过跟您的比起来简直就是个渣啊，这会真的学习了，膜拜啊 这架势应该有国一吧。我们的学生一组国一一组国二。没有您这位大神做得好。 大神。。。。肯定是国一，一个地区三等奖给跪了 做D题的孩纸快要被吓死了 编码器放在哪儿了？硬件及机械部分是不是没讲完整，我个人理解是放到倒立摆转轴那块了。 wisology 发表于 2013-10-11 21:52 static/image/common/back.gif
编码器放在哪儿了？硬件及机械部分是不是没讲完整，我个人理解是放到倒立摆转轴那块了。
两个编码器，一个装在摆臂末端，检测摆杆的转角，另一个装在步进底下，步进没有闭环的话摆杆会一直在水平方向移动停不下来，跟平衡车一样 这个厉害。。。。。想都没想过。。 太赞了！！！