Created
February 12, 2018 22:32
-
-
Save ricklon/814221cfa38c5f8114d7c20a33b410bb to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* Sample RC servo input for 4 channels using Input Capture hardware perihperal and interrupts | |
* Written for Rick Anderson by Brian Schmalz | |
* 11/11/2017 | |
* This software is put in the public domain. | |
* This sketch is means for a chipKIT Fubarino SD board, but can be easily adpated to any PIC32 based Arduino compatible board. | |
*/ | |
/* Fubarino SD can only use pins 0, 1, 2, 3, and 10 as Input Capture. | |
* On any chipKIT board with PPS (like Fubarino Mini) additional setup is necessary to map the Input Capture | |
* peripherals to speciffic I/O pins, but you have much more flexibility in pin assignments. | |
*/ | |
// Which pins correspond to which RC input channel? | |
#define RC_INPUT_1 0 | |
#define RC_INPUT_2 1 | |
#define RC_INPUT_3 2 | |
#define RC_INPUT_4 3 | |
// How many RC input channels are there? | |
#define RC_INPUT_COUNT 4 | |
// These two arrays hold the latest pulse measurements for each RC input channel | |
volatile uint16_t pulseHighTime[RC_INPUT_COUNT]; | |
volatile uint16_t pulseLowTime[RC_INPUT_COUNT]; | |
/* Four Input Capture Interrupt Service Routines | |
* These functions get called on each rising and falling edge. They check to see | |
* if a rising edge or falling edge just happened, then read out the Timer2 value that | |
* got latched on the edge, and calculate the high and low pulse times, then stick those | |
* times in the proper array. | |
*/ | |
void __USER_ISR InputCatpure1_ISR(void) { | |
static uint16_t risingEdgeTime = 0; | |
static uint16_t fallingEdgeTime = 0; | |
clearIntFlag(_INPUT_CAPTURE_1_IRQ); | |
if (IC1CONbits.ICBNE == 1) | |
{ | |
if (digitalRead(RC_INPUT_1) == HIGH) | |
{ | |
risingEdgeTime = IC1BUF; | |
pulseLowTime[0] = risingEdgeTime - fallingEdgeTime; | |
} | |
else | |
{ | |
fallingEdgeTime = IC1BUF; | |
pulseHighTime[0] = fallingEdgeTime - risingEdgeTime; | |
} | |
} | |
} | |
void __USER_ISR InputCatpure2_ISR(void) { | |
static uint16_t risingEdgeTime = 0; | |
static uint16_t fallingEdgeTime = 0; | |
clearIntFlag(_INPUT_CAPTURE_2_IRQ); | |
if (IC2CONbits.ICBNE == 1) | |
{ | |
if (digitalRead(RC_INPUT_2) == HIGH) | |
{ | |
risingEdgeTime = IC2BUF; | |
pulseLowTime[1] = risingEdgeTime - fallingEdgeTime; | |
} | |
else | |
{ | |
fallingEdgeTime = IC2BUF; | |
pulseHighTime[1] = fallingEdgeTime - risingEdgeTime; | |
} | |
} | |
} | |
void __USER_ISR InputCatpure3_ISR(void) { | |
static uint16_t risingEdgeTime = 0; | |
static uint16_t fallingEdgeTime = 0; | |
clearIntFlag(_INPUT_CAPTURE_3_IRQ); | |
if (IC3CONbits.ICBNE == 1) | |
{ | |
if (digitalRead(RC_INPUT_3) == HIGH) | |
{ | |
risingEdgeTime = IC3BUF; | |
pulseLowTime[2] = risingEdgeTime - fallingEdgeTime; | |
} | |
else | |
{ | |
fallingEdgeTime = IC3BUF; | |
pulseHighTime[2] = fallingEdgeTime - risingEdgeTime; | |
} | |
} | |
} | |
void __USER_ISR InputCatpure4_ISR(void) { | |
static uint16_t risingEdgeTime = 0; | |
static uint16_t fallingEdgeTime = 0; | |
clearIntFlag(_INPUT_CAPTURE_4_IRQ); | |
if (IC4CONbits.ICBNE == 1) | |
{ | |
if (digitalRead(RC_INPUT_4) == HIGH) | |
{ | |
risingEdgeTime = IC4BUF; | |
pulseLowTime[3] = risingEdgeTime - fallingEdgeTime; | |
} | |
else | |
{ | |
fallingEdgeTime = IC4BUF; | |
pulseHighTime[3] = fallingEdgeTime - risingEdgeTime; | |
} | |
} | |
} | |
void setup() { | |
Serial.begin(9600); | |
delay(3000); | |
Serial.println("OK we are starting now"); | |
/* Set up each of the four Input Capture modules | |
* We want them to generate an interrupt on each rising and falling edge of the input | |
* We also want them all to use Timer2 as their timebase timer | |
*/ | |
IC1CON = 0; | |
IC1CONbits.ICM = 1; // Captured and interrupt on every rising and falling edge | |
IC1CONbits.ICTMR = 1; // Set to user Timer2 | |
IC1CONbits.ON = 1; // Turn IC1 on | |
IC2CON = 0; | |
IC2CONbits.ICM = 1; // Captured and interrupt on every rising and falling edge | |
IC2CONbits.ICTMR = 1; // Set to user Timer2 | |
IC2CONbits.ON = 1; // Turn IC2 on | |
IC3CON = 0; | |
IC3CONbits.ICM = 1; // Captured and interrupt on every rising and falling edge | |
IC3CONbits.ICTMR = 1; // Set to user Timer2 | |
IC3CONbits.ON = 1; // Turn IC3 on | |
IC4CON = 0; | |
IC4CONbits.ICM = 1; // Captured and interrupt on every rising and falling edge | |
IC4CONbits.ICTMR = 1; // Set to user Timer2 | |
IC4CONbits.ON = 1; // Turn IC4 on | |
/* Set up Timer2: We want it to simply count up. We set the prescaler to 1:64 | |
* so that the 40MHz PCLK gets divided down to 1.25Mhz. This is nice because | |
* then it gives us both high and low periods under 65535 for normal RC servo | |
* times (0-3 ms high time and 17-20ms low time) | |
*/ | |
PR2 = 0xFFFF; // Run from 0 to 0xFFFF | |
T2CONbits.TCKPS = 5; // 1:32 prescale, which means 40MHz/64 or 1.25MHz clock rate | |
T2CONbits.TON = 1; // Turn on Timer2 | |
// Set all input captures as inputs | |
pinMode(RC_INPUT_1, INPUT); | |
pinMode(RC_INPUT_2, INPUT); | |
pinMode(RC_INPUT_3, INPUT); | |
pinMode(RC_INPUT_4, INPUT); | |
Serial.println("ready"); | |
// Set each Input Capture up to use its ISR routine | |
setIntVector(_INPUT_CAPTURE_1_VECTOR, InputCatpure1_ISR); | |
setIntPriority(_INPUT_CAPTURE_1_VECTOR, 4, 0); | |
clearIntFlag(_INPUT_CAPTURE_1_IRQ); | |
setIntEnable(_INPUT_CAPTURE_1_IRQ); | |
setIntVector(_INPUT_CAPTURE_2_VECTOR, InputCatpure2_ISR); | |
setIntPriority(_INPUT_CAPTURE_2_VECTOR, 4, 0); | |
clearIntFlag(_INPUT_CAPTURE_2_IRQ); | |
setIntEnable(_INPUT_CAPTURE_2_IRQ); | |
setIntVector(_INPUT_CAPTURE_3_VECTOR, InputCatpure3_ISR); | |
setIntPriority(_INPUT_CAPTURE_3_VECTOR, 4, 0); | |
clearIntFlag(_INPUT_CAPTURE_3_IRQ); | |
setIntEnable(_INPUT_CAPTURE_3_IRQ); | |
setIntVector(_INPUT_CAPTURE_4_VECTOR, InputCatpure4_ISR); | |
setIntPriority(_INPUT_CAPTURE_4_VECTOR, 4, 0); | |
clearIntFlag(_INPUT_CAPTURE_4_IRQ); | |
setIntEnable(_INPUT_CAPTURE_4_IRQ); | |
} | |
void loop() { | |
static uint32_t lastPrintTime = 0; | |
float uSscale = 0.4; | |
/* So the way things work, the ISRs will get called and they will constantly update | |
* the pulseHighTime and pulseLowTime arrays "in the background". In other words the | |
* mainline code here doesn't ever have to do anything other than simply read out | |
* the values in the arrays, and those values will always be the most recent pulse | |
* measurements. If you care about converting the values to real time (like in | |
* microsconds), the units for these values are in 1.25MHz 'counts'. So 1ms = 1250 | |
* 'counts'. | |
*/ | |
// To demo things, simply print out all 4 high and low pulse times every half second | |
if ((millis() - lastPrintTime) > 500) | |
{ | |
lastPrintTime = millis(); | |
char pulses[100]; | |
char micros[100]; | |
sprintf(pulses, "high1: %05u low1: %05u high2: %05u low2: %05u high3: %05u low3: %05u high4: %05u low4: %05u\n", | |
pulseHighTime[0], | |
pulseLowTime[0], | |
pulseHighTime[1], | |
pulseLowTime[1], | |
pulseHighTime[2], | |
pulseLowTime[2], | |
pulseHighTime[3], | |
pulseLowTime[3] | |
); | |
Serial.print(pulses); | |
sprintf(micros, "ch1: %05u ch2: %05u ch3: %05u ch4: %05u\n", | |
(int) (pulseHighTime[0] * uSscale), | |
(int) (pulseHighTime[1] * uSscale), | |
(int) (pulseHighTime[2] * uSscale), | |
(int) (pulseHighTime[3] * uSscale) | |
); | |
Serial.print(micros); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment