void pic_init()
{
pic_remap(OL_INTERRUPT_BASE, OL_INTERRUPT_BASE+8);
initPIT(100); // program pic to 100 hertz
}
void initMainHardware(void)
{
disableIrq(); /* Ensure INTC current prority=0 & enable IRQ */
initBTB();
initModesAndClock();
initPeriClkGen() ;
initPads ();
disableWatchdog();
init_LinFLEX_0_UART ();
initCAN_1(); /* Initialize FLEXCAN 0*/
initSBC(); //init SBC for CAN
initADC();
initINTC(); /* Initialize INTC for software vector mode */
initPIT(); /* Initialize PIT1 for 1KHz IRQ, priority 2 */
// initSwIrq4(); /* Initialize software interrupt 4 */
SIU.PSMI[0].R = 1; //can1rxd=43
SIU.PSMI[7].R = 2; //dspi1_sscl=114
SIU.PSMI[8].R = 2; //dspi1_sscl=114
SIU.PSMI[9].R = 3; //dspi1_sscl=114
EMIOS_0.MCR.B.GPRE= 63; /* Divide 64 MHz sysclk by 63+1 = 64 for 1MHz eMIOS clk*/
EMIOS_0.MCR.B.GPREN = 1; /* Enable eMIOS clock */
EMIOS_0.MCR.B.GTBE = 1; /* Enable global time base */
EMIOS_0.MCR.B.FRZ = 1; /* Enable stopping channels when in debug mode */
EMIOS_0.CH[0].CADR.R = 14999; /* Period will be 19999+1 = 20000 clocks (20 msec)*/
EMIOS_0.CH[0].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
EMIOS_0.CH[0].CCR.B.BSL = 0x3; /* Use internal counter */
EMIOS_0.CH[0].CCR.B.UCPRE=0; /* Set channel prescaler to divide by 1 */
EMIOS_0.CH[0].CCR.B.UCPEN = 1; /* Enable prescaler; uses default divide by 1*/
EMIOS_0.CH[0].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode*/
EMIOS_0.CH[23].CADR.R = 999; /* Period will be 999+1 = 1000 clocks (1 msec)*/
EMIOS_0.CH[23].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
EMIOS_0.CH[23].CCR.B.BSL = 0x3; /* Use internal counter */
EMIOS_0.CH[23].CCR.B.UCPRE=0; /* Set channel prescaler to divide by 1 */
EMIOS_0.CH[23].CCR.B.UCPEN = 1; /* Enable prescaler; uses default divide by 1*/
EMIOS_0.CH[23].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode*/
EMIOS_0.CH[4].CADR.R = 0; /* Leading edge when channel counter bus=0*/
EMIOS_0.CH[4].CBDR.R = 1500; /* Trailing edge when channel counter bus=1400 Middle, 1650 Right Max, 1150 Left Max*/
EMIOS_0.CH[4].CCR.B.BSL = 0x01; /* Use counter bus B */
EMIOS_0.CH[4].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output */
EMIOS_0.CH[4].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[28].R = 0x0600; /* MPC56xxS: Assign EMIOS_0 ch 6 to pad */
EMIOS_0.CH[6].CADR.R = 000; /* Leading edge when channel counter bus=0*/
EMIOS_0.CH[6].CBDR.R = 950; /* Trailing edge when channel counter bus=500*/
EMIOS_0.CH[6].CCR.B.BSL = 0x0; /* Use counter bus A (default) */
EMIOS_0.CH[6].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output */
EMIOS_0.CH[6].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[30].R = 0x0600; /* MPC56xxS: Assign EMIOS_0 ch 6 to pad */
EMIOS_0.CH[7].CADR.R = 0; /* Leading edge when channel counter bus=0*/
EMIOS_0.CH[7].CBDR.R = 950; /* Trailing edge when channel's counter bus=999*/
EMIOS_0.CH[7].CCR.B.BSL = 0x0; /* Use counter bus A (default) */
EMIOS_0.CH[7].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output*/
EMIOS_0.CH[7].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[31].R = 0x0600; /* MPC56xxS: Assign EMIOS_0 ch 7 to pad */
EMIOS_0.CH[3].CADR.R = 250; /* Ch 3: Match "A" is 250 */
EMIOS_0.CH[3].CBDR.R = 500; /* Ch 3: Match "B" is 500 */
EMIOS_0.CH[3].CCR.R= 0x000000E0; /* Ch 3: Mode is OPWMB, time base = ch 23 */
EMIOS_0.CH[2].CCR.R= 0x01020082; /* Ch 2: Mode is SAIC, time base = ch 23 */
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
INTC.CPR.B.PRI = 0; /* Single Core: Lower INTC's current priority */
asm(" wrteei 1"); /* Enable external interrupts */
}
void main (void) {
T_UWORD luw_i;
T_UBYTE lub_CountIndice = 0;
initModesAndClock(); /* Initialize mode entries and system clock */
INTC_InstallINTCInterruptHandler(isr,59,1);
INTC.CPR.R = 0;
initGPIO();
initPIT(ValTMR_0, ValTMR_1);
while (1) {
if(rub_FlagValUpAut || rub_FlagValUpMan){
if(rub_Fled){
rub_Fled = 0;
SIU.GPDO[lub_CountIndice].B.PDO = 1;
SIU.GPDO[10].B.PDO = 0;
SIU.GPDO[11].B.PDO = 1;
lub_CountIndice++;
if(lub_CountIndice >= 10){
lub_CountIndice = 10;
rub_FlagValUpAut = 0;
rub_FValAutUP = 0;
}
}
}
/******************************************************************************/
else if(rub_FlagValDownAut || rub_FlagValDownMan){
if(rub_Fled){
rub_Fled = 0;
if(lub_CountIndice <= 1) {
lub_CountIndice = 1;
rub_FlagValDownAut = 0;
rub_FValAutDown = 0;
SIU.GPDO[11].B.PDO = 1;
}
SIU.GPDO[lub_CountIndice-1].B.PDO = 0;
SIU.GPDO[10].B.PDO = 1;
SIU.GPDO[11].B.PDO = 0;
lub_CountIndice--;
if(lub_CountIndice == 0 && rub_FlagValAnPi){
SIU.GPDO[11].B.PDO = 1;
PIT.CH[1].TCTRL.B.TEN = 0;
PIT.CH[1].TCTRL.B.TEN = 1;
PIT.CH[1].LDVAL.R = ValTMR_1;
PIT.CH[1].TFLG.B.TIF = 1;
rub_FlagValAnPi = 0;
while(!PIT.CH[1].TFLG.B.TIF);
PIT.CH[1].TFLG.B.TIF = 1;
rub_Fled = 0;
rub_FlagValDownAut = 0;
rub_FlagValDownMan = 0;
rub_FlagValUpAut = 0;
rub_FlagValUpMan = 0;
rub_FValAutDown = 0;
rub_FValAutUP = 0;
}
}
}
////////////////////////////////////////////////
else if(!rub_FlagValUpAut && !rub_FlagValUpMan && !rub_FlagValDownAut && !rub_FlagValDownMan){
rub_Fled = 0;
SIU.GPDO[10].B.PDO = 1;
SIU.GPDO[11].B.PDO = 1;
}
}
}
void main (void) {
volatile uint32_t i = 0; /* Dummy idle counter */
uint8_t success=0;
uint8_t byteReceived=0;
uint8_t opcode=0;
uint8_t payload=0;
char msg[32];
uint8_t msgLength=0;
clock = 0;
initModesAndClock(); /* MPC56xxP/B/S: Initialize mode entries, set sysclk = 64 MHz*/
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
EXCEP_InitExceptionHandlers(); /* Initialize exceptions: only need to load IVPR */
initADC();
initPIT(); /* Initialize PIT1 for 10Hz IRQ, priority 2 */
initPads(); /* Initialize software interrupt 4 */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_1(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch8(); /* Initialize eMIOS 0 channel 8 as modulus counter*/
//just to make sure the wheels are facing straight
initDrive();
Drive();
SIU.PCR[64].R = 0x0100; /* Program the drive enable pin of S1 (PE0) as input*/
while((SIU.PGPDI[2].R & 0x80000000) == 0x80000000); /*Wait until S1 switch is pressed*/
for(i=0;i<100000;i++);
while((SIU.PGPDI[2].R & 0x80000000) != 0x80000000); /*Wait until S1 switch is released*/
INTC_InitVector();
INTC_InstallINTCInterruptHandler(&SwIrq4ISR,4,3);
INTC_InstallINTCInterruptHandler(&EOC_ISR,62,5);
INTC_InstallINTCInterruptHandler(&Pit1ISR,60,2);
INTC_InstallINTCInterruptHandler(&Pit2ISR,61,4);
INTC_InstallINTCInterruptHandler(&Pit3ISR,127,4);
initSerial();
flag_lineDone = -1;
initCamera();
initSteeringController();
INTC_InitINTCInterrupts();
INTC.CPR.B.PRI = 0; /* Single Core: Lower INTC's current priority */
initDrive();
// setPWMRw(48);
// setPWMLw(48);
setDirection(FORWARD);
in = getInBuffer();
out = getOutBuffer();
MESSAGE("I'm running\n\r");
fifo_write(&out->fifo,msg,msgLength);
while(isCameraReady()!=STATE_READY);
for(;;)
{
if(!(flag_lineDone==1))
{
Drive();
success=0;
success = fifo_read(&in->fifo,&byteReceived,1);
if(success==1)
{
if(opcode==0)
{
opcode = byteReceived;
MESSAGE("Command Received: ");
fifo_write(&out->fifo,msg,msgLength);
}
else
{
payload = byteReceived;
switch(opcode)
{
case DRIVE:
// TransmitData("Drive Command\n\r");
MESSAGE("Drive Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setDirection(payload);
break;
case SET_SPEED:
// TransmitData("Set Speed Command\n\r");
MESSAGE("Set Speed Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setPWMRw(payload);
setPWMLw(payload);
break;
case STEERING:
// TransmitData("Set Steering Command\n\r");
MESSAGE("Set Steering Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setAngle(payload);
break;
default:
// TransmitData("Bad command\n\r");
MESSAGE("Bad command\n\r");
fifo_write(&out->fifo,msg,msgLength);
break;
}
opcode = 0;
}
}
}
if(TRANSMISSION_DONE()&&(out->fifo.length>0))
Tx();
if(RECEPTION_DONE()&&(in->fifo.length<IN_BUFFER_SIZE))
Rx();
}
}
