void BOARD_Init() {
unsigned int dma_status;
unsigned int int_status;
//DEVCFG1bits.FCKSM = 0;
//mSYSTEMUnlock(int_status, dma_status);
//DEVCFG1bits.FCKSM = 0;
//DEVCFG2bits.FPLLIDIV=1;
SYSKEY = 0;
SYSKEY = 0xAA996655;
SYSKEY = 0x556699AA;
CFGCONbits.IOLOCK = 0;
SYSKEY = 0;
SYSTEMConfig(SYSTEM_CLOCK, SYS_CFG_ALL);
ANSELA=0;
ANSELB=0;
ANSELC=0;
//OSCConfig(OSC_FRC_PLL, OSC_PLL_MULT_20, OSC_PLL_POST_1, OSC_FRC_POST_1);
//OSCCONbits.NOSC=0x1;
OSCSetPBDIV(OSC_PB_DIV_1);
//mSYSTEMLock(int_status, dma_status);
// AD1PCFG = 0xffff;
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
SERIAL_Init();
//printf("This code compiled at %s on %s\r\n", __TIME__, __DATE__);
}
void hal_sys_init(){
SYSTEMConfigPerformance(SYS_CLK);
INTSetVectorPriority(INT_VECTOR_UART(UART3),3);
INTSetVectorPriority(INT_VECTOR_UART(UART5), 3);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTDisableInterrupts();
}
int32_t main(void)
{
DDPCONbits.JTAGEN = 0; // Disable the JTAG programming port
/*
SYS_CFG_WAIT_STATES (configures flash wait states from system clock)
SYS_CFG_PB_BUS (configures the PB bus from the system clock)
SYS_CFG_PCACHE (configures the pCache if used)
SYS_CFG_ALL (configures the flash wait states, PB bus, and pCache)*/
/* TODO Add user clock/system configuration code if appropriate. */
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
/*Configure Multivector Interrupt Mode. Using Single Vector Mode
is expensive from a timing perspective, so most applications
should probably not use a Single Vector Mode*/
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
/* TODO <INSERT USER APPLICATION CODE HERE> */
// initialise the IO pins and PWM outputs
TRISD = 0;
pickerBusTRIS = 0x00;
pickerBus = 0x00;
OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // vibration motor
OpenOC2( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // head led
OpenOC3( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // base led
OpenTimer2( T2_ON | T2_PS_1_4 | T2_SOURCE_INT, 1024);
SetDCOC1PWM(0);
SetDCOC2PWM(0);
SetDCOC3PWM(0);
setVac1off;
setVac2off;
USBOutHandle = 0;
USBInHandle = 0;
USBDeviceInit(); //usb_device.c. Initializes USB module SFRs and firmware
USBDeviceAttach();
init_component_picker();
/*while(1)
{
LATBbits.LATB0 = feederXHome;
LATBbits.LATB1 = feederZHome;
//ProcessIO();
}*/
}
//===========================
// START INTERRUPTS
//===========================
void StartInterrupts(void)
{
INT8 err;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable timer interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Timer.EnableInterrupt(TIMER_1);
// Timer.EnableInterrupt(TIMER_2);
// Timer.EnableInterrupt(TIMER_3);
// Timer.EnableInterrupt(TIMER_4);
// Timer.EnableInterrupt(TIMER_5);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable RX UART interrupts and disable TX interrupts.
// TX interrupts are disabled at init and only
// enabled when writing to the user's TX FIFO buffer
// with Uart.PutTxFifoBuffer(...)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Uart.EnableRxInterrupts (UART6); // Enable RX Interrupts for UART6
// Uart.DisableTxInterrupts(UART6); // Disable TX Interrupts for UART6
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable ADC interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Adc.EnableInterrupts(); // Works only when not in manual mode
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable InputCapture interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// InputCapture.EnableInterrupt(IC1);
// InputCapture.EnableInterrupt(IC2);
// InputCapture.EnableInterrupt(IC3);
// InputCapture.EnableInterrupt(IC4);
// InputCapture.EnableInterrupt(IC5);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable CAN interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Can.EnableInterrupt(CAN1);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable multi-vector interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
}
void SYS_Initialize ( void* data )
{
SYSTEMConfig(GetSystemClock(), SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
/* Disable JTAG to free up PORTA pins */
mJTAGPortEnable(DEBUG_JTAGPORT_OFF);
BSP_Initialize();
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
BSP_WriteString("*** UART Interrupt-driven Application Example ***\r\n");
BSP_WriteString("*** Type some characters and observe echo ***\r\n");
/* Initialize the Application */
APP_Initialize ( );
}
int32_t main(void)
{
#ifndef PIC32_STARTER_KIT
/*The JTAG is on by default on POR. A PIC32 Starter Kit uses the JTAG, but
for other debug tool use, like ICD 3 and Real ICE, the JTAG should be off
to free up the JTAG I/O */
DDPCONbits.JTAGEN = 0;
#endif
/*Refer to the C32 peripheral library documentation for more
information on the SYTEMConfig function.
This function sets the PB divider, the Flash Wait States, and the DRM
/wait states to the optimum value. It also enables the cacheability for
the K0 segment. It could has side effects of possibly alter the pre-fetch
buffer and cache. It sets the RAM wait states to 0. Other than
the SYS_FREQ, this takes these parameters. The top 3 may be '|'ed
together:
SYS_CFG_WAIT_STATES (configures flash wait states from system clock)
SYS_CFG_PB_BUS (configures the PB bus from the system clock)
SYS_CFG_PCACHE (configures the pCache if used)
SYS_CFG_ALL (configures the flash wait states, PB bus, and pCache)*/
/* TODO Add user clock/system configuration code if appropriate. */
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
/* Initialize I/O and Peripherals for application */
InitApp();
/*Configure Multivector Interrupt Mode. Using Single Vector Mode
is expensive from a timing perspective, so most applications
should probably not use a Single Vector Mode*/
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
/* TODO <INSERT USER APPLICATION CODE HERE> */
while(1)
{
//delayMs( );
}
}
void initPIC() {
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
initLEDs();
initSerialNU32v2();
// Setup and turn off electromagnets
EMAG1 = 0;
EMAG2 = 0;
TRISEbits.TRISE7 = 0;
TRISCbits.TRISC1 = 0;
// Direction Output
DIR = 1;
TRISAbits.TRISA9 = 0;
setup_counters();
CloseADC10();
#define PARAM1 ADC_MODULE_ON | ADC_FORMAT_INTG | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON
#define PARAM2 ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_16 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF
#define PARAM3 ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_31
#define PARAM4 ENABLE_AN0_ANA | ENABLE_AN1_ANA | ENABLE_AN2_ANA | ENABLE_AN3_ANA | ENABLE_AN5_ANA | ENABLE_AN15_ANA
OpenADC10(PARAM1, PARAM2, PARAM3, PARAM4, 0);
EnableADC10();
// 20kHz PWM signal, duty from 0-1000, pin D3
OpenTimer2(T2_ON | T2_PS_1_4, MAX_DUTY);
OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
SetDCOC4PWM(0);
// 200 Hz ISR
OpenTimer3(T3_ON | T3_PS_1_256, (6250/4 - 1));
//OpenTimer3(T3_ON | T3_PS_1_256, (62500 - 1));
mT3SetIntPriority(1);
mT3ClearIntFlag();
mT3IntEnable(1);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableSystemMultiVectoredInt();
}
void setupHardware()
{
SYSTEMConfig(SYS_CLOCK, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
mPORTDSetPinsDigitalOut(BIT_1); //yellow LED
mPORTGSetPinsDigitalOut(BIT_6); //green LED
//A1 PIR Input (CN4 module, RB2)
//A2 STB current consumption (AN3)
//D0 IR Output
mPORTDSetPinsDigitalOut(BIT_3); //D1 STB IRF control
mPORTDSetPinsDigitalIn(BIT_4); //D2 BUT OTG
mPORTDSetPinsDigitalOut(BIT_5); //D3 SHD RED LED Command OFF acqusition notificaiton
mPORTDSetPinsDigitalOut(BIT_6); //D4 SHD GREEN LED Command ON acqusition notificaiton
//D5 IR input (RD7)
//D6 SHD BUT1
//D7 SHD BUT2
mPORTBSetPinsDigitalOut(BIT_14); //D9 monitor relay control
DDPCONbits.JTAGEN = 0;
initLEDs();
initUART();
initADC();
setupCNModuleAnd_IR_PIR_Input();
//setupCNModuleAndPIRInput();
configureRTC();
setupIRTransmit();
switchMonitorOff();
switchSTBOff();
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
T2CONbits.ON = 1;
}
void openSerial(unsigned char serialPortIndex, unsigned long baudRate) {
// important to activate the RX for UART5. Information found on the net
if (serialPortIndex == SERIAL_PORT_5) {
PORTSetPinsDigitalIn(IOPORT_B, BIT_8);
}
UART_MODULE uart = getUartModule(serialPortIndex);
UARTConfigure(uart, UART_ENABLE_PINS_TX_RX_ONLY);
UARTSetFifoMode(uart, UART_INTERRUPT_ON_TX_NOT_FULL | UART_INTERRUPT_ON_RX_NOT_EMPTY);
UARTSetLineControl(uart, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
UARTSetDataRate(uart, GetPeripheralClock(), baudRate);
UARTEnable(uart, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));
INTEnable(INT_SOURCE_UART_RX(uart), INT_ENABLED);
INTSetVectorPriority(INT_VECTOR_UART(uart), INT_PRIORITY_LEVEL_2);
INTSetVectorSubPriority(INT_VECTOR_UART(uart), INT_SUB_PRIORITY_LEVEL_0);
// TODO : Move this code to Global Setup !
// configure for multi-vectored mode
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
// enable interrupts
INTEnableInterrupts();
}
int32_t main(void)
{
#ifndef PIC32_STARTER_KIT
/*The JTAG is on by default on POR. A PIC32 Starter Kit uses the JTAG, but
for other debug tool use, like ICD 3 and Real ICE, the JTAG should be off
to free up the JTAG I/O */
DDPCONbits.JTAGEN = 0;
#endif
/*Refer to the C32 peripheral library documentation for more
information on the SYTEMConfig function.
This function sets the PB divider, the Flash Wait States, and the DRM
/wait states to the optimum value. It also enables the cacheability for
the K0 segment. It could has side effects of possibly alter the pre-fetch
buffer and cache. It sets the RAM wait states to 0. Other than
the SYS_FREQ, this takes these parameters. The top 3 may be '|'ed
together:
SYS_CFG_WAIT_STATES (configures flash wait states from system clock)
SYS_CFG_PB_BUS (configures the PB bus from the system clock)
SYS_CFG_PCACHE (configures the pCache if used)
SYS_CFG_ALL (configures the flash wait states, PB bus, and pCache)*/
/* TODO Add user clock/system configuration code if appropriate. */
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
/* Initialize I/O and Peripherals for application */
InitApp();
/*Configure Multivector Interrupt Mode. Using Single Vector Mode
is expensive from a timing perspective, so most applications
should probably not use a Single Vector Mode*/
// 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);
// configure for multi-vectored mode
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
// enable interrupts
INTEnableInterrupts();
/* TODO <INSERT USER APPLICATION CODE HERE> */
//Open UART2
OpenUART2(UART_EN, UART_BRGH_FOUR|UART_RX_ENABLE | UART_TX_ENABLE, 21);
//Open SPI 1 channel
PORTBbits.RB11 = 1;
OpenSPI1( SPI_MODE8_ON | MASTER_ENABLE_ON | SEC_PRESCAL_1_1 | PRI_PRESCAL_1_1 | FRAME_ENABLE_OFF | CLK_POL_ACTIVE_HIGH | ENABLE_SDO_PIN , SPI_ENABLE );
SPI1BRG=39;
initRadio();
setTXAddress("UNIT2");
setRXAddress(0,"UNIT1");
char temp;
char text[6];
text[0]='H';
text[1]='e';
text[2]='l';
text[3]='l';
text[4]='o';
text[5]='!';
while(1)
{
setTransmitter();
PORTBbits.RB11 = 0;
DelayMs(20);
transmitData(&text[0],6);
printf("Hello world! \r\n");
PORTBbits.RB11 = 1;
DelayMs(20);
}
}
// Main Application
int main(int argc, char** argv) {
// Setup Main System
SYSTEMConfigPerformance(80000000L);
DDPCONbits.JTAGEN = 0;
// Initialize System Pins
initializeAllPins();
// Initialize the Real Time Clock
initializeRTCC();
// Initialize Communication Systems
initializeUART();
// Configure for multi-vectored mode & Enable Interrupts
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
// Configure XBee
xbee_baud.size = 1; strcpy(xbee_baud.data, "6");
xbee_channel.size = 2; strcpy(xbee_channel.data, "15");
xbee_network.size = 4; strcpy(xbee_network.data, "3421");
configureXBee(xbee_baud, xbee_channel, xbee_network);
// Loop Infinitely
while(1) {
// Update Terminal Every Second, using the RTCC
//while(!RtccGetSync());
// Update Termina Every 300mS
Delayms(200);
/*
//Copy new GPS string to Globals
if (gpsTempBuf.ready == 1) {
// Copy GPS TempBuf to Sentence
UINT8 gpsGGAPosition = 0;
UINT8 ggaPosition = 0;
int i = 0;
for (i = 0; i < gpsTempBuf.size; i++) {
gpsTempBuf.data[i] = gpsTempBuf.data[i];
// Parse specific GPS Data
if (gpsTempBuf.data[i] == ',') {
gpsGGAPosition++;
ggaPosition = 0;
}
char temp[1] = { gpsTempBuf.data[i] };
if (gpsTempBuf.data[i] != ',') {
switch(gpsGGAPosition) {
case 1:
gpsBaseCurrent.time.data[ggaPosition] = gpsTempBuf.data[i];
gpsBaseCurrent.time.size++;
ggaPosition++;
break;
case 2:
gpsBaseCurrent.Latitude.data[ggaPosition] = gpsTempBuf.data[i];
gpsBaseCurrent.Latitude.size++;
ggaPosition++;
break;
case 3:
gpsBaseCurrent.Longitude.data[ggaPosition] = gpsTempBuf.data[i];
gpsBaseCurrent.Longitude.size++;
ggaPosition++;
break;
case 4:
gpsBaseCurrent.Fix = atoi(temp);
ggaPosition++;
break;
case 5:
gpsBaseCurrent.NumSatellites = atoi(temp);
ggaPosition++;
break;
case 6:
gpsBaseCurrent.HorizontalDilution.data[ggaPosition] = gpsTempBuf.data[i];
gpsBaseCurrent.HorizontalDilution.size++;
ggaPosition++;
break;
case 7:
gpsBaseCurrent.Altitude.data[ggaPosition] = gpsTempBuf.data[i];
gpsBaseCurrent.Altitude.size++;
ggaPosition++;
break;
case 8:
gpsBaseCurrent.HeightOfGeoid.data[ggaPosition] = gpsTempBuf.data[i];
gpsBaseCurrent.HeightOfGeoid.size++;
ggaPosition++;
break;
default:
break;
}
}
}
// Update System Time
DFloat currentTime;
currentTime.timeBytes.RSV = 0x0000;
currentTime.timeBytes.HU = gpsBaseCurrent.time.data[0];
currentTime.timeBytes.HL = gpsBaseCurrent.time.data[1];
currentTime.timeBytes.MU = gpsBaseCurrent.time.data[2];
currentTime.timeBytes.ML = gpsBaseCurrent.time.data[3];
currentTime.timeBytes.SU = gpsBaseCurrent.time.data[4];
currentTime.timeBytes.SL = gpsBaseCurrent.time.data[5];
RtccSetTimeDate(currentTime.UValue, 0);
}
*/
/*
// Perform Magnetometer Calculations
float tempMX = (float) compassCurrent.Signed.X;
tempMX *= magGain[1] / 1000;
float tempMY = (float) compassCurrent.Signed.Y;
tempMY *= magGain[1] / 1000;
float tempMZ = (float) compassCurrent.Signed.Z;
tempMZ *= magGain[1] / 1000;
// Create Unit Vectors
float totalM = sqrtf(powf(tempMX,2) + powf(tempMY,2) + powf(tempMZ,2));
tempMX /= totalM;
tempMY /= totalM;
tempMZ /= totalM;
*/
// Convert to Degrees
//tempMX = acosf(tempMX) * degrees_per_radian;
//tempMY = acosf(tempMY) * degrees_per_radian;
//tempMZ = acosf(tempMZ) * degrees_per_radian;
// Match Current Angle with Calculated
//sprintf(buf, "%04d X(%+07.2f, %5.3f) Y(%+07.2f, %5.3f) Z(%+07.2f, %5.3f) %02dmS %02dC \r\n", outputs, angleCurrent.X.Value, tempMX, angleCurrent.Y.Value, tempMY, angleCurrent.Z.Value, tempMZ, angleCurrent.T, gyroCurrent.TU);
sprintf(buf, "%04d X(%+07.2f, %4d)\tY(%+07.2f, %4d)\tZ(%+07.2f, %4d)\tA(%3d, %4d)\tPWM(%3d, %3d, %3d, %3d) \r\n", outputs, angleCurrent.X.Value, errorReading[0], angleCurrent.Y.Value, errorReading[1], angleCurrent.Z.Value, errorReading[2], altitudeReading[0], errorReading[3], pwmReading[0], pwmReading[1], pwmReading[2], pwmReading[3]);
outputs++;
// Working, Shows Gyroscope Calculated Angles!
//sprintf(buf, "Gyro: %+08.3f, %+08.3f, %+08.3f\r\n", tempX, tempY, tempZ);
// Get Current Time
//tempTime.l=RtccGetTime();
// Format String
//sprintf(buf, "Gyro: %07f, %07f, %07f | Acl: %07.3f, %07.3f, %07.3f | Ang: %03.3f, %03.3f, %03.3f | UT: %02d mS | BST: %02x:%02x:%02x | FCT: %02x:%02x:%02x\r\n", tempX, tempY, tempZ, accelCurrent.X, accelCurrent.Y, accelCurrent.Z, angleCurrent.X.Value, angleCurrent.Y.Value, angleCurrent.Z.Value, angleCurrent.T, tempTime.hour, tempTime.min, tempTime.sec, timeFCBCurrent.hour, timeFCBCurrent.min, timeFCBCurrent.sec);
// Place on Bluetooth
putsBluetooth(buf, strlen(buf));
}
return (EXIT_SUCCESS);
}
int main(void)
{
unsigned int temp;
// Configure the device for maximum performance but do not change the PBDIV
// Given the options, this function will change the flash wait states, RAM
// wait state and enable prefetch cache but will not change the PBDIV.
// The PBDIV value is already set via the pragma FPBDIV option above..
SYSTEMConfig(GetSystemClock(), SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
/*PORT CONFIGURATION for UART*/
// Blinky LED for Uart activity
//mPORTAClearBits(BIT_7);
//mPORTASetPinsDigitalOut(BIT_7);
// PINS used for the buttons
PORTSetPinsDigitalIn(IOPORT_B, BIT_2 | BIT_3 | BIT_4);
#define CONFIG (CN_ON | CN_IDLE_CON)
#define INTERRUPT (CHANGE_INT_ON | CHANGE_INT_PRI_2)
mCNOpen(CONFIG, CN4_ENABLE | CN5_ENABLE | CN6_ENABLE,
CN4_PULLUP_ENABLE | CN5_PULLUP_ENABLE | CN6_PULLUP_ENABLE);
temp = mPORTBRead();
//Analog input
CloseADC10();
#define PARAM1 ADC_MODULE_ON | ADC_FORMAT_INTG32 | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON
#define PARAM2 ADC_VREF_AVDD_AVSS | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_2 | ADC_BUF_16 | ADC_ALT_INPUT_OFF
#define PARAM3 ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_5
#define PARAM4 ENABLE_AN0_ANA | ENABLE_AN1_ANA
#define PARAM5 SKIP_SCAN_AN2 | SKIP_SCAN_AN3 | SKIP_SCAN_AN4 | SKIP_SCAN_AN5 | SKIP_SCAN_AN6 | SKIP_SCAN_AN7 | SKIP_SCAN_AN8 | SKIP_SCAN_AN9 | SKIP_SCAN_AN10 | SKIP_SCAN_AN11 | SKIP_SCAN_AN12 | SKIP_SCAN_AN13 | SKIP_SCAN_AN14 | SKIP_SCAN_AN15
SetChanADC10( ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN0);
OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4, PARAM5 );
EnableADC10();
//PORT D for motors
mPORTDClearBits(BIT_0 | BIT_1 | BIT_2 | BIT_3 |
BIT_4 | BIT_5 | BIT_6 | BIT_7 |
BIT_8 | BIT_9 | BIT_10 | BIT_11 |
BIT_12 | BIT_13 | BIT_14 | BIT_15); // Turn off PORTD on startup.
mPORTDSetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2 | BIT_3 |
BIT_4 | BIT_5 | BIT_6 | BIT_7 |
BIT_8 | BIT_9 | BIT_10 | BIT_11 |
BIT_12 | BIT_13 | BIT_14 | BIT_15); // Make PORTD output.
// Explorer-16 uses 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(), DESIRED_BAUDRATE);
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);
// Congifure Change/Notice Interrupt Flag
ConfigIntCN(INTERRUPT);
// configure for multi-vectored mode
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
// enable interrupts
INTEnableInterrupts();
WriteString("*** UART Interrupt-driven Example ***\r\n");
unsigned int channel1, channel2;
unsigned int motor1Time, motor2Time;
// Let interrupt handler do the work
while (1) {
while ( ! mAD1GetIntFlag() );
channel1 = ReadADC10(0);
channel2 = ReadADC10(1);
motor1Time = (channel1*(60000)/(1023) + 80000);
motor2Time = (channel2*(60000)/(1023) + 80000);
//maximo valor de motorTime = 140000
//use motor time for stepping delay
if (MotorsON) {
if (M1forward) {
adelante(1, motor1Time, 1);
} else {
atras(1, motor1Time, 1);
}
if (M2forward) {
adelante(1, motor2Time, 3);
} else {
atras(1, motor2Time, 3);
}
}
mAD1ClearIntFlag();
}
return 0;
}
int main(void)
{
int i, spi_timeout;
unsigned long counter;
/* Disable JTAG port so we get our I/O pins back */
DDPCONbits.JTAGEN = 0;
/* Enable optimal performance */
SYSTEMConfigPerformance(GetSystemClock());
/* Use 1:1 CPU Core:Peripheral clocks */
OSCSetPBDIV(OSC_PB_DIV_1);
/* configure the core timer roll-over rate */
OpenCoreTimer(CORE_TICK_RATE);
/* set up the core timer interrupt */
mConfigIntCoreTimer((CT_INT_ON | CT_INT_PRIOR_6 | CT_INT_SUB_PRIOR_0));
/* enable multi vector interrupts */
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
map_peripherals();
init_io_ports();
configure_pwm();
init_spi();
init_dma();
/* wait until tx buffer is filled up */
while (!SPI2STATbits.SPITBF);
reset_board();
spi_data_ready = 0;
spi_timeout = 0;
counter = 0;
/* enable watchdog */
WDTCONSET = 0x8000;
/* main loop */
while (1) {
if (spi_data_ready) {
spi_data_ready = 0;
/* the first element received is a command string */
switch (rxBuf[0]) {
case 0x5453523E: /* >RST */
reset_board();
break;
case 0x314D433E: /* >CM1 */
stepgen_update_input((const void *)&rxBuf[1]);
stepgen_get_position((void *)&txBuf[1]);
break;
case 0x324D433E: /* >CM2 */
update_outputs(rxBuf[1]);
update_pwm_duty((uint32_t *)&rxBuf[2]);
txBuf[1] = read_inputs();
break;
case 0x4746433E: /* >CFG */
stepgen_update_stepwidth(rxBuf[1]);
update_pwm_period(rxBuf[2]);
stepgen_reset();
break;
case 0x5453543E: /* >TST */
for (i=0; i<BUFSIZE; i++)
txBuf[i] = rxBuf[i] ^ ~0;
break;
}
}
/* if rx buffer is half-full, update the integrity check.
There isn't enough time if we wait for complete transfer */
if (DCH0INTbits.CHDHIF) {
DCH0INTCLR = 1<<4; /* clear flag */
txBuf[0] = rxBuf[0] ^ ~0;
}
/* if rx buffer is full, data from spi bus is ready */
if (DCH0INTbits.CHBCIF) {
DCH0INTCLR = 1<<3; /* clear flag */
spi_data_ready = 1;
spi_timeout = SPI_TIMEOUT;
}
/* reset the board if there is no SPI activity */
if (spi_timeout)
spi_timeout--;
if (spi_timeout == 1) {
DCH0ECONSET=BIT_6; /* abort DMA transfers */
DCH1ECONSET=BIT_6;
init_spi();
init_dma();
reset_board();
/* wait until tx buffer is filled up */
while (!SPI2STATbits.SPITBF);
}
/* blink onboard led */
if (!(counter++ % (spi_timeout ? 0x10000 : 0x40000))) {
LED_TOGGLE;
}
/* keep alive */
WDTCONSET = 0x01;
}
return 0;
}
/* main ***********************************************************************/
int main (void){
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
/* Configure system for maximum performance and enable multi vector interrupts. */
SYSTEMConfig(CO_FSYS*1000, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
/* Disable JTAG and trace port */
DDPCONbits.JTAGEN = 0;
DDPCONbits.TROEN = 0;
/* Verify, if OD structures have proper alignment of initial values */
if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) while(1) ClearWDT();
if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) while(1) ClearWDT();
if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) while(1) ClearWDT();
/* initialize EEPROM - part 1 */
#ifdef USE_EEPROM
CO_ReturnError_t eeStatus = CO_EE_init_1(&CO_EEO, (uint8_t*) &CO_OD_EEPROM, sizeof(CO_OD_EEPROM),
(uint8_t*) &CO_OD_ROM, sizeof(CO_OD_ROM));
#endif
programStart();
/* increase variable each startup. Variable is stored in eeprom. */
OD_powerOnCounter++;
while(reset != CO_RESET_APP){
/* CANopen communication reset - initialize CANopen objects *******************/
CO_ReturnError_t err;
uint16_t timer1msPrevious;
uint16_t TMR_TMR_PREV = 0;
/* disable timer and CAN interrupts */
CO_TMR_ISR_ENABLE = 0;
CO_CAN_ISR_ENABLE = 0;
CO_CAN_ISR2_ENABLE = 0;
/* initialize CANopen */
err = CO_init();
if(err != CO_ERROR_NO){
while(1) ClearWDT();
/* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
}
/* initialize eeprom - part 2 */
#ifdef USE_EEPROM
CO_EE_init_2(&CO_EEO, eeStatus, CO->SDO, CO->em);
#endif
/* initialize variables */
timer1msPrevious = CO_timer1ms;
OD_performance[ODA_performance_mainCycleMaxTime] = 0;
OD_performance[ODA_performance_timerCycleMaxTime] = 0;
reset = CO_RESET_NOT;
/* Configure Timer interrupt function for execution every 1 millisecond */
CO_TMR_CON = 0;
CO_TMR_TMR = 0;
#if CO_PBCLK > 65000
#error wrong timer configuration
#endif
CO_TMR_PR = CO_PBCLK - 1; /* Period register */
CO_TMR_CON = 0x8000; /* start timer (TON=1) */
CO_TMR_ISR_FLAG = 0; /* clear interrupt flag */
CO_TMR_ISR_PRIORITY = 3; /* interrupt - set lower priority than CAN (set the same value in interrupt) */
/* Configure CAN1 Interrupt (Combined) */
CO_CAN_ISR_FLAG = 0; /* CAN1 Interrupt - Clear flag */
CO_CAN_ISR_PRIORITY = 5; /* CAN1 Interrupt - Set higher priority than timer (set the same value in '#define CO_CAN_ISR_PRIORITY') */
CO_CAN_ISR2_FLAG = 0; /* CAN2 Interrupt - Clear flag */
CO_CAN_ISR2_PRIORITY = 5; /* CAN Interrupt - Set higher priority than timer (set the same value in '#define CO_CAN_ISR_PRIORITY') */
communicationReset();
/* start CAN and enable interrupts */
CO_CANsetNormalMode(ADDR_CAN1);
CO_TMR_ISR_ENABLE = 1;
CO_CAN_ISR_ENABLE = 1;
#if CO_NO_CAN_MODULES >= 2
CO_CANsetNormalMode(ADDR_CAN2);
CO_CAN_ISR2_ENABLE = 1;
#endif
while(reset == CO_RESET_NOT){
/* loop for normal program execution ******************************************/
uint16_t timer1msCopy, timer1msDiff;
ClearWDT();
/* calculate cycle time for performance measurement */
timer1msCopy = CO_timer1ms;
timer1msDiff = timer1msCopy - timer1msPrevious;
timer1msPrevious = timer1msCopy;
uint16_t t0 = CO_TMR_TMR;
uint16_t t = t0;
if(t >= TMR_TMR_PREV){
t = t - TMR_TMR_PREV;
t = (timer1msDiff * 100) + (t / (CO_PBCLK / 100));
}
else if(timer1msDiff){
t = TMR_TMR_PREV - t;
t = (timer1msDiff * 100) - (t / (CO_PBCLK / 100));
}
else t = 0;
OD_performance[ODA_performance_mainCycleTime] = t;
if(t > OD_performance[ODA_performance_mainCycleMaxTime])
OD_performance[ODA_performance_mainCycleMaxTime] = t;
TMR_TMR_PREV = t0;
/* Application asynchronous program */
programAsync(timer1msDiff);
ClearWDT();
/* CANopen process */
reset = CO_process(CO, timer1msDiff);
ClearWDT();
#ifdef USE_EEPROM
CO_EE_process(&CO_EEO);
#endif
}
}
/* program exit ***************************************************************/
CO_DISABLE_INTERRUPTS();
/* delete objects from memory */
programEnd();
CO_delete();
/* reset */
SoftReset();
}
//===========================
// START INTERRUPTS
//===========================
void StartInterrupts(void)
{
INT8 err;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable timer interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Timer.EnableInterrupt(TIMER_1);
// Timer.EnableInterrupt(TIMER_2);
// Timer.EnableInterrupt(TIMER_3);
Timer.EnableInterrupt(TIMER_4);
// Timer.EnableInterrupt(TIMER_5);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable SPI interrupts // Not functionnal yet
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Spi.EnableRxInterrupts(SPI4); // Enable RX Interrupts for SPI4
// Spi.EnableTxInterrupts(SPI4); // Enable TX Interrupts for SPI4
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable CAN interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Can.EnableInterrupt(CAN1);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable I2C interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// err = I2c.EnableInterrupt (I2C4, I2C_MASTER_INTERRUPT);
// if (err < 0)
// {
// LED_ERROR_ON;
// }
// err = I2c.DisableInterrupt(I2C4, I2C_SLAVE_INTERRUPT);
// if (err < 0)
// {
// LED_ERROR_ON;
// }
// err = I2c.DisableInterrupt(I2C4, I2C_BUS_COLLISION_INTERRUPT);
// if (err < 0)
// {
// LED_ERROR_ON;
// }
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable ADC interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Adc.EnableInterrupts (); // Enable Interrupts for ADC
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable UART interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (SEND_DATA_TO_UART)
{
Uart.EnableRxInterrupts (UART6); // Enable RX Interrupts for UART6
Uart.DisableTxInterrupts(UART6); // Disable TX Interrupts for UART6
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Enable multi-vector interrupts
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
}
// Perform startup routines
void startupPIC(void)
{
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableSystemMultiVectoredInt();
}
int main(void) {
timesec=0;
// set PIC32 to max computing power
SYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableSystemMultiVectoredInt();
initLEDs();
LED0 = 1;
LED1 = 1;
initSerialNU32v2();
setup_counters();
CloseADC10();
#define PARAM1 ADC_MODULE_ON | ADC_FORMAT_INTG | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON
#define PARAM2 ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_16 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF
#define PARAM3 ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_31
#define PARAM4 ENABLE_AN0_ANA | ENABLE_AN1_ANA | ENABLE_AN2_ANA | ENABLE_AN3_ANA | ENABLE_AN5_ANA | ENABLE_AN15_ANA
OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4,0);
EnableADC10();
// Setup and turn off electromagnets
EMAG1 = 0; EMAG2 = 0;
TRISEbits.TRISE7 = 0; TRISCbits.TRISC1 = 0;
//Direction Output
DIR = 1;
TRISAbits.TRISA9 = 0;
//g-select Outputs
GSEL1 = 0; GSEL2 = 0;
TRISEbits.TRISE2 = 0; TRISCbits.TRISC13= 0;
//0g Inputs
TRISAbits.TRISA0 = 1;
TRISAbits.TRISA4 = 1;
// 20kHz PWM signal, duty from 0-1000, pin D3
OpenTimer2(T2_ON | T2_PS_1_4, 1000);
OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
HBridgeDuty = 0;
SetDCOC4PWM(HBridgeDuty);
// 20Hz ISR
OpenTimer3(T3_ON | T3_PS_1_256, 15625);
mT3SetIntPriority(1);
mT3ClearIntFlag();
mT3IntEnable(1);
while(1) {
if(start){
EMAG2=0;
SetDCOC4PWM(100);
delaysec(delay1);
SetDCOC4PWM(1000);
delaysec(delay2);
SetDCOC4PWM(500);
delaysec(delay3);
EMAG2=1;
SetDCOC4PWM(0);
// EMAG1=0;
// SetDCOC4PWM(900);
// DIR = 0;
// delaysec(delay1);
// SetDCOC4PWM(0);
// delaysec(delay2);
// SetDCOC4PWM(700);
// delaysec(delay1);
// SetDCOC4PWM(1000);
// EMAG1=1;
start=0;
}
}
}
void InitializeSystem() {
#ifdef BOARD_UBW32
// Disable ADC port (allows PORTB to be used for digital I/O)
AD1PCFG = 0xFFFF;
TRISE = 0x0000;
TRISB = 0x0000;
TRISC = 0x0000;
TRISD = 0x0000;
LATE = 0x0000;
LATB = 0x0000;
LATC = 0x0000;
LATD = 0x0000;
#endif
#ifdef BOARD_HEXLIGHT
ANSELA = 0x0000;
ANSELB = 0x0000;
#endif
LATA = 0x0000;
LATB = 0x0000;
// Ensure LED drivers are driven low as soon as possible
// _TRIS(PIO_OC1) = 0;
// _TRIS(PIO_OC2) = 0;
// _TRIS(PIO_OC3) = 0;
// _TRIS(PIO_OC4) = 0;
// _LAT(PIO_OC1) = OUTPUT;
// _LAT(PIO_OC2) = OUTPUT;
// _LAT(PIO_OC3) = OUTPUT;
// _LAT(PIO_OC4) = OUTPUT;
// Force disconnect of USB bootloader
U1CON = 0x00000000;
U1PWRC = 0x00000000;
// LEDs
// _TRIS(PIO_LED1) = OUTPUT;
// _TRIS(PIO_LED2) = OUTPUT;
#ifdef BOARD_UBW32
_TRIS(PIO_LED3) = OUTPUT;
_TRIS(PIO_LED_USB) = OUTPUT;
_TRIS(PIO_BTN_PGM) = 1;
_TRIS(PIO_BTN_USR) = 1;
#elif BOARD_HEXLIGHT
_TRIS(PIO_BTN1) = INPUT;
_TRIS(PIO_BTN2) = INPUT;
#endif
// _TRIS(PIO_USBP) = INPUT;
// _TRIS(PIO_USBN) = INPUT;
// _LAT(PIO_LED1) = LOW;
// _LAT(PIO_LED2) = LOW;
#ifdef BOARD_UBW32
_LAT(PIO_LED3) = HIGH;
_LAT(PIO_LED_USB) = LOW;
#endif
mJTAGPortEnable(0);
// Initializethe PIC32 core
//OSCConfig(OSC_POSC_PLL, OSC_PLL_MULT_20, OSC_PLL_POST_2, OSC_FRC_POST_2);
sys_clock = F_SYSCLK;
mOSCSetPBDIV(OSC_PB_DIV_1);
pb_clock = SYSTEMConfig(sys_clock, SYS_CFG_ALL);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
// Initialize core time base
SystickInit();
}
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 InitApp(void)
{
// PORT A is all f****d because of JTAG. Input only on A0 and A1 it seems...
// Disable JTAG so we can use A4 and A5 for gpio (and maybe A0-A3 too?)
DDPCONbits.JTAGEN = 0;
// Set GPIO pin functionality:
// ===========================
// Note: how to set pins:
// TRIS*CLR -> clears a pin/bit in the * port
// TRIS*SET -> sets a pin/bit in the * port
// TRIS*INV -> inverts a pin/bit in the * port
// Note: UBW32 pin meaning:
// Set the LED's on the UBW32 as outputs:
// RE0 -> yellow
// RE1 -> red
// RE2 -> white
// RE3 -> green
//
// RE6 -> PRG button
// RE7 -> USER button
// Cleared pins (set to zero) are configured as outputs.
// This is the hard way to clear pins:
// TRISECLR = 0x0F;
// And this is the easy way:
_TRISE0 = 0;
_TRISE1 = 0;
_TRISE2 = 0;
_TRISE3 = 0;
// Set the PRG and USER buttons on the UBW32 as an inputs:
// This is the hard way:
// TRISESET = 0x00C0; // This sets pin RE7 and RE6: 1100 0000 = C0
// This is the easy way:
_TRISE6 = 1; // Set pins, set to one, are configured as inputs
_TRISE7 = 1;
// Set RE9 as an output to test the 44.1kHz interupt timing:
_TRISE9 = 0;
// Set pin RD8 as an output, could be written as TRISD = 0xFEFF;
// but that takes more clock cycles to perform:
// TRISDCLR = 0x0100;
// PWM Intialization:
// ==================
// configure RD0 as output for PWM
// PWM mode, Single output, Active High
// TRISDCLR = 0x00000001;
// OC1R = 0;
// OC1CON = 0x0000;
// OC1RS = 0;
// OC1CON = 0x0006;
// OC1CONSET = 0x8000;
// Set up some interupts:
// ======================
OpenCoreTimer( 0xFFFFFFFF ); // This is needed for the delay functions
// The next line: turn on timer2 | have it use an internal clock source | have it
// use a prescaler of 1:256, and use a period of 0xFFFF or 2^16 cycles
//
// This fires an interrupt at a frequency of (80MHZ/256/65535), or 4.77
// times a second.
// OpenTimer2( T2_ON | T2_SOURCE_INT | T2_PS_1_256, 0xFFFF);
// Set Timer2 to fire at 44.1kHz:
// (80MHz/1/44100) = 1814.059 = 1814 = 0x0716 with a prescaler of 1:1
OpenTimer2( T2_ON | T2_SOURCE_INT | T2_PS_1_1, 0x0716);
/*Configure Multivector Interrupt Mode. Using Single Vector Mode
is expensive from a timing perspective, so most applications
should probably not use a Single Vector Mode*/
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
// The next line seems to provide the exact same functionality...
INTEnableSystemMultiVectoredInt();// Use multi-vectored interrupts:
// This statement configured the timer to produce an interrupt with a priority of 2
ConfigIntTimer2( T2_INT_ON | T2_INT_PRIOR_2);
}
