📄 dsp281x_ev.c
字号:
//###########################################################################
//
// FILE: DSP281x_Ev.c
// TITLE: DSP281x Event Manager Initialization & Support Functions.
// WRITER: ZHANG X.L.
//
//###########################################################################
#include "DSP281x_Device.h" // DSP281x Headerfile Include File
#include "DSP281x_Examples.h" // DSP281x Examples Include File
struct ROBOTSTATE RobAimSta;
struct ROBOTSTATE RobSta={ 0,0,PIE/2,30,30,0};
struct MOTOR Motor[9]; //Motor[0]-Motor[8],Motor[0] is not used;
Uint16 MDir = 0;
Uint32 LEcoderCount = 0 , REcoderCount = 0;
//---------------------------------------------------------------------------
// InitEv:
// This function initializes to a known state.
//---------------------------------------------------------------------------
void InitEv(void)
{
//InitEVA;
EALLOW;
/* GpioMuxRegs.GPAMUX.bit.PWM1_GPIOA0 = 1;
GpioMuxRegs.GPAMUX.bit.PWM2_GPIOA1 = 0;
GpioMuxRegs.GPAMUX.bit.PWM3_GPIOA2 = 1;
GpioMuxRegs.GPAMUX.bit.PWM4_GPIOA3 = 0;
GpioMuxRegs.GPAMUX.bit.PWM5_GPIOA4 = 1;
GpioMuxRegs.GPAMUX.bit.PWM6_GPIOA5 = 0;
GpioMuxRegs.GPAMUX.bit.T1PWM_GPIOA6 = 1;
GpioMuxRegs.GPAMUX.bit.T2PWM_GPIOA7 = 0;
GpioMuxRegs.GPAMUX.bit.CAP1Q1_GPIOA8 = 1;
GpioMuxRegs.GPAMUX.bit.CAP2Q2_GPIOA9 = 1;
GpioMuxRegs.GPAMUX.bit.CAP3QI1_GPIOA10 = 1;
GpioMuxRegs.GPAMUX.bit.TDIRA_GPIOA11 = 0;
GpioMuxRegs.GPAMUX.bit.TCLKINA_GPIOA12 = 0;
GpioMuxRegs.GPAMUX.bit.C1TRIP_GPIOA13 = 0;
GpioMuxRegs.GPAMUX.bit.C2TRIP_GPIOA14 = 0;
GpioMuxRegs.GPAMUX.bit.C3TRIP_GPIOA15 = 0; */
GpioMuxRegs.GPAMUX.all |= 0x0755;
GpioMuxRegs.GPDMUX.bit.T1CTRIP_PDPA_GPIOD0 = 0;
GpioMuxRegs.GPDMUX.bit.T2CTRIP_SOCA_GPIOD1 = 0;
EDIS;
EvaRegs.EXTCONA.all = 0x0001; // INDCOE = 1,enable independent compare output enable mode;
EvaRegs.GPTCONA.all = 0x0005; // Reserved,0;
// T2STAT = 0,R;
// T1STAT = 0,R;
// T2CTRIP = 0,??
// T1CTRIP = 0,??
// T2TOADC = 00,no timer 2 event starts ADC;
// T1TOADC = 00,no timer 1 event starts ADC;
// TCMPOE = 0,reserved as EXTCONA(0) = 1;
// T2CMPOE = 0,timer 1 compare outputs T2PWM_T2CMP is in
// high-impendance state;
// T1CMPOE = 0,timer 1 compare outputs T1PWM_T1CMP is in
// high-impendance state;
// if set,driven by individual timer compare logic;
// T2PIN = 01,polarity of timer 2 compare output is active low;
// T1PIN = 01,
EvaRegs.T1CON.all = 0x1742; // 00 0 10 111 0 1 00 00 1 0;
// Free:Soft = 00,仿真挂起则立即停止;
// Reserved,0;
// TMODE1:0 = 10,连续增模式;
// TSP2:0 = 111, HSPCLK/128;
// T2SWT1/T4SWT3 = 0,使用T2、T4自身的使能位;
// TENABLE = 1,使能定时器操作（定时器保持，TSP2:0=000);
// TCLKS1:0 = 00,时钟源：HSPCLK;
// TCLD1:0 = 00,当计数器值为0时，定时器比较寄存器重装；
// TECMPR = 1,使能定时器比较操作；
// SELT1PR/SELT3PR = 0,T2、T4使用自身的周期寄存器；
EvaRegs.T2CON.all = 0x1F70; // 00 0 11 111 0 1 11 00 0 0;
// Relate to QEP;
// TMODE1:0 = 11,Timer2 count mode is directional-up/-down count mode;
// TENABLE = 1,enable timer 2 operation;
// TCLKS1:0 = 11,clock source is QEP circuit；
// TECMPR = 0,diable timer compare operation;
// The value of the else bits are the same with T1CON;
// EvaRegs.COMCONA.all = 0xA4E7;
EvaRegs.COMCONA.all = 0xA400; // CENABLE = 1,使能比较操作；
// CLD1:0 = 01,T1下溢或周期匹配时CMPRx重载；
// SVENABLE = 0,禁止空间矢量PWM模式；
// ACTRLD1:0 = 01,T1下溢或周期匹配时ACTRA重载；
// FCMPOE = 0,reserved as EXTCONA(0) = 1;；
// #PDPINTA,R;
// FCMP3OE = 0;禁止全比较3输出；
// FCMP2OE = 0;
// FCMP1OE = 0;
// Reserved;
// Reserved;
// C3TRP = 0;禁止全比较3进程；
// C2TRP = 0;
// C1TRP = 0;
EvaRegs.ACTRA.all = 0x0555; // Action on compare output pins 6:1 is active low;
EvaRegs.DBTCONA.all = 0x0000; // EDBT3:1 = 000;Disable dead-band Timer 3/2/1;
EvaRegs.CAPCONA.all =0x10FC; // 1 00 0 0 0 0 0 11 11 11 0;
// Relate to QEP;
// CAPRES = 1,no action;
// CAP12EN = 00,disable captures 1 and 2;
// CAP3EN = 0,disable capture 3;
// Reserved,0;
// CAP3TSEL = 0,capture unit 3 selects GP timer 2;
// CAP12TSEL = 0,capture units 1 and 2 selects GP timer 2;
// CAP3TOADC = 0,no action;
// CAP1EDGE = 11,capture unit 1 detects both edges;
// CAP2EDGE = 11,
// CAP3EDGE = 11,
// Reserved,0;
EvaRegs.T2CNT = 0x8000; // Relate to QEP;
// The time compare logic and full compare units outputs are all disabled.It must be eanbled
//individually,if needed;
// The QEP circuit gets ready for work;
//////////////////////////////////////////////////////////////////////////////////////////////
//InitEVB,the Cfguration is the same with the EVA;
EALLOW;
/* GpioMuxRegs.GPBMUX.bit.PWM7_GPIOB0 = 1;
GpioMuxRegs.GPBMUX.bit.PWM8_GPIOB1 = 0;
GpioMuxRegs.GPBMUX.bit.PWM9_GPIOB2 = 1;
GpioMuxRegs.GPBMUX.bit.PWM10_GPIOB3 = 0;
GpioMuxRegs.GPBMUX.bit.PWM11_GPIOB4 = 1;
GpioMuxRegs.GPBMUX.bit.PWM12_GPIOB5 = 0;
GpioMuxRegs.GPBMUX.bit.T3PWM_GPIOB6 = 1;
GpioMuxRegs.GPBMUX.bit.T4PWM_GPIOB7 = 0;
GpioMuxRegs.GPBMUX.bit.CAP4Q1_GPIOB8 = 1;
GpioMuxRegs.GPBMUX.bit.CAP5Q2_GPIOB9 = 1;
GpioMuxRegs.GPBMUX.bit.CAP6QI2_GPIOB10 = 1;
GpioMuxRegs.GPBMUX.bit.TDIRB_GPIOB11 = 0;
GpioMuxRegs.GPBMUX.bit.TCLKINB_GPIOB12 = 0;
GpioMuxRegs.GPBMUX.bit.C4TRIP_GPIOB13 = 0;
GpioMuxRegs.GPBMUX.bit.C5TRIP_GPIOB14 = 0;
GpioMuxRegs.GPBMUX.bit.C6TRIP_GPIOB15 = 0; */
GpioMuxRegs.GPBMUX.all |= 0x0755;
GpioMuxRegs.GPDMUX.bit.T3CTRIP_PDPB_GPIOD5 = 0;
GpioMuxRegs.GPDMUX.bit.T4CTRIP_SOCB_GPIOD6 = 0;
EDIS;
EvbRegs.EXTCONB.all = 0x0001;
EvbRegs.GPTCONB.all = 0x0005;
EvbRegs.T3CON.all = 0x1742;
EvbRegs.T4CON.all = 0x1F70;
EvbRegs.COMCONB.all = 0xA400;
EvbRegs.ACTRB.all = 0x0555;
EvbRegs.DBTCONB.all = 0x0000;
EvbRegs.CAPCONB.all =0x10FC;
EvbRegs.T4CNT = 0x8000;
}
//---------------------------------------------------------------------------------------
//Configure the GP Times and full compare units to generate PWM;
//PWmfreq's unit is Hz;
//DutyCycle's unit is 1%.For example , DutyCycle = 30 ,then the PWM's duty cycle is 30%;
//---------------------------------------------------------------------------------------
void CfgPWM(Uchar n,Uint16 PWMfreq, Uchar DutyCycle) //
{
Uint16 TnPR , TnCMPR_CMPRn;
TnPR = 480000 / PWMfreq - 1; // 600 000 Hz is the timer's clock frequence
// = 30 000 000 * SysCtrlRegs.PLLCR.bit.DIV / 2 /( SysCtrlRegs.HISPCP.all * 2) / ( 2 ^ EvaRegs.T1CON.bit.TPS );
TnCMPR_CMPRn = (Uint16)(((float32)(TnPR + 1)) * DutyCycle / 100);
switch(n)
{
/* case 21: //T1PWM;
EvaRegs.T1PR = TnPR;
EvaRegs.T1CMPR = TnCMPR_CMPRn;
EvaRegs.GPTCONA.bit.T1CMPOE = 1; //Driven by individual timer compare logic;
break;
case 22: //T2PWM;
//EvaRegs.T2PR = TnPR;
//EvaRegs.T2CMPR = TnCMPR_CMPRn;
//EvaRegs.GPTCONA.bit.T2CMPOE = 1; //The same above;
break;
case 23: //T3PWM;
EvbRegs.T3PR = TnPR;
EvbRegs.T3CMPR = TnCMPR_CMPRn;
EvbRegs.GPTCONB.bit.T3CMPOE = 1; //The same above;
break;
case 24: //T4PWM;
//EvbRegs.T4PR = TnPR;
//EvbRegs.T4CMPR = TnCMPR_CMPRn;
//EvbRegs.GPTCONB.bit.T4CMPOE = 1; //The same above;
break;
case 1: //PWM1;
EvaRegs.T1PR = TnPR;
EvaRegs.CMPR1 = TnCMPR_CMPRn;
EvaRegs.COMCONA.bit.FCMP1OE = 1; //使能全比较1输出；
break;
case 2: //PWM2;
break;
case 3: //PWM3;
EvaRegs.T1PR = TnPR;
EvaRegs.CMPR2 = TnCMPR_CMPRn;
EvaRegs.COMCONA.bit.FCMP2OE = 1; //使能全比较2输出；
break;
case 4: //PWM4;
break;
case 5: //PWM5;
EvaRegs.T1PR = TnPR;
EvaRegs.CMPR3 = TnCMPR_CMPRn;
EvaRegs.COMCONA.bit.FCMP3OE = 1; //使能全比较3输出；
break;
case 6: //PWM6;
break;
case 7: //PWM7;
EvbRegs.T3PR = TnPR;
EvbRegs.CMPR4 = TnCMPR_CMPRn;
EvbRegs.COMCONB.bit.FCMP4OE = 1; //使能全比较器4输出；
break;
case 8: //PWM8;
break;
case 9: //PWM9;
EvbRegs.T3PR = TnPR;
EvbRegs.CMPR5 = TnCMPR_CMPRn;
EvbRegs.COMCONB.bit.FCMP5OE = 1; //使能全比较器5输出；
break;
case 10: //PWM10;
break;
case 11: //PWM11;
EvbRegs.T3PR = TnPR;
EvbRegs.CMPR6 = TnCMPR_CMPRn;
EvbRegs.COMCONB.bit.FCMP6OE = 1; //使能全冉掀?输出；
break;
case 12: //PWM12;
break;
default: break;
*/
//This configuration is consistent with the FPGA board;
case 7: //T1PWM;
EvaRegs.T1PR = TnPR;
EvaRegs.T1CMPR = TnCMPR_CMPRn;
EvaRegs.GPTCONA.bit.T1CMPOE = 1; //Driven by individual timer compare logic;
break;
case 8: //T3PWM;
EvbRegs.T3PR = TnPR;
EvbRegs.T3CMPR = TnCMPR_CMPRn;
EvbRegs.GPTCONB.bit.T3CMPOE = 1; //The same above;
break;
case 1: //PWM1;
EvaRegs.T1PR = TnPR;
EvaRegs.CMPR1 = TnCMPR_CMPRn;
EvaRegs.COMCONA.bit.FCMP1OE = 1; //使能全比较1输出；
break;
case 2: //PWM3;
EvaRegs.T1PR = TnPR;
EvaRegs.CMPR2 = TnCMPR_CMPRn;
EvaRegs.COMCONA.bit.FCMP2OE = 1; //使能全比较2输出；
break;
case 3: //PWM5;
EvaRegs.T1PR = TnPR;
EvaRegs.CMPR3 = TnCMPR_CMPRn;
EvaRegs.COMCONA.bit.FCMP3OE = 1; //使能全比较3输出；
break;
case 4: //PWM7;
EvbRegs.T3PR = TnPR;
EvbRegs.CMPR4 = TnCMPR_CMPRn;
EvbRegs.COMCONB.bit.FCMP4OE = 1; //使能全比较器4输出；
break;
case 5: //PWM9;
EvbRegs.T3PR = TnPR;
EvbRegs.CMPR5 = TnCMPR_CMPRn;
EvbRegs.COMCONB.bit.FCMP5OE = 1; //使能全比较器5输出；
break;
case 6: //PWM11;
EvbRegs.T3PR = TnPR;
EvbRegs.CMPR6 = TnCMPR_CMPRn;
EvbRegs.COMCONB.bit.FCMP6OE = 1; //使能全比较器6输出；
break;
default: break;
}
}
//---------------------------------------------------------------------------
//Caculate the robot's real-time state;
//---------------------------------------------------------------------------
void CaRobSta(void)
{
Uint16 T2CNTtemp = 0x0000 , T4CNTtemp = 0x0000;
static Uint16 CaRobSta_count = 0x01 ;
float32 L1,L2,sita,fx,fy,ftemp1,ftemp2,fsin,fcos;
// KickDog();
// Measure the robot's wheels' speed;
// RobSta.LSpeed = ((float32)(EvaRegs.T2CNT - 0x8000)) * (float32)LEcoderParam / DetaTime;
// RobSta.Rspeed = ((float32)(EvbRegs.T4CNT - 0x8000)) * (float32)REcoderParam / DetaTime;
// Caculate the robot's position and direction;
if ((abs(EvaRegs.T2CNT-0x8000) > CaculateThreshold) || (abs(EvbRegs.T4CNT-0x8000) > CaculateThreshold))
{
// L1 = ((float32)(EvaRegs.T2CNT - 0x8000)) * (float32)LEcoderParam;
// L2 = ((float32)(EvbRegs.T4CNT - 0x8000)) * (float32)REcoderParam;
T2CNTtemp = EvaRegs.T2CNT ;
T4CNTtemp = EvbRegs.T4CNT ;
EvaRegs.T2CNT = 0x8000;
EvbRegs.T4CNT = 0x8000;
L1 = ((float32)(T2CNTtemp - 0x8000)) * (float32)LEcoderParam;
L2 = ((float32)(T4CNTtemp - 0x8000)) * (float32)REcoderParam;
RobSta.LSpeed = ((float32)(T2CNTtemp - 0x8000)) * (float32)LEcoderParam / (DetaTime * CaRobSta_count) ;
RobSta.Rspeed = ((float32)(T4CNTtemp - 0x8000)) * (float32)REcoderParam / (DetaTime * CaRobSta_count) ;
#if TEST_RUN
LEcoderCount = LEcoderCount + abs(T2CNTtemp - 0x8000);
REcoderCount = REcoderCount + abs(T4CNTtemp - 0x8000);
#endif
CaRobSta_count = 0x01;
}
else
{
CaRobSta_count++;
return;
}
/* L1=((float32)((DSP_LONG)EvaRegs.T2CNT-OldEvaRegs.T2CNT)*(float32)LEcoderParam);
L2=((float32)((DSP_LONG)EvbRegs.T4CNT-OldEvbRegs.T4CNT)*(float32)REcoderParam);
OldEvaRegs.T2CNT=EvaRegs.T2CNT;
OldEvbRegs.T4CNT=EvbRegs.T4CNT;*/
sita = (L2 - L1) / (float32)EcoderLen;
if (L1 == L2)
{
fx = 0;
fy = L1;
}
else
{
ftemp1 = L2 - L1;
ftemp2 = L1 + L2;
ftemp2 = ftemp2 / ftemp1;
ftemp2 = ftemp2 * (float32)EcoderLen / 2.0;
ftemp1 = 1.0 - cos(sita);
fx = -(ftemp1 * ftemp2);
ftemp1 = sin(sita);
fy = ftemp1 * ftemp2;
}
fsin = sin(RobSta.sita); //坐标变换
fcos = cos(RobSta.sita);
RobSta.x += (fx * fcos - fy * fsin);
RobSta.y += (fx * fsin + fy * fcos);
RobSta.sita += sita;
//Check the robot's environment;
RobSta.Envir = ReadBus(FUredSwAddr);
}
//------------------------------------------------------------------------------
//Cfg the robot's temporarily aim state;
//------------------------------------------------------------------------------
void ConRobAimSta()
{
switch(RobSta.Envir)
{
//case RobAimSta.Envir:
// break;
case ObstaF:
break;
case ObstaL:
break;
case ObstaR:
break;
default:break;
}
}
/*
void RegulateSpeed(Uchar RM) // RM:short for regulate mode;
{
Uint16 FMD1,FMD2;
Uchar sp1,sp2;
if( RobSta.sita == RobAimSta.sita)
return;
else
{
switch(RM)
{
case 1: if( RobSta.sita > RobAimSta.sita ) //Go along borken line;
{
FMD1 = M1Reverse;
FMD2 = M2Forward;
sp1 = 20;
sp2 = 20;
}
else
{
FMD1 = M1Forward;
FMD2 = M2Reverse;
sp1 = 20;
sp2 = 20;
}
break;
case 2: if( RobSta.sita > RobAimSta.sita ) //Go along curve;
{
FMD1 = M1FreeStop;
FMD2 = M2Forward;
//sp1 = 10;
sp2 = 40;
}
else
{
FMD1 = M1Forward;
FMD2 = M2FreeStop;
sp1 = 40;
//sp2 = 10;
}
break;
case 3: if( RobSta.sita > RobAimSta.sita ) //Go along arc;
{
FMD1 = M1Forward;
FMD2 = M2Forward;
sp1 = 10;
sp2 = 40;
}
else
{
FMD1 = M1Forward;
FMD2 = M2Forward;
sp1 = 40;
sp2 = 10;
}
break;
default :break;
}
WriteBus(FMotorAddr,FMD1);
WriteBus(FMotorAddr,FMD2);
CfgPWM(1,50,sp1);
CfgPWM(2,50,sp2);
}
}
*/
void GoAlong()
{
}
void CfgMotorDir(Uchar n,Uint16 nDir)
{
MDir = (MDir & (~(0x0003 << ((n-1) * 2)))) | (nDir << ((n-1) * 2));
}
//-----------------------------------------------------------------------------
//Function:used for test the motor's speed
//-----------------------------------------------------------------------------
void CaMotorSp(void)
{
// LEcoderCount = LEcoderCount + abs(EvaRegs.T2CNT - 0x8000);
LEcoderCount = LEcoderCount + EvaRegs.T2CNT - 0x8000 ;
EvaRegs.T2CNT = 0x8000;
}
//===========================================================================
// No more.
//===========================================================================
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -