void init(void)
{
int i;
volatile WordVal src_addr = {SRC_ADDR};
volatile WordVal src_pan_id = {SRC_PAN_ID};
SetupClock();
SwitchClocks();
SetupPorts();
for (i = 0; i < 6; i++)
{
LED_RED = ~LED_RED;
delay_ms(50);
LED_YLW1 = ~LED_YLW1;
delay_ms(50);
LED_YLW2 = ~LED_YLW2;
delay_ms(50);
LED_BLU = ~LED_BLU;
delay_ms(50);
}
SetupUART1();
SetupInterrupts();
EnableIntU1TX;
EnableIntU1RX;
radioInit(src_addr, src_pan_id, 150, 150);
atSetPromMode(1); //This turns off Automatic Acknowledgements and puts the radio in prom mode
radioSetChannel(MY_CHAN); //Set to my channel
//atSetAntDiversity(1);
}
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;
}
int16_t main(void)
{
unsigned int buttonCounter = 20000;
int i;
/* Configure the oscillator for the device */
ConfigureOscillator();
/* Initialize IO ports and peripherals */
InitApp();
//initSerial();
initSPI1();
initSPI2();
InitI2C();
__delay32(1600000); // allow for POR for all devices
initnRF();
ControlByte = 0x00D0; // mpu6050 address
InitMPU6050(ControlByte);
// InitHMC5883L();
__delay_ms(500);
/** Init control loop *****************************************************/
readSensorData();
accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG);
accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;
AngleOffset[0] = accXangle;
AngleOffset[1] = accYangle;
CalibrateGyro(); // Finding the gyro zero-offset
SetupInterrupts();
////// int serStringN = 14;
////// char nRFstatus = 0;
//////// int i;
////// delaytime = 1;
////// bool mode = 0;
while(1)
{
/** Read Button RB7 for enable steppers *******************************/
if (buttonState == 0 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 1;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 1 && !PORTBbits.RB7 && !buttonCounter)
{
enableSteppers = 2;
__delay32(40000000);
buttonState = 2;
buttonCounter = 20000;
enableSteppers = 1;
}
if (buttonState == 2 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 3;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 3 && !PORTBbits.RB7 && !buttonCounter)
{
buttonState = 0;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonCounter)
{
buttonCounter--;
}
for (i=0;i<100;i++)
{
i=i;
}
// __delay32(100);
}
}
