void MyMIWI_Init(void) {
// Configure Pins for MRF24J40MB
mPORTESetBits(RST_MIWI);
mPORTESetPinsDigitalOut(RST_MIWI);
mPORTBSetBits(MIWI_WAKE);
mPORTBSetPinsDigitalOut(MIWI_WAKE);
// Configure the INT3 controller for MIWI
// Set RD10/INT3 as input
mPORTDSetPinsDigitalIn(BIT_10);
// Clear corresponding bits in INTCON for falling edge trigger
INTCONCLR = _INTCON_INT3EP_MASK;
// Set up interrupt prioirty and sub-priority
INTSetVectorPriority(INT_EXTERNAL_3_VECTOR, My_INT_EXTERNAL_3_PRIORITY);
INTSetVectorSubPriority(INT_EXTERNAL_3_VECTOR, My_INT_EXTERNAL_3_SUB_PRIORITY);
// Clear the interrupt flags
INTClearFlag(INT_INT3);
// Enable INT3
INTEnable(INT_INT3, INT_ENABLED);
// WARNING : Change in file MRF24J40.c in Microchip Application Library
// the line : void __ISR(_EXTERNAL_1_VECTOR, ipl4) _INT1Interrupt(void)
// by : void __ISR(_EXTERNAL_3_VECTOR, ipl4) _INT3Interrupt(void)
}
void MyWIFI_Init(void) {
// Configure Pins for MRF24J40MB
mPORTESetBits(RST_WIFI);
mPORTESetPinsDigitalOut(RST_WIFI);
mPORTBSetBits(WIFI_HIB);
mPORTBSetPinsDigitalOut(WIFI_HIB);
mPORTBClearBits(WIFI_WP); // Has to be pulled low
mPORTBSetPinsDigitalOut(WIFI_WP);
/*
// Configure the INT4 controller for WIFI
// Set RD11/INT4 as input
mPORTDSetPinsDigitalIn(BIT_11);
// Clear corresponding bits in INTCON for falling edge trigger
INTCONCLR = _INTCON_INT4EP_MASK;
// Set up interrupt prioirty and sub-priority
INTSetVectorPriority(INT_EXTERNAL_4_VECTOR, My_INT_EXTERNAL_4_PRIORITY);
INTSetVectorSubPriority(INT_EXTERNAL_4_VECTOR, My_INT_EXTERNAL_4_SUB_PRIORITY);
// Clear the interrupt flags
INTClearFlag(INT_INT4);
// Enable INT4
INTEnable(INT_INT4, INT_ENABLED);
*/
}
void motorinit()
{
//SET MOTOR1 GPIOS
//LATECLR = 0x001F;
//TRISECLR = 0x001F;
mPORTESetBits(BIT_2); //set standby bit
}
void MyCyclone_Write(unsigned int theAddress, unsigned int theData)
{
unsigned int intStatus;
intStatus = INTDisableInterrupts();
mPORTEClearBits(CS_FPGA);
MySPI_PutC(theAddress | 0x8000); // Bit 7 = R/W = 1
MySPI_PutC(theData);
mPORTESetBits(CS_FPGA);
INTRestoreInterrupts(intStatus);
}
unsigned int MyCyclone_Read(unsigned int theAddress)
{
unsigned int theData;
unsigned int intStatus;
intStatus = INTDisableInterrupts();
mPORTEClearBits(CS_FPGA);
MySPI_PutC(theAddress & 0x7FFF); // Bit 7 = R/W = 0
theData = MySPI_GetC();
mPORTESetBits(CS_FPGA);
INTRestoreInterrupts(intStatus);
return(theData);
}
void InitializeSystem()
{
SYSTEMConfigWaitStatesAndPB(CLOCK_FREQ);
mOSCSetPBDIV(OSC_PB_DIV_4); // Set to get 20MHz PB clock
//mOSCSetPBDIV(OSC_PB_DIV_2);
CheKseg0CacheOn();
mJTAGPortEnable(0);
// Initialize the pins to all digital output and driven to ground.
// Exception is RE7 and RE6 which are switch inputs
PORTSetPinsDigitalIn(IOPORT_E, BIT_6);
PORTSetPinsDigitalIn(IOPORT_E, BIT_7);
mPORTASetPinsDigitalOut(0xFFFF);
mPORTBSetPinsDigitalOut(0xFFFF);
mPORTCSetPinsDigitalOut(0xFFFF);
mPORTDSetPinsDigitalOut(0xFFFF);
mPORTESetPinsDigitalOut(0xFF3F);
mPORTFSetPinsDigitalOut(0xFFFF);
mPORTGSetPinsDigitalOut(0xFFFF);
mPORTAClearBits(0xFFFF);
mPORTBClearBits(0xFFFF);
mPORTCClearBits(0xFFFF);
mPORTDClearBits(0xFFFF);
mPORTEClearBits(0xFF3F);
mPORTESetBits(0x000F); // LED latches need to be set high for off
mPORTFClearBits(0xFFFF);
mPORTGClearBits(0xFFFF);
INTEnableSystemMultiVectoredInt();
#ifdef SANITY_CHECK
mLED_Green_On();
#endif
//LCD_Initialize();
//WIFI_Initialize();
//SPRINKLER_Initialize();
//RTCC_Initialize();
//SERIALUSB_Initialize();
SDCARD_Initialize();
TCPIP_Initialize();
}
/*****************************************************************************
* CONFIG_3909()
*
* This function configures the MCP3909 to transmit data from its two
* A/D converters to the PIC32 via the SPI port
*****************************************************************************/
void CONFIG_3909()
{
DBPRINTF("CONFIG_3909...\n");
// SPI mode code
int code = 0xA4;
// set CS high
mPORTDSetBits(BIT_9);
// set MCLR low
mPORTEClearBits(BIT_0);
// delay
DelayMs(5);
// set MCLR high
mPORTESetBits(BIT_0);
// set CS low
mPORTDClearBits(BIT_9);
// feed SPI mode code to MCP3909
SpiChnPutC(1, code);
DelayMs(1000);
SpiChnPutC(1, 0x12345678);
int data;
data = getcSPI1();
char a = (char)data;
char * c;
c = &a;
DBPUTC(c);
data = getcSPI1();
a = (char)data;
c = &a;
DBPUTC(c);
}
int main(void)
{
//LOCALS
unsigned int temp;
unsigned int channel1, channel2;
M1_stepPeriod = M2_stepPeriod = M3_stepPeriod = M4_stepPeriod = 50; // in tens of u-seconds
unsigned char M1_state = 0, M2_state = 0, M3_state = 0, M4_state = 0;
SYSTEMConfig(GetSystemClock(), SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
/* TIMER1 - now configured to interrupt at 10 khz (every 100us) */
OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_1, T1_TICK);
ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_2);
/* TIMER2 - 100 khz interrupt for distance measure*/
OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, T2_TICK);
ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_3); //It is off until trigger
/* PORTA b2 and b3 for servo-PWM */
mPORTAClearBits(BIT_2 | BIT_3);
mPORTASetPinsDigitalOut(BIT_2 | BIT_3);
/* ULTRASONICS: some bits of PORTB for ultrasonic sensors */
PORTResetPins(IOPORT_B, BIT_8 | BIT_9| BIT_10 | BIT_11 );
PORTSetPinsDigitalOut(IOPORT_B, BIT_8 | BIT_9| BIT_10 | BIT_11); //trigger
/* Input Capture pins for echo signals */
//interrupt on every risging/falling edge starting with a rising edge
PORTSetPinsDigitalIn(IOPORT_D, BIT_8| BIT_9| BIT_10| BIT_11); //INC1, INC2, INC3, INC4 Pin
mIC1ClearIntFlag();
OpenCapture1( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//front
ConfigIntCapture1(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3);
OpenCapture2( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//back
ConfigIntCapture2(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3);
OpenCapture3( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//left
ConfigIntCapture3(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3);
OpenCapture4( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//right
ConfigIntCapture4(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3);
/* PINS used for the START (RD13) BUTTON */
PORTSetPinsDigitalIn(IOPORT_D, BIT_13);
#define CONFIG (CN_ON | CN_IDLE_CON)
#define INTERRUPT (CHANGE_INT_ON | CHANGE_INT_PRI_2)
mCNOpen(CONFIG, CN19_ENABLE, CN19_PULLUP_ENABLE);
temp = mPORTDRead();
/* PORT D and E for motors */
//motor 1
mPORTDSetBits(BIT_4 | BIT_5 | BIT_6 | BIT_7); // Turn on PORTD on startup.
mPORTDSetPinsDigitalOut(BIT_4 | BIT_5 | BIT_6 | BIT_7); // Make PORTD output.
//motor 2
mPORTCSetBits(BIT_1 | BIT_2 | BIT_3 | BIT_4); // Turn on PORTC on startup.
mPORTCSetPinsDigitalOut(BIT_1 | BIT_2 | BIT_3 | BIT_4); // Make PORTC output.
//motor 3 and 4
mPORTESetBits(BIT_0 | BIT_1 | BIT_2 | BIT_3 |
BIT_4 | BIT_5 | BIT_6 | BIT_7); // Turn on PORTE on startup.
mPORTESetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2 | BIT_3 |
BIT_4 | BIT_5 | BIT_6 | BIT_7); // Make PORTE output.
// UART2 to connect to the PC.
// This initialization assumes 36MHz Fpb clock. If it changes,
// you will have to modify baud rate initializer.
UARTConfigure(UART2, UART_ENABLE_PINS_TX_RX_ONLY);
UARTSetFifoMode(UART2, UART_INTERRUPT_ON_TX_NOT_FULL | UART_INTERRUPT_ON_RX_NOT_EMPTY);
UARTSetLineControl(UART2, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
UARTSetDataRate(UART2, GetPeripheralClock(), BAUD);
UARTEnable(UART2, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));
// Configure UART2 RX Interrupt
INTEnable(INT_SOURCE_UART_RX(UART2), INT_ENABLED);
INTSetVectorPriority(INT_VECTOR_UART(UART2), INT_PRIORITY_LEVEL_2);
INTSetVectorSubPriority(INT_VECTOR_UART(UART2), INT_SUB_PRIORITY_LEVEL_0);
/* PORTD for LEDs - DEBUGGING */
mPORTDClearBits(BIT_0 | BIT_1 | BIT_2);
mPORTDSetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2);
// Congifure Change/Notice Interrupt Flag
ConfigIntCN(INTERRUPT);
// configure for multi-vectored mode
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
// enable interrupts
INTEnableInterrupts();
counterDistanceMeasure=600; //measure ULTRASONICS distance each 60 ms
while (1) {
/***************** Robot MAIN state machine *****************/
unsigned char ret = 0;
switch (Robo_State) {
case 0:
MotorsON = 0;
Robo_State = 0;
InvInitialOrientation(RESET);
TestDog(RESET);
GoToRoom4short(RESET);
BackToStart(RESET);
InitialOrientation(RESET);
GoToCenter(RESET);
GoToRoom4long(RESET);
break;
case 1:
ret = InvInitialOrientation(GO);
if (ret == 1) {
Robo_State = 2;
}
break;
case 2:
ret = TestDog(GO);
if (ret == 1) {
Robo_State = 3; //DOG not found
} else if (ret == 2) {
Robo_State = 4; //DOG found
}
break;
case 3:
ret = GoToRoom4short(GO);
if (ret == 1) {
Robo_State = 0;
}
break;
case 4:
ret = BackToStart(GO);
if (ret == 1) {
Robo_State = 5;
}
break;
case 5:
ret = GoToCenter(GO);
if (ret == 1) {
Robo_State = 6;
}
break;
case 6:
ret = GoToRoom4long(GO);
if (ret == 1) {
Robo_State = 0;
}
break;
}
if (frontDistance < 30 || backDistance < 30 || leftDistance < 30 || rightDistance < 30)
mPORTDSetBits(BIT_0);
else
mPORTDClearBits(BIT_0);
/***************************************************************/
/***************** Motors State Machine ************************/
if (MotorsON) {
/****************************
MOTOR MAP
M1 O-------------O M2 ON EVEN MOTORS, STEPS MUST BE INVERTED
| /\ | i.e. FORWARD IS BACKWARD
| / \ |
| || |
| || |
M3 O-------------O M4
*****************************/
if (M1_counter == 0) {
switch (M1_state) {
case 0: // set 0011
step (0x3 , 1);
if (M1forward)
M1_state = 1;
else
M1_state = 3;
break;
case 1: // set 1001
step (0x9 , 1);
if (M1forward)
M1_state = 2;
else
M1_state = 0;
break;
case 2: // set 1100
step (0xC , 1);
if (M1forward)
M1_state = 3;
else
M1_state = 1;
break;
case 3: // set 0110
default:
step (0x6 , 1);
if (M1forward)
M1_state = 0;
else
M1_state = 2;
break;
}
M1_counter = M1_stepPeriod;
step_counter[0]--;
if (directionNow == countingDirection)
step_counter[1]--;
}
if (M2_counter == 0) {
switch (M2_state) {
case 0: // set 0011
step (0x3 , 2);
if (M2forward)
M2_state = 1;
else
M2_state = 3;
break;
case 1: // set 0110
step (0x6 , 2);
if (M2forward)
M2_state = 2;
else
M2_state = 0;
break;
case 2: // set 1100
step (0xC , 2);
if (M2forward)
M2_state = 3;
else
M2_state = 1;
break;
case 3: // set 1001
default:
step (0x9 , 2);
if (M2forward)
M2_state = 0;
else
M2_state = 2;
break;
}
M2_counter = M2_stepPeriod;
}
if (M3_counter == 0) {
switch (M3_state) {
case 0: // set 0011
step (0x3 , 3);
if (M3forward)
M3_state = 1;
else
M3_state = 3;
break;
case 1: // set 1001
step (0x9 , 3);
if (M3forward)
M3_state = 2;
else
M3_state = 0;
break;
case 2: // set 1100
step (0xC , 3);
if (M3forward)
M3_state = 3;
else
M3_state = 1;
break;
case 3: // set 0110
default:
step (0x6 , 3);
if (M3forward)
M3_state = 0;
else
M3_state = 2;
break;
}
M3_counter = M3_stepPeriod;
}
if (M4_counter == 0) {
switch (M4_state) {
case 0: // set 0011
step (0x3 , 4);
if (M4forward)
M4_state = 1;
else
M4_state = 3;
break;
case 1: // set 0110
step (0x6 , 4);
if (M4forward)
M4_state = 2;
else
M4_state = 0;
break;
case 2: // set 1100
step (0xC , 4);
if (M4forward)
M4_state = 3;
else
M4_state = 1;
break;
case 3: // set 1001
default:
step (0x9 , 4);
if (M4forward)
M4_state = 0;
else
M4_state = 2;
break;
}
M4_counter = M4_stepPeriod;
}
} else {
//motors off
mPORTDSetBits(BIT_4 | BIT_5 | BIT_6 | BIT_7);
mPORTCSetBits(BIT_1 | BIT_2 | BIT_3 | BIT_4);
mPORTESetBits(BIT_0 | BIT_1 | BIT_2 | BIT_3 |
BIT_4 | BIT_5 | BIT_6 | BIT_7);
}
/************************************************************/
/******* TEST CODE, toggles the servos (from 90 deg. to -90 deg.) every 1 s. ********/
/* if (auxcounter == 0) {
servo1_angle = 0;
if (servo2_angle == 90)
servo2_angle = -90;
else
servo2_angle = 90;
auxcounter = 20000; // toggle angle every 2 s.
}
*/
servo1_angle = 0;
servo2_angle = -90;
/*
if (frontDistance > 13 && frontDistance < 17) {
servo2_angle = 90;
}
else
servo2_angle = -90;
*/
/*******************************************************************/
/****************** SERVO CONTROL ******************/
/*
Changing the global servoX_angle at any point in the code will
move the servo to the desired angle.
*/
servo1_counter = (servo1_angle + 90)*(18)/180 + 6; // between 600 and 2400 us
if (servo1_period == 0) {
mPORTASetBits(BIT_2);
servo1_period = SERVOMAXPERIOD; /* 200 * 100us = 20000us period */
}
servo2_counter = (servo2_angle + 90)*(18)/180 + 6; // between 600 and 2400 us
if (servo2_period == 0) {
mPORTASetBits(BIT_3);
servo2_period = SERVOMAXPERIOD; /* 200 * 100us = 20000us period */
}
/*****************************************************/
} /* end of while(1) */
return 0;
}
void motorLrev()
{
mPORTESetBits(BIT_4);
mPORTEClearBits(BIT_3);
}
void motorRrev()
{
mPORTESetBits(BIT_0);
mPORTEClearBits(BIT_1);
}
