/**
*
* This function sets up the interrupt example.
*
* @param None.
*
* @return
* - XST_SUCCESS to indicate Success
* - XST_FAILURE to indicate Failure.
*
* @note None
*
****************************************************************************/
static int TmrInterruptExample(void)
{
int Status;
/*
* Make sure the interrupts are disabled, in case this is being run
* again after a failure.
*/
/*
* Connect the Intc to the interrupt subsystem such that interrupts can
* occur. This function is application specific.
*/
Status = SetupInterruptSystem(INTC_DEVICE_ID, &InterruptController);
if (Status != XST_SUCCESS) {
return XST_FAILURE;
}
/*
* Set up the Ticker timer
*/
Status = SetupTicker();
if (Status != XST_SUCCESS) {
return Status;
}
/*
* Set up the PWM timer
*/
Status = SetupPWM();
if (Status != XST_SUCCESS) {
return Status;
}
/*
* Enable interrupts
*/
Status = WaitForDutyCycleFull();
if (Status != XST_SUCCESS) {
return Status;
}
/*
* Stop the counters
*/
XTtcPs_Stop(&(TtcPsInst[TTC_TICK_DEVICE_ID]));
XTtcPs_Stop(&(TtcPsInst[TTC_PWM_DEVICE_ID]));
return XST_SUCCESS;
}
int main ( void )
{
fun_queue = queueInit(FUN_Q_LEN);
rx_pay_queue = pqInit(12); //replace 12 with a #define const later
test_function tf;
/* Initialization */
SetupClock();
SwitchClocks();
SetupPorts();
SetupInterrupts();
SetupI2C();
SetupADC();
SetupTimer1();
SetupPWM();
SetupTimer2();
gyroSetup();
xlSetup();
dfmemSetup();
WordVal pan_id = {RADIO_PAN_ID};
WordVal src_addr = {RADIO_SRC_ADDR};
WordVal dest_addr = {RADIO_DEST_ADDR};
radioInit(src_addr, pan_id, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetDestAddr(dest_addr);
radioSetChannel(RADIO_MY_CHAN);
char j;
for(j=0; j<3; j++){
LED_2 = ON;
delay_ms(500);
LED_2 = OFF;
delay_ms(500);
}
LED_2 = ON;
EnableIntT2;
while(1){
while(!queueIsEmpty(fun_queue))
{
rx_payload = pqPop(rx_pay_queue);
tf = (test_function)queuePop(fun_queue);
(*tf)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload));
payDelete(rx_payload);
}
}
return 0;
}
void domex_setup(void) {
//int rep;
signal(SIGINT, sig_handler);
signal(SIGALRM, sig_handler);
// Set up gpi pointer for direct register access
setup_io();
// GPIO23 - NTX2 enable
INP_GPIO(23); // must use INP_GPIO before we can use OUT_GPIO
OUT_GPIO(23);
// GPIO40 and GPIO45 are audio and also support PWM Connect to analogue audio circitury via R21 and R27
// GPIO18 - NTX2 Data
// Set GPIO18 to PWM Function
INP_GPIO(18); // must use INP_GPIO before we can use OUT_GPIO or SET_GPIO_ALT
//OUT_GPIO(18); // For BitBang
SET_GPIO_ALT(18, 5); // For PWM
delayMicrosecs(110);
SetupPWM();
/*
// Setup timer
struct itimerval new,old;
new.it_interval.tv_usec = 0;
new.it_interval.tv_sec = 0;
new.it_value.tv_usec = 10;
new.it_value.tv_sec = 0;
setitimer (ITIMER_REAL, &new, &old);
*/
}
int main(void)
{
// Set system frequency to 48MHz
SystemFrequency = ConfigurePLL(12000000UL, 48000000UL);
// Enable clock to IO Configuration block. Needed for UART, SPI, I2C, etc...
SYSCON_SYSAHBCLKCTRL_IOCON(SYSAHBCLKCTRL_ENABLE);
SysTick_Config(SystemFrequency/10000 - 1); // Generate interrupt each .1 ms
// set direction on port 0_7 (pin 28) to output
GPIO0_DIR(7, GPIO_OUTPUT);
GPIO0_DATA(7, 1); // turn on diagnostic led
GPIO1_DIR(4, GPIO_OUTPUT); // configure port 1_5 (pin 14) for output
GPIO1_DIR(5, GPIO_OUTPUT); // configure port 1_5 (pin 14) for output
// PWM output (pin 1) is tied to 1,2EN on H-bridge (SN754410 Quad Half H-bridge) //
SetupPWM();
MotorDir(0, 0); // turn on braking at 100%
MotorPower(0, 100);
// motor details:
// 120:1 gearbox
// 8 teeth on motor gear
// 32 teeth on gear with optical wheel
// 4 edges per rot of optical wheel
// 8/32*120*4 = 106.666 edges per rev
// At 5.5V max speed with just 2 gears,
// pulse width is avg 9.5 ms.
//
// a full stop should be no edge for about 150ms
// and motor on brake.
//
g_motorPos = 0;
g_motorStop = 0;
g_motorTime = 0;
g_motorInc = 0;
// setup interrupts on PIO1_0
IOCON_PIO1_0_MODE(PIO1_0_MODE_PULLDOWN_RESISTOR);
GPIO1_DIR(0, GPIO_INPUT);
GPIO1_IS(0, GPIO_EDGE_SENSITIVE);
GPIO1_IBE(0, GPIO_BOTH_EDGES);
GPIO1_IE(0, GPIO_INTERRUPT_ENABLE);
NVIC_EnableIRQ(EINT1_IRQn);
U64 time1, time2, oldTime;
int pos, oldPos;
oldTime = 0xFFFFFFFFFFFFFFFF;
U64 systickcnt;
int desiredMotorInc; // g_motorInc can only be changed when in a stopped state
while (1)
{
FlashLED();
desiredMotorInc = g_motorInc = 1;
g_motorStop = 100;
MotorDir(0, 1);
MotorPower(0, 100);
while (1)
{
do {
time1 = g_motorTime;
pos = g_motorPos;
time2 = g_motorTime;
} while (time1 != time2);
if (pos > g_motorStop)
{
desiredMotorInc = -1;
}
else if (pos < g_motorStop)
{
desiredMotorInc = 1;
}
systickcnt = SysTickCnt;
if ((systickcnt - time2) > 200 /*ms*/)
{
// stopped
if (pos == g_motorStop)
{
break; //achieved goal pos and we are stopped
}
if (desiredMotorInc != g_motorInc)
{
g_motorInc = desiredMotorInc; // g_motorInc must always be 1 or -1, otherwise we lose track of position
// change direction and turn power back on
MotorDir(0, desiredMotorInc);
MotorPower(0, 20);
}
}
}
FlashLED();
desiredMotorInc = g_motorInc = -1;
g_motorStop = 0;
MotorDir(0, -1);
MotorPower(0, 100);
while (1)
{
do {
time1 = g_motorTime;
pos = g_motorPos;
time2 = g_motorTime;
} while (time1 != time2);
if (pos > g_motorStop)
{
desiredMotorInc = -1;
}
else if (pos < g_motorStop)
{
desiredMotorInc = 1;
}
systickcnt = SysTickCnt;
if ((systickcnt - time2) > 200 /*ms*/)
{
// stopped
if (pos == g_motorStop)
{
break; //achieved goal pos and we are stopped
}
if (desiredMotorInc != g_motorInc)
{
g_motorInc = desiredMotorInc; // g_motorInc must always be 1 or -1, otherwise we lose track of position
// change direction and turn power back on
MotorDir(0, desiredMotorInc);
MotorPower(0, 20);
}
}
}
}
}
