////////////////////////////////////////////////////////////////////////
// Functions
//---------------------------------------------------------------------
//IC1Setup -- This function configures the Input Capture 1 register. During
//execution of the code, the setup for each IC will change depending on the current
//state.
void IC1_Config(void)
{
#if DEBUG>0
printf("Loading Interrupt Capture...");
#endif
//Enable IC1, Interrupt on, Pri to 6
ConfigIntCapture1(IC_INT_PRIOR_6 & IC_INT_ON);
ConfigIntCapture3(IC_INT_PRIOR_6 & IC_INT_ON);
//Configuration options
//----Idle while in stop
//----Use Timer 3 as timing source
//----Interrupt on the first capture
//----Capture on every edge
OpenCapture1(IC_IDLE_STOP & IC_TIMER3_SRC & IC_INT_1CAPTURE & IC_EVERY_EDGE);
OpenCapture3(IC_IDLE_STOP & IC_TIMER3_SRC & IC_INT_1CAPTURE & IC_EVERY_FALL_EDGE);
#if DEBUG>0
printf("DONE\r\n");
#endif
return;
}
/*! **********************************************************************
* Function: configureRange(void)
*
* Include: Range.h
*
* Description: Configures the Range module
*
* Arguments: None
*
* Returns: None
*************************************************************************/
void configureRange(void)
{
unsigned char config;
INIT_PIN = 0;
//Enable global interrupts and interrupt priority
INT_SETUP()
readTemp();
//Make sure the AD is configured
configureAD();
CCP1_INPT = 1;
INIT_TRIS = 0; //Make the INIT
//Open Timer
config = T1_16BIT_RW & T1_SOURCE_INT & T1_OSC1EN_OFF & T1_PS_1_1 & T1_SYNC_EXT_OFF &TIMER_INT_ON;
OpenTimer1(config);
config = CAPTURE_INT_ON & CAP_EVERY_RISE_EDGE;
//CloseCapture1, which will clear any interrupt flags etc
CloseCapture1();
//Open the input capture on compare1
OpenCapture1(config);
}
/*
* ir_timeout : this function is called when timer1 overflows.
*/
void
ir_timeout(void)
{
#if IR_RAW_SUPPORT == 1
ir_raw[ir_ptr] = 0xff;
#endif
ir_flags.timeout = 1;
/* stop timer 1 */
T1CONbits.TMR1ON = 0;
/* decode what we got ! */
ir_decode();
if (ir_flags.decoded) {
/* wait for the caller to re-enable reception */
ir_disable();
} else {
Nop();
}
/* reset capture edge */
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_FALL_EDGE);
ir_flags.edge = 0;
ir_reset();
}
void init_core(void)
{
// === Timer und Capture/Compare-Einheit ===
// 10 MHz Oszillatortakt -> 40 MHz Systemtakt -> 10 MHz Instruktionstakt
// 10 MHz Instruktionstakt / 16 Prescaler -> 1,6 µs Timertaktlänge, daher theoretisch alle 65536
// Timertakte = 104,8ms ein Timerüberlauf
OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_16);
// 10 MHz Instruktionstakt / 8 Prescaler -> 0,8 µs Timertaktlänge
OpenTimer1(T1_16BIT_RW & TIMER_INT_OFF & T1_PS_1_8 & T1_SYNC_EXT_ON & T1_SOURCE_CCP & T1_SOURCE_INT);
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_FALL_EDGE);
// TxQ.Head = TxQ.Tail = RxCheckSum = 0;
INTCONbits.PEIE = true; // Wofür ist das?
// INTCONbits.TMR0IE = true;
// === Fernbedienungsdecoder ===
raw_rc_data[0] = 0xDEADBEEF;
current_rc_frame_ok = false;
new_rc_values_available = false;
}
int main(void)
{
uint16_t i;
FRESULT rc;
map_io();
init_port();
InitRTCC();
uart2_init();
xdev_out(uart2_put);
xdev_in(uart2_get);
dbg_printf("$" PROJECT_NAME "\n");
dbg_printf("$" __DATE__ " " __TIME__ "\n");
rc = f_mount(&fatfs, "", 1);
dbg_printf("$FF,f_mount,%s\n", get_rc(rc));
OpenTimer1(T1_PS_1_256 & T1_GATE_OFF & T1_SOURCE_INT & T1_IDLE_CON &
T1_ON & T1_SYNC_EXT_OFF, 0xFFFF);
ConfigIntTimer1(T1_INT_ON & T1_INT_PRIOR_1);
OpenCapture1(IC_IDLE_STOP & IC_TIMER1_SRC & IC_INT_1CAPTURE & IC_EVERY_RISE_EDGE,
IC_CASCADE_DISABLE & IC_TRIGGER_ENABLE & IC_UNTRIGGER_TIMER & IC_SYNC_TRIG_IN_DISABLE);
ConfigIntCapture1(IC_INT_ON & IC_INT_PRIOR_5);
_IC1IF = 0;
while (1) {
while (_RTCSYNC == 0);
while (_RTCSYNC == 1);
if (gps_pr > 0) {
_T1IE = 0;
float f = (float) TMR1 / gps_pr;
_T1IE = 1;
xprintf("%u\n", (uint16_t) (f * 1000));
}
if (ngpslines > 0) {
ngpslines--;
if (xgets(gps_line, 128)) {
xprintf("$GPS%s\n", gps_line);
}
}
}
while (0) {
while (_RTCSYNC == 0);
while (_RTCSYNC == 1);
if (gps_pr > 0) {
_T1IE = 0;
float f = (float) TMR1 / gps_pr;
_T1IE = 1;
xprintf("%u\n", (uint16_t) (f * 1000));
}
}
return (EXIT_SUCCESS);
}
/*
* ir_interrupt : this function must be called when the capture interrupt
* occurs.
*/
void
ir_interrupt(void)
{
/* reload timer 1 */
TMR1H = T1_RELOAD_H;
TMR1L = T1_RELOAD_L;
/* change edge */
if (ir_flags.edge) {
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_FALL_EDGE);
ir_flags.edge = 0;
} else {
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_RISE_EDGE);
ir_flags.edge = 1;
}
if (ir_flags.first) {
/* This is the first edge. Start timer1 */
T1CONbits.TMR1ON = 1;
#if IR_RAW_SUPPORT == 1
ir_ptr = 0;
#endif
ir_decode();
ir_flags.first = 0;
return;
}
#if IR_RAW_SUPPORT == 1
ir_raw[ir_ptr] = CCPR1H; // ir_raw in multiple of 64us
ir_ptr++;
if (ir_ptr == IR_RAW_SIZE - 1) {
ir_ptr = 0;
//ir_timeout();
}
#endif
ir_decode();
}
//===================== Main ======================= //
void main(void) {
SYSTEMConfig( SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
ANSELA = 0; ANSELB = 0; CM1CON = 0; CM2CON = 0;
PT_setup();
INTEnableSystemMultiVectoredInt();
// initialize the threads
PT_INIT(&pt_blink);
PT_INIT(&pt_capture);
// initialize the display
tft_init_hw();
tft_begin();
tft_fillScreen(ILI9340_BLACK);
tft_setRotation(0); //240x320 vertical display
// initialize the comparator
CMP1Open(CMP_ENABLE | CMP_OUTPUT_ENABLE | CMP1_NEG_INPUT_IVREF);
// initialize the timer2
OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_64, 0xffffffff);
// initialize the input capture, uses timer2
OpenCapture1( IC_EVERY_RISE_EDGE | IC_FEDGE_RISE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON);
ConfigIntCapture1(IC_INT_ON | IC_INT_PRIOR_3 | IC_INT_SUB_PRIOR_3 );
INTClearFlag(INT_IC1);
// initialize the input/output I/O
mPORTBSetPinsDigitalOut(BIT_3);
mPORTBClearBits(BIT_3);
PPSOutput(4, RPB9, C1OUT); //set up output of comparator for debugging
PPSInput(3, IC1, RPB13); //Either Pin 6 or Pin 24 idk
//round-robin scheduler for threads
while(1) {
PT_SCHEDULE(protothread_blink(&pt_blink));
PT_SCHEDULE(protothread_capture(&pt_capture));
}
} //main
/*
* ir_init : this function must be called to initialize the library.
*/
void
ir_init(void)
{
/* configure timer 1 */
OpenTimer1(TIMER_INT_ON & T1_16BIT_RW & T1_SOURCE_INT &
T1_PS_1_1 & T1_SOURCE_CCP);
T1CONbits.TMR1ON = 0;
TMR1H = T1_RELOAD_H;
TMR1L = T1_RELOAD_L;
/* configure capture unit */
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_FALL_EDGE);
ir_flags.edge = 0;
/* configure interrupts */
PIE1bits.TMR1IE = 1;
INTCONbits.PEIE = 1;
/* init data */
ir_reset();
}
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 main(void)
{
int8 i;
DisableInterrupts;
InitPorts();
InitADC();
OpenUSART(USART_TX_INT_OFF&USART_RX_INT_OFF&USART_ASYNCH_MODE&
USART_EIGHT_BIT&USART_CONT_RX&USART_BRGH_HIGH, _B38400);
OpenTimer0(TIMER_INT_OFF&T0_8BIT&T0_SOURCE_INT&T0_PS_1_16);
OpenTimer1(T1_8BIT_RW&TIMER_INT_OFF&T1_PS_1_8&T1_SYNC_EXT_ON&T1_SOURCE_CCP&T1_SOURCE_INT);
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_FALL_EDGE); // capture mode every falling edge
CCP1CONbits.CCP1M0 = NegativePPM;
OpenTimer2(TIMER_INT_ON&T2_PS_1_16&T2_POST_1_16);
PR2 = TMR2_5MS; // set compare reg to 9ms
INTCONbits.TMR0IE = false;
// setup flags register
for ( i = 0; i<16; i++ )
Flags[i] = false;
_NoSignal = true;
InitArrays();
ReadParametersEE();
ConfigParam = 0;
ALL_LEDS_OFF;
Beeper_OFF;
LedBlue_ON;
INTCONbits.PEIE = true; // enable peripheral interrupts
EnableInterrupts;
LedRed_ON; // red LED on
Delay1mS(100);
IsLISLactive();
#ifdef ICD2_DEBUG
_UseLISL = 1; // because debugger uses RB7 (=LISL-CS) :-(
#endif
NewK1 = NewK2 = NewK3 = NewK4 =NewK5 = NewK6 = NewK7 = 0xFFFF;
PauseTime = 0;
InitBarometer();
InitDirection();
Delay1mS(COMPASS_TIME);
// send hello text to serial COM
Delay1mS(100);
ShowSetup(true);
Delay1mS(BARO_PRESS_TIME);
while(1)
{
// turn red LED on of signal missing or invalid, green if OK
// Yellow led to indicate linear sensor functioning.
if( _NoSignal || !Switch )
{
LedRed_ON;
LedGreen_OFF;
if ( _UseLISL )
LedYellow_ON;
}
else
{
LedGreen_ON;
LedRed_OFF;
LedYellow_OFF;
}
ProcessComCommand();
}
} // main
void updateEdge(void) {
if (capture.edge)
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_RISE_EDGE);
else
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_FALL_EDGE);
}
