?? main.c
字號(hào):
/**** sample alpha by electrograph
Using Pendulum Function board designed 07.03.26
pzhao@mail.ustc.edu.cn 07.05.23 ****/
#include "Regs240x.h"
#include "Initsystem.h"
#include "Sample.h"
#include "Filter.h"
#include "Control.h"
#include "PWM.h"
#include "Send.h"
#include "ServoControl.h"
#include "JaoZHControl.h"
#include "sinsource.h"
enum Mode{Calibration,PCControl,InvPdl,Servo,StopControl};
enum Mode ControlMode;
int SampleTime=0,ControlTime=0,SendTime=0;
int PCdata;
int alpha,theta;
int init_alpha,init_theta;
//inverted pendulum coeff
float FeedbackCoeff[4]={-0.3887f, -92.0636f, -7.7248f, -10.5986f};
/**ServoPara:input,K,H,A,StartOmega,EndOmega,deltaW,T1,T2 **/
float ServoPara[9]={1.0, 1.0, 1.0, 2.0, 0.1, 5.0, 0.1,0.083,0.0041};
float DeadZone=5.9f;
float MaxCtrlVol=12.0f;
int UpdatecoffFlag=0,UpdateVolFlag=0,UpdateServoFlag=0;
int UpdatecoffBuf[8] = {0};
int UpdateVolBuf[4]={0};
int UpdateServoBuf[18]={0};
int UpdatecoffNum = 0;
int UpdateVolNum = 0;
int UpdateServoNum = 0;
int ServoExpIndex = 0;
unsigned char msg_buf[5];
/*******servo ***********/
#define Ts 0.01f //采樣間隔
#define ATurn_Rad 0.00322f //AD采樣值轉(zhuǎn)變成弧度值
int count=0;
float Phase=0.0f; //相位
float f=0.0f; //頻率
/************************/
void main()
{
int dir,duty,sduty;
int i,Filter_buf[40]={0};
const int CaliNum = 40;
const int SampNum = 12;
float CtrlValue,sfctrl;
int sendnum=0; /* 通信序號(hào) */
int TempData;
/****** servo *******/
float StandValue; //輸入的跟蹤信號(hào)
float rin;
float K,H,A,hz_start,hz_end,hz_delta;
float T1,T2;
float Xl = 0,Xn = 0,Yl = 0;
/*******************/
Initsystem();
ControlMode=Calibration;
*IFR = 0x0FF;
*IMR = 0x03;
init_alpha=490;
init_theta=539;
asm(" CLRC INTM ");
while(1)
{
if(Calibration==ControlMode)
{
*CMPR1 = 1000; /* do not output */
for(i=CaliNum-1;i>=0;i--)
Filter_buf[i]=SampleAlpha();
init_alpha=Filter(Filter_buf,CaliNum);
for(i=CaliNum-1;i>=0;i--)
Filter_buf[i]=Sampletheta();
init_theta=Filter(Filter_buf,CaliNum);
msg_buf[0] = ((init_alpha>>4)&0x30)|((init_theta>>8)&0x03)|0x0C0;
msg_buf[1] = init_alpha&0xff;
msg_buf[2] = init_theta&0xff;
msg_buf[4] = 0;
}
if(StopControl==ControlMode)
{
*CMPR1 = 1000; /* do not output */
}
if(SampleTime)
{
for(i=SampNum-1;i>=0;i--)
Filter_buf[i]=SampleAlpha();
alpha=Filter(Filter_buf,SampNum);
for(i=SampNum-1;i>=0;i--)
Filter_buf[i]=Sampletheta();
theta=Filter(Filter_buf,SampNum);
msg_buf[0] = ((alpha>>4)&0x30)|((theta>>8)&0x03);
msg_buf[1] = alpha&0xff;
msg_buf[2] = theta&0xff;
SampleTime=0;
}
if(1 <= ControlTime)
{
switch(ControlMode)
{
case PCControl:
dir=PCdata&0x80;
duty=PCdata&0x7F;
sduty = duty;
break;
case InvPdl:
CtrlValue = Control();
break;
case Servo:
/**/
//servo control mode
if(4 == ServoExpIndex)
{
StandValue = sinsource(A,hz_end,hz_delta,Ts); //正弦輸入
}
else
{
StandValue = rin/ATurn_Rad; //階躍輸入
}
//計(jì)算隨動(dòng)控制的控制量
Xn=ServoControl(K,H,StandValue,alpha,init_alpha);
CtrlValue = Xn;
if(5 == ServoExpIndex)
{
Yl=JaoZHControl(T1,T2,Ts,Yl,Xl,Xn); //T1=0.049,T2=0.083
Xl = Xn;
CtrlValue = Yl;
}
/**/
break;
}/*end_switch*/
//sfctrl = CtrlValue;
if(PCControl != ControlMode)
{
if( CtrlValue>0 )
{
CtrlValue += DeadZone;
}
else
{
CtrlValue -= DeadZone;
}
if( CtrlValue>MaxCtrlVol)
{
CtrlValue = MaxCtrlVol;
//sfctrl = MaxCtrlVol;
}
if( CtrlValue<-MaxCtrlVol)
{
CtrlValue = -MaxCtrlVol;
//sfctrl = -MaxCtrlVol;
}
if( CtrlValue<0 ){
dir = 0x80;
duty = -(int)(100.0*(CtrlValue/MaxCtrlVol));
//sduty = -(int)(100.0*(sfctrl/MaxCtrlVol));
}
else{
dir = 0;
duty = (int)(100.0*(CtrlValue/MaxCtrlVol));
}
}
PWM(dir,duty);
msg_buf[4] = dir | duty;
ControlTime=0;
}/*end_if(ControlTime)*/
if(1 <= SendTime){
sendnum++;
if(sendnum>100)
{
sendnum = 1;
}
msg_buf[3] = sendnum;
Send();
SendTime=0;
}/*end_if(2<=SendTime)*/
if(2 == UpdatecoffFlag && 8==UpdatecoffNum) //更新底層的反饋系數(shù)
{
for(i=0;i<4;i++)
{
TempData = (UpdatecoffBuf[2*i]&0x7F);
TempData = (TempData<<8) + UpdatecoffBuf[2*i+1];
if(UpdatecoffBuf[2*i]>>7) //判斷符號(hào)位
{
FeedbackCoeff[i] = (-1.0f)*(TempData/100.0f);
}
else
{
FeedbackCoeff[i] = (1.0f)*(TempData/100.0f);
}
}
UpdatecoffFlag = 0; //over to update
}
if(2 == UpdateVolFlag && 4==UpdateVolNum) //更新電壓設(shè)置，電壓不會(huì)有負(fù)值
{
TempData = (UpdateVolBuf[0]&0x7F);
MaxCtrlVol=((TempData<<8) + UpdateVolBuf[1])/100.0f;
TempData = (UpdateVolBuf[2]&0x7F);
DeadZone=((TempData<<8) + UpdateVolBuf[3])/100.0f;
UpdateVolFlag = 0; //over to update
}
if((2 == UpdateServoFlag)&&(18 == UpdateServoNum))
{
for(i=0;i<9;i++)
{
TempData = (UpdateServoBuf[2*i]&0x7F);
TempData = (TempData<<8) + UpdateServoBuf[2*i+1];
if(UpdateServoBuf[2*i]>>7) //判斷符號(hào)位
{
ServoPara[i] = (-1.0f)*(TempData/100.0f);
}
else
{
ServoPara[i] = (1.0f)*(TempData/100.0f);
}
}
//ServoPara:input(rin),K,H,A,StartOmega,EndOmega,deltaW,T1,T2
/****/
rin = *ServoPara;
K = *(ServoPara+1);
H = *(ServoPara+2);
A = *(ServoPara+3);
hz_start = *(ServoPara+4);
hz_end = *(ServoPara+5);
hz_delta = *(ServoPara+6);;
T1= (*(ServoPara+7))/10.0f;
T2= (*(ServoPara+8))/100.0f;
f=hz_start;
/****/
UpdateServoFlag = 0; //over to update
}
}/*end_while */
}
void interrupt c_int1() /* SCI interrupt*/
{
int msg;
asm(" SETC INTM ");
while( !(* SCIRXST & 0x40) ); /*Receive message */
msg = * SCIRXBUF;
//更新數(shù)據(jù)區(qū)間，收到的msg<=248,為更新的數(shù)據(jù)，而非命令字
if((1 == UpdatecoffFlag)&&(msg<=248)) //receive K[4]
{
UpdatecoffBuf[UpdatecoffNum] = msg;
UpdatecoffNum++;
}
else if((1 == UpdateVolFlag)&&(msg<=248)) //receive update voltage
{
UpdateVolBuf[UpdateVolNum] = msg;
UpdateVolNum++;
}
else if((1== UpdateServoFlag)&&(msg<=248)) //receive update servo para
{
UpdateServoBuf[UpdateServoNum] = msg;
UpdateServoNum++;
}
else
{
switch(msg){
case 229:
ControlMode=Calibration;
break;
case 230:
ControlMode=StopControl;
break;
case 234:
ControlMode=PCControl;
break;
case 239:
ControlMode=InvPdl;
break;
case 240:
ControlMode=Servo;
break;
case 241:
ServoExpIndex = 2;
break;
case 242:
ServoExpIndex = 3;
break;
case 243:
ServoExpIndex = 4;
break;
case 244:
ServoExpIndex = 5;
break;
case 249:
UpdateServoFlag = 1; //Start to receive update servo coeff
UpdateServoNum = 0;
break;
case 250:
UpdateServoFlag = 2; //End to receive update servo para
break;
case 251:
UpdatecoffFlag = 1; //Start to receive update K[4]
UpdatecoffNum = 0;
break;
case 252:
UpdatecoffFlag = 2; //end to receive update K[4]
break;
case 253:
UpdateVolFlag = 1; //Start to receive Update DeadZone,MaxCtrlVol;
UpdateVolNum = 0;
break;
case 254:
UpdateVolFlag = 2; //end to receive Update DeadZone,MaxCtrlVol;
default:
if(((msg&0x7F)<=100)&&(PCControl==ControlMode)) //receive ctrl value
{
PCdata=msg;
}
} /*end_switch */
}
asm(" CLRC INTM ");
}
void interrupt c_int2() /*Timer interrupt,10ms(100Hz)*/
{
asm(" SETC INTM");
//forbid automaic sample/control/send during calibration
if((Calibration!=ControlMode)&&(StopControl!=ControlMode))
{
SampleTime=1; /* Time to sample （10ms）*/
ControlTime+=1; /* Time to Control (20ms)*/
}
SendTime+=1; /* Time to Send (10ms)*/
* EVBIFRA=0x00FF; /*clear Timer3 period intrrupt flag */
asm(" CLRC INTM ");
}
?? 快捷鍵說(shuō)明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -