lots of formatting fixes
It should still compile, but I can't check.
diff --git a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c b/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
index 2920584..394ade0 100644
--- a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
+++ b/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
@@ -1,28 +1,28 @@
/*
- LPCUSB, an USB device driver for LPC microcontrollers
- Copyright (C) 2006 Bertrik Sikken (bertrik@sikken.nl)
+ LPCUSB, an USB device driver for LPC microcontrollers
+ Copyright (C) 2006 Bertrik Sikken (bertrik@sikken.nl)
- Redistribution and use in source and binary forms, with or without
- modification, are permitted provided that the following conditions are met:
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
- 1. Redistributions of source code must retain the above copyright
- notice, this list of conditions and the following disclaimer.
- 2. Redistributions in binary form must reproduce the above copyright
- notice, this list of conditions and the following disclaimer in the
- documentation and/or other materials provided with the distribution.
- 3. The name of the author may not be used to endorse or promote products
- derived from this software without specific prior written permission.
+ 1. Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ 2. Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ 3. The name of the author may not be used to endorse or promote products
+ derived from this software without specific prior written permission.
- THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
- IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
- OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
- IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
- INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
- NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
- THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "FreeRTOS.h"
@@ -38,106 +38,106 @@
#include "LPC17xx.h"
-#define usbMAX_SEND_BLOCK ( 20 / portTICK_RATE_MS )
-#define usbRXBUFFER_LEN ( 80 )
-#define usbTXBUFFER_LEN ( 600 )
+#define usbMAX_SEND_BLOCK ( 20 / portTICK_RATE_MS )
+#define usbRXBUFFER_LEN ( 80 )
+#define usbTXBUFFER_LEN ( 600 )
-#define INT_IN_EP 0x81 //read manual for picking these...
-#define INT_OUT_EP 0x04
-#define BULK_IN_EP 0x82
-#define BULK_OUT_EP 0x05
+#define INT_IN_EP 0x81 //read manual for picking these...
+#define INT_OUT_EP 0x04
+#define BULK_IN_EP 0x82
+#define BULK_OUT_EP 0x05
-#define MAX_PACKET_SIZE 64
+#define MAX_PACKET_SIZE 64
-#define LE_WORD(x) ((x)&0xFF),((x)>>8)
+#define LE_WORD(x) ((x)&0xFF),((x)>>8)
static xQueueHandle xRxedChars = NULL, xCharsForTx = NULL;
static const unsigned char abDescriptors[] = {
// Device descriptor
- 0x12,
- DESC_DEVICE,
- LE_WORD(0x0200), // bcdUSB
- 0xFF, // bDeviceClass
- 0x00, // bDeviceSubClass
- 0x00, // bDeviceProtocol
- MAX_PACKET_SIZE0, // bMaxPacketSize
- LE_WORD(0x1424), // idVendor
- LE_WORD(0xd243), // idProduct
- LE_WORD(0x0153), // bcdDevice
- 0x03, // iManufacturer
- 0x02, // iProduct
- 0x01, // iSerialNumber
- 0x01, // bNumConfigurations
+ 0x12,
+ DESC_DEVICE,
+ LE_WORD(0x0200), // bcdUSB
+ 0xFF, // bDeviceClass
+ 0x00, // bDeviceSubClass
+ 0x00, // bDeviceProtocol
+ MAX_PACKET_SIZE0, // bMaxPacketSize
+ LE_WORD(0x1424), // idVendor
+ LE_WORD(0xd243), // idProduct
+ LE_WORD(0x0153), // bcdDevice
+ 0x03, // iManufacturer
+ 0x02, // iProduct
+ 0x01, // iSerialNumber
+ 0x01, // bNumConfigurations
// Configuration descriptor
- 0x09,
- DESC_CONFIGURATION,
- LE_WORD(46), // wTotalLength
- 0x01, // bNumInterfaces
- 0x01, // bConfigurationValue
- 0x00, // iConfiguration
- 0xC0, // bmAttributes
- 0x32, // bMaxPower
+ 0x09,
+ DESC_CONFIGURATION,
+ LE_WORD(46), // wTotalLength
+ 0x01, // bNumInterfaces
+ 0x01, // bConfigurationValue
+ 0x00, // iConfiguration
+ 0xC0, // bmAttributes
+ 0x32, // bMaxPower
// Data class interface descriptor
- 0x09,
- DESC_INTERFACE,
- 0x00, // bInterfaceNumber
- 0x00, // bAlternateSetting
- 0x04, // bNumEndPoints
- 0x0A, // bInterfaceClass = data
- 0x00, // bInterfaceSubClass
- 0x00, // bInterfaceProtocol
- 0x00, // iInterface
+ 0x09,
+ DESC_INTERFACE,
+ 0x00, // bInterfaceNumber
+ 0x00, // bAlternateSetting
+ 0x04, // bNumEndPoints
+ 0x0A, // bInterfaceClass = data
+ 0x00, // bInterfaceSubClass
+ 0x00, // bInterfaceProtocol
+ 0x00, // iInterface
// Debug EP OUT
- 0x07,
- DESC_ENDPOINT,
- BULK_OUT_EP, // bEndpointAddress
- 0x02, // bmAttributes = bulk
- LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
- 0x00, // bInterval
+ 0x07,
+ DESC_ENDPOINT,
+ BULK_OUT_EP, // bEndpointAddress
+ 0x02, // bmAttributes = bulk
+ LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
+ 0x00, // bInterval
// Debug EP in
- 0x07,
- DESC_ENDPOINT,
- BULK_IN_EP, // bEndpointAddress
- 0x02, // bmAttributes = bulk
- LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
- 0x00, // bInterval
+ 0x07,
+ DESC_ENDPOINT,
+ BULK_IN_EP, // bEndpointAddress
+ 0x02, // bmAttributes = bulk
+ LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
+ 0x00, // bInterval
// Data EP OUT
- 0x07,
- DESC_ENDPOINT,
- INT_OUT_EP, // bEndpointAddress
- 0x03, // bmAttributes = intr
- LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
- 0x01, // bInterval
+ 0x07,
+ DESC_ENDPOINT,
+ INT_OUT_EP, // bEndpointAddress
+ 0x03, // bmAttributes = intr
+ LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
+ 0x01, // bInterval
// Data EP in
- 0x07,
- DESC_ENDPOINT,
- INT_IN_EP, // bEndpointAddress
- 0x03, // bmAttributes = intr
- LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
- 0x01, // bInterval
+ 0x07,
+ DESC_ENDPOINT,
+ INT_IN_EP, // bEndpointAddress
+ 0x03, // bmAttributes = intr
+ LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
+ 0x01, // bInterval
- // string descriptors
- 0x04,
- DESC_STRING,
- LE_WORD(0x0409),
+ // string descriptors
+ 0x04,
+ DESC_STRING,
+ LE_WORD(0x0409),
- 0x0E,
- DESC_STRING,
- 'A', 0, 'S', 0, 'C', 0, 'H', 0, 'U', 0, 'H', 0,
+ 0x0E,
+ DESC_STRING,
+ 'A', 0, 'S', 0, 'C', 0, 'H', 0, 'U', 0, 'H', 0,
- 0x14,
- DESC_STRING,
- 'U', 0, 'S', 0, 'B', 0, 'S', 0, 'e', 0, 'n', 0, 's', 0, 'o', 0, 'r', 0,
+ 0x14,
+ DESC_STRING,
+ 'U', 0, 'S', 0, 'B', 0, 'S', 0, 'e', 0, 'n', 0, 's', 0, 'o', 0, 'r', 0,
- 0x12,
- DESC_STRING,
- 'A', 0, 'O', 0, 'S', 0, '_', 0, 'G', 0, 'y', 0, 'r', 0, 'o', 0,
+ 0x12,
+ DESC_STRING,
+ 'A', 0, 'O', 0, 'S', 0, '_', 0, 'G', 0, 'y', 0, 'r', 0, 'o', 0,
// terminating zero
- 0
+ 0
};
@@ -205,25 +205,25 @@
static unsigned char abDataBuf[64];
int VCOM_putcharFromISR(int c, long *woken);
static void DataOut(unsigned char bEP, unsigned char bEPStatus) {
- int iLen;
- long lHigherPriorityTaskWoken = pdFALSE;
- /*
- char *a = "hello\n";
- while(*a){
- VCOM_putcharFromISR(*a,&lHigherPriorityTaskWoken);
- a ++;
- }
- */
- iLen = USBHwEPRead(bEP, abDataBuf, sizeof(abDataBuf));
- portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
+ int iLen;
+ long lHigherPriorityTaskWoken = pdFALSE;
+ /*
+ char *a = "hello\n";
+ while(*a){
+ VCOM_putcharFromISR(*a,&lHigherPriorityTaskWoken);
+ a ++;
+ }
+ */
+ iLen = USBHwEPRead(bEP, abDataBuf, sizeof(abDataBuf));
+ portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
}
#include "analog.h"
static struct DataStruct usbPacket;
static void DataIn(unsigned char bEP, unsigned char bEPStatus) {
- long lHigherPriorityTaskWoken = pdFALSE;
- fillSensorPacket(&usbPacket);
- USBHwEPWrite(bEP, (unsigned char *)&usbPacket, sizeof(usbPacket));
- portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
+ long lHigherPriorityTaskWoken = pdFALSE;
+ fillSensorPacket(&usbPacket);
+ USBHwEPWrite(bEP, (unsigned char *)&usbPacket, sizeof(usbPacket));
+ portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
}
/**
@@ -233,26 +233,26 @@
* @returns character written, or EOF if character could not be written
*/
int VCOM_putcharFromISR(int c, long *lHigherPriorityTaskWoken) {
- char cc = (char) c;
+ char cc = (char) c;
- if (xQueueSendFromISR(xCharsForTx, &cc,
- lHigherPriorityTaskWoken) == pdPASS) {
- return c;
- } else {
- return EOF;
- }
+ if (xQueueSendFromISR(xCharsForTx, &cc,
+ lHigherPriorityTaskWoken) == pdPASS) {
+ return c;
+ } else {
+ return EOF;
+ }
}
int VCOM_putchar(int c) {
- char cc = (char) c;
+ char cc = (char) c;
- // Don't block if not connected to USB.
- if (xQueueSend(xCharsForTx, &cc,
- USBIsConnected() ? usbMAX_SEND_BLOCK : 0) == pdPASS) {
- return c;
- } else {
- return EOF;
- }
+ // Don't block if not connected to USB.
+ if (xQueueSend(xCharsForTx, &cc,
+ USBIsConnected() ? usbMAX_SEND_BLOCK : 0) == pdPASS) {
+ return c;
+ } else {
+ return EOF;
+ }
}
@@ -262,13 +262,13 @@
* @returns character read, or EOF if character could not be read
*/
int VCOM_getchar(void) {
- unsigned char c;
+ unsigned char c;
- /* Block the task until a character is available. */
- if(xQueueReceive(xRxedChars, &c, 0) == pdTRUE){ //portMAX_DELAY);
- return c;
- }
- return -1;
+ /* Block the task until a character is available. */
+ if(xQueueReceive(xRxedChars, &c, 0) == pdTRUE){ //portMAX_DELAY);
+ return c;
+ }
+ return -1;
}
@@ -278,7 +278,7 @@
* Simply calls the USB ISR
*/
void USB_IRQHandler(void) {
- USBHwISR();
+ USBHwISR();
}
@@ -347,12 +347,12 @@
//USBHwAllowConnect();
// echo any character received (do USB stuff in interrupt)
for (;;) {
- // c = VCOM_getchar();
- // if (c != EOF) {
- // // Echo character back with INCREMENT_ECHO_BY offset, so for example if
- // // INCREMENT_ECHO_BY is 1 and 'A' is received, 'B' will be echoed back.
- // VCOM_putchar(c + INCREMENT_ECHO_BY);
- // }
+ // c = VCOM_getchar();
+ // if (c != EOF) {
+ // // Echo character back with INCREMENT_ECHO_BY offset, so for example if
+ // // INCREMENT_ECHO_BY is 1 and 'A' is received, 'B' will be echoed back.
+ // VCOM_putchar(c + INCREMENT_ECHO_BY);
+ // }
vTaskDelayUntil(&xLastFlashTime, 1000 / portTICK_RATE_MS);
}
}
diff --git a/gyro_board/src/usb/analog.c b/gyro_board/src/usb/analog.c
index 35c7fd3..c328189 100644
--- a/gyro_board/src/usb/analog.c
+++ b/gyro_board/src/usb/analog.c
@@ -14,36 +14,35 @@
#include "analog.h"
-void analog_init (void)
-{
- // b[1:0] CAN RD1 p0.0
- // b[3:2] CAN TD1 p0.1
- //PINCON->PINSEL0 = 0x00000005;
+void analog_init(void) {
+ // b[1:0] CAN RD1 p0.0
+ // b[3:2] CAN TD1 p0.1
+ //PINCON->PINSEL0 = 0x00000005;
- // b[29:28] USB_DMIN p0.30
- // b[27:26] USB_DPLUS p0.29
- // b[21:20] AD0.3 p0.26
- // b[19:18] AD0.2 p0.25
- // PINCON->PINSEL1 = 0x14140000;
+ // b[29:28] USB_DMIN p0.30
+ // b[27:26] USB_DPLUS p0.29
+ // b[21:20] AD0.3 p0.26
+ // b[19:18] AD0.2 p0.25
+ // PINCON->PINSEL1 = 0x14140000;
- // PINCON->PINSEL2 = 0x0;
+ // PINCON->PINSEL2 = 0x0;
- // b[31:30] AD0.5 p1.31
- // b[29:28] V_BUS p1.30
- // b[21:20] MCOB1 p1.26
- // b[19:18] MCOA1 p1.25
- // b[15:14] MCI1 p1.23
- // b[13:12] MCOB0 p1.22
- // b[09:08] MCI0 p1.20
- // b[07:06] MCOA0 p1.19
- // b[05:04] USB_UP_LED p1.18
- //PINCON->PINSEL3 = 0xE0145150;
- SC->PCONP |= PCONP_PCAD;
+ // b[31:30] AD0.5 p1.31
+ // b[29:28] V_BUS p1.30
+ // b[21:20] MCOB1 p1.26
+ // b[19:18] MCOA1 p1.25
+ // b[15:14] MCI1 p1.23
+ // b[13:12] MCOB0 p1.22
+ // b[09:08] MCI0 p1.20
+ // b[07:06] MCOA0 p1.19
+ // b[05:04] USB_UP_LED p1.18
+ //PINCON->PINSEL3 = 0xE0145150;
+ SC->PCONP |= PCONP_PCAD;
- // Enable AD0.0, AD0.1, AD0.2, AD0.3
- PINCON->PINSEL1 &= 0xFFC03FFF;
- PINCON->PINSEL1 |= 0x00D54000;
- ADC->ADCR = 0x00200500;
+ // Enable AD0.0, AD0.1, AD0.2, AD0.3
+ PINCON->PINSEL1 &= 0xFFC03FFF;
+ PINCON->PINSEL1 |= 0x00D54000;
+ ADC->ADCR = 0x00200500;
}
// ****************************************************************************
@@ -52,17 +51,16 @@
// **************** macros
-// starts convertion [26:24] = 001
+// starts conversion [26:24] = 001
// **************** functions
-int analog(int channel)
-{
- ADC->ADCR = ((ADC->ADCR & 0xF8FFFF00) | (0x01000000 | (1 << channel)));
+int analog(int channel) {
+ ADC->ADCR = ((ADC->ADCR & 0xF8FFFF00) | (0x01000000 | (1 << channel)));
- // Poll until it is done.
- while(!(ADC->ADGDR & 0x80000000));
+ // Poll until it is done.
+ while(!(ADC->ADGDR & 0x80000000));
- return ((ADC->ADGDR & 0x0000FFF0) >> 4);
+ return ((ADC->ADGDR & 0x0000FFF0) >> 4);
}
// GPIO1 P0.4
// GPIO2 P0.5
@@ -82,46 +80,44 @@
// DIP2 P0.11
// DIP3 P0.10
#define readGPIO(gpio,chan) ((((gpio)->FIOPIN) >> (chan)) & 1)
-inline int readGPIO_inline(int major,int minor){
- switch(major){
- case 0:
- return readGPIO(GPIO0,minor);
- case 1:
- return readGPIO(GPIO1,minor);
- case 2:
- return readGPIO(GPIO2,minor);
- default:
- return -1;
- }
+inline int readGPIO_inline(int major, int minor) {
+ switch (major) {
+ case 0:
+ return readGPIO(GPIO0, minor);
+ case 1:
+ return readGPIO(GPIO1, minor);
+ case 2:
+ return readGPIO(GPIO2, minor);
+ default:
+ return -1;
+ }
}
-int digital(int channel)
-{
- if(channel < 1){
- return -1;
- }else if(channel < 7){
- int chan = channel + 3;
- return readGPIO(GPIO0,chan);
- }else if(channel < 13){
- int chan = channel - 7;
- return readGPIO(GPIO2,chan);
- }
- return -1;
+int digital(int channel) {
+ if (channel < 1) {
+ return -1;
+ } else if (channel < 7) {
+ int chan = channel + 3;
+ return readGPIO(GPIO0,chan);
+ } else if (channel < 13) {
+ int chan = channel - 7;
+ return readGPIO(GPIO2,chan);
+ }
+ return -1;
}
-int dip(int channel)
-{
- switch(channel){
- case 0:
- return readGPIO(GPIO1,29);
- case 1:
- return readGPIO(GPIO2,13);
- case 2:
- return readGPIO(GPIO0,11);
- case 3:
- return readGPIO(GPIO0,10);
- default:
- return -1;
+int dip(int channel) {
+ switch (channel) {
+ case 0:
+ return readGPIO(GPIO1, 29);
+ case 1:
+ return readGPIO(GPIO2, 13);
+ case 2:
+ return readGPIO(GPIO0, 11);
+ case 3:
+ return readGPIO(GPIO0, 10);
+ default:
+ return -1;
- }
+ }
}
//ENC0A 1.20
//ENC0B 1.23
@@ -133,221 +129,216 @@
//ENC3B 0.20
#define ENC(gpio,a,b) readGPIO(gpio,a) * 2 + readGPIO(gpio,b)
-int encoder_bits(int channel)
-{
- switch(channel){
- case 0:
- return ENC(GPIO1,20,23);
- case 1:
- return ENC(GPIO2,11,12);
- case 2:
- return ENC(GPIO0,21,22);
- case 3:
- return ENC(GPIO0,19,20);
- default:
- return -1;
- }
- return -1;
+int encoder_bits(int channel) {
+ switch (channel) {
+ case 0:
+ return ENC(GPIO1, 20, 23);
+ case 1:
+ return ENC(GPIO2, 11, 12);
+ case 2:
+ return ENC(GPIO0, 21, 22);
+ case 3:
+ return ENC(GPIO0, 19, 20);
+ default:
+ return -1;
+ }
+ return -1;
}
+#undef ENC
+// Uses 2 EINT1 and EINT2 on 2.11 and 2.12.
volatile int32_t encoder1_val;
+// On GPIO pins 0.22 and 0.21.
volatile int32_t encoder2_val;
+// On GPIO pins 0.20 and 0.19.
volatile int32_t encoder3_val;
+// On GPIO pins 2.0 and 2.1.
volatile int32_t encoder4_val;
+// On GPIO pins 2.2 and 2.3.
volatile int32_t encoder5_val;
-// indexer encoder
-void EINT1_IRQHandler(void){
- //ENC1A 2.11
- SC->EXTINT = 0x2;
- int stored_val = encoder1_val;
- int fiopin = GPIO2->FIOPIN;
- if(((fiopin >> 1) ^ fiopin) & 0x800){
- stored_val ++;
- }else{
- stored_val --;
- }
- encoder1_val = stored_val;
- SC->EXTPOLAR ^= 0x2;
+// ENC1A 2.11
+void EINT1_IRQHandler(void) {
+ SC->EXTINT = 0x2;
+ int stored_val = encoder1_val;
+ int fiopin = GPIO2->FIOPIN;
+ if (((fiopin >> 1) ^ fiopin) & 0x800) {
+ ++stored_val;
+ } else {
+ --stored_val;
+ }
+ encoder1_val = stored_val;
+ SC->EXTPOLAR ^= 0x2;
}
-void EINT2_IRQHandler(void){
- //ENC1B 2.12
- SC->EXTINT = 0x4;
- int stored_val = encoder1_val;
- int fiopin = GPIO2->FIOPIN;
- if(((fiopin >> 1) ^ fiopin) & 0x800){
- stored_val --;
- }else{
- stored_val ++;
- }
- encoder1_val = stored_val;
- SC->EXTPOLAR ^= 0x4;
-}
-__attribute__( ( always_inline ) ) static __INLINE uint8_t __clz(uint32_t value)
-{
- uint8_t result;
-
- __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
- return(result);
-}
-//GPIO Interrupt handlers:
-void NoGPIO(){
-}
-void Encoder2ARise(){
- GPIOINT->IO0IntClr |= (1<<22);
- if(GPIO0->FIOPIN & (1<<21)){
- encoder2_val ++;
- }else{
- encoder2_val --;
- }
-}
-void Encoder2AFall(){
- GPIOINT->IO0IntClr |= (1<<22);
- if(GPIO0->FIOPIN & (1<<21)){
- encoder2_val --;
- }else{
- encoder2_val ++;
- }
-}
-void Encoder2BRise(){
- GPIOINT->IO0IntClr |= (1<<21);
- if(GPIO0->FIOPIN & (1<<22)){
- encoder2_val --;
- }else{
- encoder2_val ++;
- }
-}
-void Encoder2BFall(){
- GPIOINT->IO0IntClr |= (1<<21);
- if(GPIO0->FIOPIN & (1<<22)){
- encoder2_val ++;
- }else{
- encoder2_val --;
- }
+// ENC1B 2.12
+void EINT2_IRQHandler(void) {
+ SC->EXTINT = 0x4;
+ int stored_val = encoder1_val;
+ int fiopin = GPIO2->FIOPIN;
+ if (((fiopin >> 1) ^ fiopin) & 0x800) {
+ --stored_val;
+ } else {
+ ++stored_val;
+ }
+ encoder1_val = stored_val;
+ SC->EXTPOLAR ^= 0x4;
}
-void Encoder3ARise(){
- GPIOINT->IO0IntClr |= (1<<20);
- if(GPIO0->FIOPIN & (1<<19)){
- encoder3_val ++;
- }else{
- encoder3_val --;
- }
+// GPIO Interrupt handlers
+void NoGPIO() {}
+void Encoder2ARise() {
+ GPIOINT->IO0IntClr |= (1 << 22);
+ if (GPIO0->FIOPIN & (1 << 21)) {
+ ++encoder2_val;
+ } else {
+ --encoder2_val;
+ }
}
-void Encoder3AFall(){
- GPIOINT->IO0IntClr |= (1<<20);
- if(GPIO0->FIOPIN & (1<<19)){
- encoder3_val --;
- }else{
- encoder3_val ++;
- }
+void Encoder2AFall() {
+ GPIOINT->IO0IntClr |= (1 << 22);
+ if (GPIO0->FIOPIN & (1 << 21)) {
+ --encoder2_val;
+ } else {
+ ++encoder2_val;
+ }
}
-void Encoder3BRise(){
- GPIOINT->IO0IntClr |= (1<<19);
- if(GPIO0->FIOPIN & (1<<20)){
- encoder3_val --;
- }else{
- encoder3_val ++;
- }
+void Encoder2BRise() {
+ GPIOINT->IO0IntClr |= (1 << 21);
+ if (GPIO0->FIOPIN & (1 << 22)) {
+ --encoder2_val;
+ } else {
+ ++encoder2_val;
+ }
}
-void Encoder3BFall(){
- GPIOINT->IO0IntClr |= (1<<19);
- if(GPIO0->FIOPIN & (1<<20)){
- encoder3_val ++;
- }else{
- encoder3_val --;
- }
+void Encoder2BFall() {
+ GPIOINT->IO0IntClr |= (1 << 21);
+ if (GPIO0->FIOPIN & (1 << 22)) {
+ ++encoder2_val;
+ } else {
+ --encoder2_val;
+ }
}
-void Encoder4ARise(){
- GPIOINT->IO2IntClr |= (1<<0);
- if(GPIO2->FIOPIN & (1<<1)){
- encoder4_val ++;
- }else{
- encoder4_val --;
- }
+void Encoder3ARise() {
+ GPIOINT->IO0IntClr |= (1 << 20);
+ if (GPIO0->FIOPIN & (1 << 19)) {
+ ++encoder3_val;
+ } else {
+ --encoder3_val;
+ }
}
-void Encoder4AFall(){
- GPIOINT->IO2IntClr |= (1<<0);
- if(GPIO2->FIOPIN & (1<<1)){
- encoder4_val --;
- }else{
- encoder4_val ++;
- }
+void Encoder3AFall() {
+ GPIOINT->IO0IntClr |= (1 << 20);
+ if (GPIO0->FIOPIN & (1 << 19)) {
+ --encoder3_val;
+ } else {
+ ++encoder3_val;
+ }
}
-void Encoder4BRise(){
- GPIOINT->IO2IntClr |= (1<<1);
- if(GPIO2->FIOPIN & (1<<0)){
- encoder4_val --;
- }else{
- encoder4_val ++;
- }
+void Encoder3BRise() {
+ GPIOINT->IO0IntClr |= (1 << 19);
+ if (GPIO0->FIOPIN & (1 << 20)) {
+ --encoder3_val;
+ } else {
+ ++encoder3_val;
+ }
}
-void Encoder4BFall(){
- GPIOINT->IO2IntClr |= (1<<1);
- if(GPIO2->FIOPIN & (1<<0)){
- encoder4_val ++;
- }else{
- encoder4_val --;
- }
+void Encoder3BFall() {
+ GPIOINT->IO0IntClr |= (1 << 19);
+ if (GPIO0->FIOPIN & (1 << 20)) {
+ ++encoder3_val;
+ } else {
+ --encoder3_val;
+ }
}
-//
+void Encoder4ARise() {
+ GPIOINT->IO2IntClr |= (1 << 0);
+ if (GPIO2->FIOPIN & (1 << 1)) {
+ ++encoder4_val;
+ } else {
+ --encoder4_val;
+ }
+}
+void Encoder4AFall() {
+ GPIOINT->IO2IntClr |= (1 << 0);
+ if (GPIO2->FIOPIN & (1 << 1)) {
+ --encoder4_val;
+ } else {
+ ++encoder4_val;
+ }
+}
+void Encoder4BRise() {
+ GPIOINT->IO2IntClr |= (1 << 1);
+ if (GPIO2->FIOPIN & (1 << 0)) {
+ --encoder4_val;
+ } else {
+ ++encoder4_val;
+ }
+}
+void Encoder4BFall() {
+ GPIOINT->IO2IntClr |= (1 << 1);
+ if (GPIO2->FIOPIN & (1 << 0)) {
+ ++encoder4_val;
+ } else {
+ --encoder4_val;
+ }
+}
-void Encoder5ARise(){
- GPIOINT->IO2IntClr |= (1<<2);
- if(GPIO2->FIOPIN & (1<<3)){
- encoder5_val ++;
- }else{
- encoder5_val --;
- }
+void Encoder5ARise() {
+ GPIOINT->IO2IntClr |= (1 << 2);
+ if (GPIO2->FIOPIN & (1 << 3)) {
+ ++encoder5_val;
+ } else {
+ --encoder5_val;
+ }
}
-void Encoder5AFall(){
- GPIOINT->IO2IntClr |= (1<<2);
- if(GPIO2->FIOPIN & (1<<3)){
- encoder5_val --;
- }else{
- encoder5_val ++;
- }
+void Encoder5AFall() {
+ GPIOINT->IO2IntClr |= (1 << 2);
+ if (GPIO2->FIOPIN & (1 << 3)) {
+ --encoder5_val;
+ } else {
+ ++encoder5_val;
+ }
}
-void Encoder5BRise(){
- GPIOINT->IO2IntClr |= (1<<3);
- if(GPIO2->FIOPIN & (1<<2)){
- encoder5_val --;
- }else{
- encoder5_val ++;
- }
+void Encoder5BRise() {
+ GPIOINT->IO2IntClr |= (1 << 3);
+ if (GPIO2->FIOPIN & (1 << 2)) {
+ --encoder5_val;
+ } else {
+ ++encoder5_val;
+ }
}
-void Encoder5BFall(){
- GPIOINT->IO2IntClr |= (1<<3);
- if(GPIO2->FIOPIN & (1<<2)){
- encoder5_val ++;
- }else{
- encoder5_val --;
- }
+void Encoder5BFall() {
+ GPIOINT->IO2IntClr |= (1 << 3);
+ if (GPIO2->FIOPIN & (1 << 2)) {
+ ++encoder5_val;
+ } else {
+ --encoder5_val;
+ }
}
volatile int32_t capture_top_rise;
-volatile int8_t top_rise_count;
-void IndexerTopRise(){
- GPIOINT->IO0IntClr |= (1<<5);
- // edge counting encoder capture
- top_rise_count ++;
- capture_top_rise = encoder3_val;
+volatile int8_t top_rise_count;
+void IndexerTopRise() {
+ GPIOINT->IO0IntClr |= (1 << 5);
+ // edge counting encoder capture
+ ++top_rise_count;
+ capture_top_rise = encoder3_val;
}
volatile int32_t capture_top_fall;
volatile int8_t top_fall_count;
-void IndexerTopFall(){
- GPIOINT->IO0IntClr |= (1<<5);
- // edge counting encoder capture
- top_fall_count ++;
- capture_top_fall = encoder3_val;
+void IndexerTopFall() {
+ GPIOINT->IO0IntClr |= (1 << 5);
+ // edge counting encoder capture
+ ++top_fall_count;
+ capture_top_fall = encoder3_val;
}
volatile int8_t bottom_rise_count;
-void IndexerBottomRise(){
- GPIOINT->IO0IntClr |= (1<<4);
- // edge counting
- bottom_rise_count ++;
+void IndexerBottomRise() {
+ GPIOINT->IO0IntClr |= (1 << 4);
+ // edge counting
+ ++bottom_rise_count;
}
volatile int32_t capture_bottom_fall_delay;
volatile int8_t bottom_fall_delay_count;
@@ -355,398 +346,402 @@
portTickType xDelayTimeFrom;
static portTASK_FUNCTION(vDelayCapture, pvParameters)
{
- portTickType xSleepFrom;
- xSleepFrom = xTaskGetTickCount();
+ portTickType xSleepFrom;
+ xSleepFrom = xTaskGetTickCount();
- for(;;){
- NVIC_DisableIRQ(EINT3_IRQn);
- if(dirty_delay){
- xSleepFrom = xDelayTimeFrom;
- dirty_delay = 0;
- NVIC_EnableIRQ(EINT3_IRQn);
+ for(;;) {
+ NVIC_DisableIRQ(EINT3_IRQn);
+ if (dirty_delay) {
+ xSleepFrom = xDelayTimeFrom;
+ dirty_delay = 0;
+ NVIC_EnableIRQ(EINT3_IRQn);
- vTaskDelayUntil(&xSleepFrom, 32 / portTICK_RATE_MS);
+ vTaskDelayUntil(&xSleepFrom, 32 / portTICK_RATE_MS);
- NVIC_DisableIRQ(EINT3_IRQn);
- bottom_fall_delay_count ++;
- capture_bottom_fall_delay = encoder3_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- }else{
- NVIC_EnableIRQ(EINT3_IRQn);
- vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
- }
- }
+ NVIC_DisableIRQ(EINT3_IRQn);
+ bottom_fall_delay_count ++;
+ capture_bottom_fall_delay = encoder3_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ } else {
+ NVIC_EnableIRQ(EINT3_IRQn);
+ vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
+ }
+ }
}
volatile int8_t bottom_fall_count;
-void IndexerBottomFall(){
- GPIOINT->IO0IntClr |= (1<<4);
- bottom_fall_count ++;
- // edge counting start delayed capture
- xDelayTimeFrom = xTaskGetTickCount();
- dirty_delay = 1;
+void IndexerBottomFall() {
+ GPIOINT->IO0IntClr |= (1 << 4);
+ bottom_fall_count ++;
+ // edge counting start delayed capture
+ xDelayTimeFrom = xTaskGetTickCount();
+ dirty_delay = 1;
}
volatile int32_t capture_wrist_rise;
volatile int8_t wrist_rise_count;
-void WristHallRise(){
- GPIOINT->IO0IntClr |= (1<<6);
- // edge counting encoder capture
- wrist_rise_count ++;
- capture_wrist_rise = (int32_t)QEI->QEIPOS;
+void WristHallRise() {
+ GPIOINT->IO0IntClr |= (1 << 6);
+ // edge counting encoder capture
+ wrist_rise_count ++;
+ capture_wrist_rise = (int32_t)QEI->QEIPOS;
}
volatile int32_t capture_shooter_angle_rise;
volatile int8_t shooter_angle_rise_count;
-void ShooterHallRise(){
- GPIOINT->IO0IntClr |= (1<<7);
- // edge counting encoder capture
- shooter_angle_rise_count ++;
- capture_shooter_angle_rise = encoder2_val;
+void ShooterHallRise() {
+ GPIOINT->IO0IntClr |= (1 << 7);
+ // edge counting encoder capture
+ shooter_angle_rise_count ++;
+ capture_shooter_angle_rise = encoder2_val;
}
-typedef void (*PFUNC)(void);
-inline static void IRQ_Dispatch(void){
- uint8_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
- (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
+// Count leading zeros.
+// Returns 0 if bit 31 is set etc.
+__attribute__((always_inline)) static __INLINE uint8_t __clz(uint32_t value) {
+ uint8_t result;
+ __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value));
+ return result;
+}
+inline static void IRQ_Dispatch(void) {
+ uint8_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
+ (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
- const static PFUNC table[] = {
- Encoder5BFall, // index 0: P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
- Encoder5AFall, // index 1: P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
- Encoder4BFall, // index 2: P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
- Encoder4AFall, // index 3: P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
- NoGPIO, // index 4: NO GPIO #bit 27
- Encoder2AFall, // index 5: P0.22 Fall #bit 26 //Encoder 2 A
- Encoder2BFall, // index 6: P0.21 Fall #bit 25 //Encoder 2 B
- Encoder3AFall, // index 7: P0.20 Fall #bit 24 //Encoder 3 A
- Encoder3BFall, // index 8: P0.19 Fall #bit 23 //Encoder 3 B
- Encoder2ARise, // index 9: P0.22 Rise #bit 22 //Encoder 2 A
- Encoder2BRise, // index 10: P0.21 Rise #bit 21 //Encoder 2 B
- Encoder3ARise, // index 11: P0.20 Rise #bit 20 //Encoder 3 A
- Encoder3BRise, // index 12: P0.19 Rise #bit 19 //Encoder 3 B
- NoGPIO, // index 13: NO GPIO #bit 18
- NoGPIO, // index 14: NO GPIO #bit 17
- NoGPIO, // index 15: NO GPIO #bit 16
- NoGPIO, // index 16: NO GPIO #bit 15
- NoGPIO, // index 17: NO GPIO #bit 14
- NoGPIO, // index 18: NO GPIO #bit 13
- NoGPIO, // index 19: NO GPIO #bit 12
- NoGPIO, // index 20: NO GPIO #bit 11
- NoGPIO, // index 21: NO GPIO #bit 10
- IndexerTopFall, // index 22: P0.5 Fall #bit 9 //Indexer Top //Dio 2
- IndexerBottomFall, // index 23: P0.4 Fall #bit 8 //Indexer Bottom //Dio 1
- ShooterHallRise, // index 24: P0.7 Rise #bit 7 //Shooter Hall //Dio 4
- WristHallRise, // index 25: P0.6 Rise #bit 6 //Wrist Hall //Dio 3
- IndexerTopRise, // index 26: P0.5 Rise #bit 5 //Indexer Top //Dio 2
- IndexerBottomRise, // index 27: P0.4 Rise #bit 4 //Indexer Bottom //Dio 1
- Encoder5BRise, // index 28: P2.3 Rise #bit 3 //Encoder 5 B //Dio 10
- Encoder5ARise, // index 29: P2.2 Rise #bit 2 //Encoder 5 A //Dio 9
- Encoder4BRise, // index 30: P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
- Encoder4ARise, // index 31: P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
- NoGPIO // index 32: NO BITS SET #False Alarm
- };
- table[index]();
-
+ typedef void (*Handler)(void);
+ const static Handler table[] = {
+ Encoder5BFall, // index 0: P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
+ Encoder5AFall, // index 1: P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
+ Encoder4BFall, // index 2: P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
+ Encoder4AFall, // index 3: P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
+ NoGPIO, // index 4: NO GPIO #bit 27
+ Encoder2AFall, // index 5: P0.22 Fall #bit 26 //Encoder 2 A
+ Encoder2BFall, // index 6: P0.21 Fall #bit 25 //Encoder 2 B
+ Encoder3AFall, // index 7: P0.20 Fall #bit 24 //Encoder 3 A
+ Encoder3BFall, // index 8: P0.19 Fall #bit 23 //Encoder 3 B
+ Encoder2ARise, // index 9: P0.22 Rise #bit 22 //Encoder 2 A
+ Encoder2BRise, // index 10: P0.21 Rise #bit 21 //Encoder 2 B
+ Encoder3ARise, // index 11: P0.20 Rise #bit 20 //Encoder 3 A
+ Encoder3BRise, // index 12: P0.19 Rise #bit 19 //Encoder 3 B
+ NoGPIO, // index 13: NO GPIO #bit 18
+ NoGPIO, // index 14: NO GPIO #bit 17
+ NoGPIO, // index 15: NO GPIO #bit 16
+ NoGPIO, // index 16: NO GPIO #bit 15
+ NoGPIO, // index 17: NO GPIO #bit 14
+ NoGPIO, // index 18: NO GPIO #bit 13
+ NoGPIO, // index 19: NO GPIO #bit 12
+ NoGPIO, // index 20: NO GPIO #bit 11
+ NoGPIO, // index 21: NO GPIO #bit 10
+ IndexerTopFall, // index 22: P0.5 Fall #bit 9 //Indexer Top //Dio 2
+ IndexerBottomFall, // index 23: P0.4 Fall #bit 8 //Indexer Bottom //Dio 1
+ ShooterHallRise, // index 24: P0.7 Rise #bit 7 //Shooter Hall //Dio 4
+ WristHallRise, // index 25: P0.6 Rise #bit 6 //Wrist Hall //Dio 3
+ IndexerTopRise, // index 26: P0.5 Rise #bit 5 //Indexer Top //Dio 2
+ IndexerBottomRise, // index 27: P0.4 Rise #bit 4 //Indexer Bottom //Dio 1
+ Encoder5BRise, // index 28: P2.3 Rise #bit 3 //Encoder 5 B //Dio 10
+ Encoder5ARise, // index 29: P2.2 Rise #bit 2 //Encoder 5 A //Dio 9
+ Encoder4BRise, // index 30: P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
+ Encoder4ARise, // index 31: P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
+ NoGPIO // index 32: NO BITS SET #False Alarm
+ };
+ table[index]();
/*
- switch(index){
- case 0: //P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
- Encoder5BFall();
- return;
+ switch(index) {
+ case 0: //P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
+ Encoder5BFall();
+ return;
- case 1: //P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
- Encoder5AFall();
- return;
+ case 1: //P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
+ Encoder5AFall();
+ return;
- case 2: //P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
- Encoder4BFall();
- return;
+ case 2: //P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
+ Encoder4BFall();
+ return;
- case 3: //P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
- Encoder4AFall();
- return;
+ case 3: //P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
+ Encoder4AFall();
+ return;
- case 4: //NO GPIO #bit 27
- NoGPIO();
- return;
+ case 4: //NO GPIO #bit 27
+ NoGPIO();
+ return;
- case 5: //P0.22 Fall #bit 26 //Encoder 2 A
- Encoder2AFall();
- return;
+ case 5: //P0.22 Fall #bit 26 //Encoder 2 A
+ Encoder2AFall();
+ return;
- case 6: //P0.21 Fall #bit 25 //Encoder 2 B
- Encoder2BFall();
- return;
+ case 6: //P0.21 Fall #bit 25 //Encoder 2 B
+ Encoder2BFall();
+ return;
- case 7: //P0.20 Fall #bit 24 //Encoder 3 A
- Encoder3AFall();
- return;
+ case 7: //P0.20 Fall #bit 24 //Encoder 3 A
+ Encoder3AFall();
+ return;
- case 8: //P0.19 Fall #bit 23 //Encoder 3 B
- Encoder3BFall();
- return;
+ case 8: //P0.19 Fall #bit 23 //Encoder 3 B
+ Encoder3BFall();
+ return;
- case 9: //P0.22 Rise #bit 22 //Encoder 2 A
- Encoder2ARise();
- return;
+ case 9: //P0.22 Rise #bit 22 //Encoder 2 A
+ Encoder2ARise();
+ return;
- case 10: //P0.21 Rise #bit 21 //Encoder 2 B
- Encoder2BRise();
- return;
+ case 10: //P0.21 Rise #bit 21 //Encoder 2 B
+ Encoder2BRise();
+ return;
- case 11: //P0.20 Rise #bit 20 //Encoder 3 A
- Encoder3ARise();
- return;
+ case 11: //P0.20 Rise #bit 20 //Encoder 3 A
+ Encoder3ARise();
+ return;
- case 12: //P0.19 Rise #bit 19 //Encoder 3 B
- Encoder3BRise();
- return;
+ case 12: //P0.19 Rise #bit 19 //Encoder 3 B
+ Encoder3BRise();
+ return;
- case 13: //NO GPIO #bit 18
- NoGPIO();
- return;
+ case 13: //NO GPIO #bit 18
+ NoGPIO();
+ return;
- case 14: //NO GPIO #bit 17
- NoGPIO();
- return;
+ case 14: //NO GPIO #bit 17
+ NoGPIO();
+ return;
- case 15: //NO GPIO #bit 16
- NoGPIO();
- return;
+ case 15: //NO GPIO #bit 16
+ NoGPIO();
+ return;
- case 16: //NO GPIO #bit 15
- NoGPIO();
- return;
+ case 16: //NO GPIO #bit 15
+ NoGPIO();
+ return;
- case 17: //NO GPIO #bit 14
- NoGPIO();
- return;
+ case 17: //NO GPIO #bit 14
+ NoGPIO();
+ return;
- case 18: //NO GPIO #bit 13
- NoGPIO();
- return;
+ case 18: //NO GPIO #bit 13
+ NoGPIO();
+ return;
- case 19: //NO GPIO #bit 12
- NoGPIO();
- return;
+ case 19: //NO GPIO #bit 12
+ NoGPIO();
+ return;
- case 20: //NO GPIO #bit 11
- NoGPIO();
- return;
+ case 20: //NO GPIO #bit 11
+ NoGPIO();
+ return;
- case 21: //NO GPIO #bit 10
- NoGPIO();
- return;
+ case 21: //NO GPIO #bit 10
+ NoGPIO();
+ return;
- case 22: //P0.3 Fall #bit 9 //Indexer Top //Dio 2
- IndexerTopFall();
- return;
+ case 22: //P0.3 Fall #bit 9 //Indexer Top //Dio 2
+ IndexerTopFall();
+ return;
- case 23: //P0.4 Fall #bit 8 //Indexer Bottom //Dio 1
- IndexerBottomFall();
- return;
+ case 23: //P0.4 Fall #bit 8 //Indexer Bottom //Dio 1
+ IndexerBottomFall();
+ return;
- case 24: //P0.7 Rise #bit 7 //Shooter Hall //Dio 4
- ShooterHallRise();
- return;
+ case 24: //P0.7 Rise #bit 7 //Shooter Hall //Dio 4
+ ShooterHallRise();
+ return;
- case 25: //P0.6 Rise #bit 6 //Wrist Hall //Dio 3
- WristHallRise();
- return;
+ case 25: //P0.6 Rise #bit 6 //Wrist Hall //Dio 3
+ WristHallRise();
+ return;
- case 26: //P0.5 Rise #bit 5 //Indexer Top //Dio 2
- IndexerTopRise();
- return;
+ case 26: //P0.5 Rise #bit 5 //Indexer Top //Dio 2
+ IndexerTopRise();
+ return;
- case 27: //P0.4 Rise #bit 4 //Indexer Bottom //Dio 1
- IndexerBottomRise();
- return;
+ case 27: //P0.4 Rise #bit 4 //Indexer Bottom //Dio 1
+ IndexerBottomRise();
+ return;
- case 28: //P2.3 Rise #bit 3 //Encoder 5 B //Dio 10
- Encoder5BRise();
- return;
+ case 28: //P2.3 Rise #bit 3 //Encoder 5 B //Dio 10
+ Encoder5BRise();
+ return;
- case 29: //P2.2 Rise #bit 2 //Encoder 5 A //Dio 9
- Encoder5ARise();
- return;
+ case 29: //P2.2 Rise #bit 2 //Encoder 5 A //Dio 9
+ Encoder5ARise();
+ return;
- case 30: //P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
- Encoder4BRise();
- return;
+ case 30: //P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
+ Encoder4BRise();
+ return;
- case 31: //P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
- Encoder4ARise();
- return;
+ case 31: //P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
+ Encoder4ARise();
+ return;
- case 32: //NO BITS SET #False Alarm
- NoGPIO();
- return;
- }
- */
+ case 32: //NO BITS SET #False Alarm
+ NoGPIO();
+ return;
+ }
+ */
}
-void EINT3_IRQHandler(void){
- NVIC_DisableIRQ(EINT3_IRQn);
- IRQ_Dispatch();
- NVIC_EnableIRQ(EINT3_IRQn);
+void EINT3_IRQHandler(void) {
+ NVIC_DisableIRQ(EINT3_IRQn);
+ IRQ_Dispatch();
+ NVIC_EnableIRQ(EINT3_IRQn);
}
int32_t encoder_val(int chan)
{
- int32_t val;
- switch(chan){
- case 0: //Wrist
- return (int32_t)QEI->QEIPOS;
- case 1: //Shooter Wheel
- NVIC_DisableIRQ(EINT1_IRQn);
- NVIC_DisableIRQ(EINT2_IRQn);
- val = encoder1_val;
- NVIC_EnableIRQ(EINT2_IRQn);
- NVIC_EnableIRQ(EINT1_IRQn);
- return val;
- case 2: //Shooter Angle
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder2_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 3: //Indexer
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder3_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 4: //Drive R
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder4_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 5: //Drive L
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder5_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- default:
- return -1;
- }
+ int32_t val;
+ switch(chan) {
+ case 0: //Wrist
+ return (int32_t)QEI->QEIPOS;
+ case 1: //Shooter Wheel
+ NVIC_DisableIRQ(EINT1_IRQn);
+ NVIC_DisableIRQ(EINT2_IRQn);
+ val = encoder1_val;
+ NVIC_EnableIRQ(EINT2_IRQn);
+ NVIC_EnableIRQ(EINT1_IRQn);
+ return val;
+ case 2: //Shooter Angle
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder2_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 3: //Indexer
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder3_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 4: //Drive R
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder4_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 5: //Drive L
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder5_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ default:
+ return -1;
+ }
}
-void fillSensorPacket(struct DataStruct *packet){
- packet->gyro_angle = gyro_angle;
- NVIC_DisableIRQ(EINT1_IRQn);
- NVIC_DisableIRQ(EINT2_IRQn);
- packet->shooter = encoder1_val;
- NVIC_EnableIRQ(EINT2_IRQn);
- NVIC_EnableIRQ(EINT1_IRQn);
- NVIC_DisableIRQ(EINT3_IRQn);
- packet->right_drive = encoder4_val;
- packet->left_drive = encoder5_val;
- packet->shooter_angle = encoder2_val;
- packet->indexer = encoder3_val;
- packet->wrist = (int32_t)QEI->QEIPOS;
+void fillSensorPacket(struct DataStruct *packet) {
+ packet->gyro_angle = gyro_angle;
- packet->capture_top_rise = capture_top_rise;
- packet->top_rise_count = top_rise_count;
+ NVIC_DisableIRQ(EINT1_IRQn);
+ NVIC_DisableIRQ(EINT2_IRQn);
+ packet->shooter = encoder1_val;
+ NVIC_EnableIRQ(EINT2_IRQn);
+ NVIC_EnableIRQ(EINT1_IRQn);
- packet->capture_top_fall = capture_top_fall;
- packet->top_fall_count = top_fall_count;
+ NVIC_DisableIRQ(EINT3_IRQn);
- packet->bottom_rise_count = bottom_rise_count;
+ packet->right_drive = encoder4_val;
+ packet->left_drive = encoder5_val;
+ packet->shooter_angle = encoder2_val;
+ packet->indexer = encoder3_val;
- packet->capture_bottom_fall_delay = capture_bottom_fall_delay;
- packet->bottom_fall_delay_count = bottom_fall_delay_count;
- packet->bottom_fall_count = bottom_fall_count;
+ packet->capture_top_rise = capture_top_rise;
+ packet->top_rise_count = top_rise_count;
- packet->capture_wrist_rise = capture_wrist_rise;
- packet->wrist_rise_count = wrist_rise_count;
+ packet->capture_top_fall = capture_top_fall;
+ packet->top_fall_count = top_fall_count;
- packet->capture_shooter_angle_rise = capture_shooter_angle_rise;
- packet->shooter_angle_rise_count = shooter_angle_rise_count;
+ packet->bottom_rise_count = bottom_rise_count;
- NVIC_EnableIRQ(EINT3_IRQn);
+ packet->capture_bottom_fall_delay = capture_bottom_fall_delay;
+ packet->bottom_fall_delay_count = bottom_fall_delay_count;
+ packet->bottom_fall_count = bottom_fall_count;
+
+ packet->capture_wrist_rise = capture_wrist_rise;
+ packet->wrist_rise_count = wrist_rise_count;
+
+ packet->capture_shooter_angle_rise = capture_shooter_angle_rise;
+ packet->shooter_angle_rise_count = shooter_angle_rise_count;
+
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+ packet->wrist = (int32_t)QEI->QEIPOS;
}
void encoder_init(void)
{
- // port 0
- // Setup the encoder interface.
- SC->PCONP |= PCONP_PCQEI;
- PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
+ // Setup the encoder interface.
+ SC->PCONP |= PCONP_PCQEI;
+ PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
+ // Reset the count and velocity.
+ QEI->QEICON = 0x00000005;
+ QEI->QEICONF = 0x00000004;
+ // Wrap back to 0 when we wrap the int and vice versa.
+ QEI->QEIMAXPOS = 0xFFFFFFFF;
- // Reset the count and velocity
- QEI->QEICON = 0x00000005;
+ // Set up encoder 1.
+ // Make GPIOs 2.11 and 2.12 trigger EINT1 and EINT2 (respectively).
+ // PINSEL4[23:22] = {0 1}
+ // PINSEL4[25:24] = {0 1}
+ PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
+ PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 24)) | (0x1 << 24);
+ // Clear the interrupt flags for EINT1 and EINT2 (0x6 = 0b0110).
+ SC->EXTMODE = 0x6;
+ SC->EXTINT = 0x6;
+ NVIC_EnableIRQ(EINT1_IRQn);
+ NVIC_EnableIRQ(EINT2_IRQn);
+ encoder1_val = 0;
- QEI->QEICONF = 0x00000004;
- // Wrap back to 0 when we wrap the int...
- QEI->QEIMAXPOS = 0xffffffff;
-// port 1
- //PINSEL4 23/22 0 1
- //PINSEL4 25 24 0 1
- PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
- PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 24)) | (0x1 << 24);
+ // Set up encoder 2.
+ GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt.
+ GPIOINT->IO0IntEnF |= (1 << 21); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 21); // Set GPIO rising interrupt.
+ PINCON->PINSEL1 &= ~(0x3 << 12);
+ PINCON->PINSEL1 &= ~(0x3 << 10);
+ encoder2_val = 0;
- //EXTMODE 1 2 1 1 // all others off
- SC->EXTMODE = 0x6;
- SC->EXTINT = 0x6;
- NVIC_EnableIRQ(EINT1_IRQn);
- NVIC_EnableIRQ(EINT2_IRQn);
- encoder1_val = 0;
+ // Set up encoder 3.
+ GPIOINT->IO0IntEnF |= (1 << 20); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 20); // Set GPIO rising interrupt.
+ GPIOINT->IO0IntEnF |= (1 << 19); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 19); // Set GPIO rising interrupt.
+ PINCON->PINSEL1 &= ~(0x3 << 8);
+ PINCON->PINSEL1 &= ~(0x3 << 6);
+ encoder3_val = 0;
-//port 2
+ // Set up encoder 4.
+ GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
+ PINCON->PINSEL4 &= ~(0x3 << 0);
+ PINCON->PINSEL4 &= ~(0x3 << 2);
+ encoder4_val = 0;
- encoder2_val = 0;
- GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt
- GPIOINT->IO0IntEnF |= (1 << 21); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 21); // Set GPIO rising interrupt
- PINCON->PINSEL1 &= ~(0x3 << 12);
- PINCON->PINSEL1 &= ~(0x3 << 10);
+ // Set up encoder 5.
+ GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
+ PINCON->PINSEL4 &= ~(0x3 << 4);
+ PINCON->PINSEL4 &= ~(0x3 << 6);
+ encoder5_val = 0;
-//port 3
-
- encoder3_val = 0;
- GPIOINT->IO0IntEnF |= (1 << 20); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 20); // Set GPIO rising interrupt
- GPIOINT->IO0IntEnF |= (1 << 19); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 19); // Set GPIO rising interrupt
- PINCON->PINSEL1 &= ~(0x3 << 8);
- PINCON->PINSEL1 &= ~(0x3 << 6);
+ // Enable interrupts from the GPIO pins.
+ NVIC_EnableIRQ(EINT3_IRQn);
-//port 4
- encoder4_val = 0;
- GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt
- GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt
- GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt
- GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt
- PINCON->PINSEL4 &= ~(0x3 << 0);
- PINCON->PINSEL4 &= ~(0x3 << 2);
+ xTaskCreate(vDelayCapture,
+ (signed char *) "SENSORs",
+ configMINIMAL_STACK_SIZE + 100,
+ NULL /*parameters*/,
+ tskIDLE_PRIORITY + 5,
+ NULL /*return task handle*/);
-//port 5
- encoder5_val = 0;
- GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt
- GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt
- GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt
- GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt
- PINCON->PINSEL4 &= ~(0x3 << 4);
- PINCON->PINSEL4 &= ~(0x3 << 6);
+ GPIOINT->IO0IntEnF |= (1 << 4); // Set GPIO falling interrupt
+ GPIOINT->IO0IntEnR |= (1 << 4); // Set GPIO rising interrupt
+ PINCON->PINSEL0 &= ~(0x3 << 8);
+ GPIOINT->IO0IntEnF |= (1 << 5); // Set GPIO falling interrupt
+ GPIOINT->IO0IntEnR |= (1 << 5); // Set GPIO rising interrupt
+ PINCON->PINSEL0 &= ~(0x3 << 10);
-//gpio pins:
- NVIC_EnableIRQ(EINT3_IRQn);
+ GPIOINT->IO0IntEnR |= (1 << 6); // Set GPIO rising interrupt
+ PINCON->PINSEL0 &= ~(0x3 << 12);
-
-// delay capture:
- xTaskCreate(vDelayCapture, (signed char *) "SENSORs", configMINIMAL_STACK_SIZE + 100, NULL, tskIDLE_PRIORITY + 5, NULL);
-
-
-// other:
- GPIOINT->IO0IntEnF |= (1 << 4); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 4); // Set GPIO rising interrupt
- PINCON->PINSEL0 &= ~(0x3 << 8);
-
- GPIOINT->IO0IntEnF |= (1 << 5); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 5); // Set GPIO rising interrupt
- PINCON->PINSEL0 &= ~(0x3 << 10);
-
- GPIOINT->IO0IntEnR |= (1 << 6); // Set GPIO rising interrupt
- PINCON->PINSEL0 &= ~(0x3 << 12);
-
- GPIOINT->IO0IntEnR |= (1 << 7); // Set GPIO rising interrupt
- PINCON->PINSEL0 &= ~(0x3 << 14);
-
-
+ GPIOINT->IO0IntEnR |= (1 << 7); // Set GPIO rising interrupt
+ PINCON->PINSEL0 &= ~(0x3 << 14);
}
diff --git a/gyro_board/src/usb/analog.h b/gyro_board/src/usb/analog.h
index 7026af0..267308b 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -2,7 +2,8 @@
#define __ANALOG_H__
extern int64_t gyro_angle;
-struct DataStruct{
+
+struct DataStruct {
int64_t gyro_angle;
int32_t right_drive;
@@ -30,15 +31,19 @@
int8_t wrist_rise_count;
int8_t shooter_angle_rise_count;
-}__attribute__((__packed__));
+} __attribute__((__packed__));
void fillSensorPacket(struct DataStruct *packet);
+void analog_init(void);
+int analog(int channel);
-void analog_init (void);
-int analog(int chan);
-int digital(int chan);
-int encoder_bits(int chan);
+int digital(int channel);
+
+// Returns the current values of the inputs for the given encoder (as the low 2
+// bits).
+int encoder_bits(int channel);
void encoder_init(void);
-int32_t encoder_val(int chan);
-int dip(int chan);
-#endif // __ANALOG_H__
+int32_t encoder_val(int channel);
+
+int dip(int channel);
+#endif // __ANALOG_H__
diff --git a/gyro_board/src/usb/main.c b/gyro_board/src/usb/main.c
index 6bf1721..6e69970 100644
--- a/gyro_board/src/usb/main.c
+++ b/gyro_board/src/usb/main.c
@@ -200,8 +200,8 @@
{
portTickType xLastFlashTime;
xLastFlashTime = xTaskGetTickCount();
-
-
+
+
vTaskDelayUntil(&xLastFlashTime, 1000 / portTICK_RATE_MS);
for(;;){
diff --git a/gyro_board/src/usb/quad encoder config.txt b/gyro_board/src/usb/quad encoder config.txt
index 4c0fe4e..6f6ecf5 100644
--- a/gyro_board/src/usb/quad encoder config.txt
+++ b/gyro_board/src/usb/quad encoder config.txt
@@ -1,37 +1,37 @@
-// Quad Encoder stuff
-
-
-// enable the quadrature encode peripheral and peripherial clock
-
- SC->PCONP |=0x00040000; // bit 18 of PCONP set to 1
- SC->PCLKSEL1 |=0x00000001; /* bits 1,0 00 CCLK/4
- 01 CCLK
- 10 CCLK/2
- 11 CCLK/8 */
-
-
-// Enable pins for QEI MCI0, MCI1 (PhA PhB, signals respectively)
-
-
- PINCON->PINSEL3 = ((PINSEL3 &=0xFFFF3DFF) |= 0x00004100); /* 01 in bits 9:8 and 15:14*/
- //PINCON->PINSEL3 = ((PINSEL3 &=0xFFFCFFFF) |= 0x00010000); /* Turns on MCI2 index input 17:16 = 01*/
-
-
-
-// Specify Quadriture phase mode
-
- // QEI->QEICON &= 0xFFFFFFFE; /* Clears position counter */
- QEI->QEICONF &= 0XFFFFFFFD; /* Sets bit 1 to 0, Signal mode to Quad Phase */
- // QEI->QEICON |= 0X00000002; /* Bit 2 to 1 counts both PhA and PhB for higher resolution */
-
-
-
- // QEI->FILTER = ****Whatever Sampling size we want*** /* Sets the number of consecutive pulses for a change in direction, etc to accepted */
-
-
-// Set Various Attributes
-
- // QEI->QEIMAXPOS = *****insert max position*****;
-
-
+// Quad Encoder stuff
+
+
+// enable the quadrature encode peripheral and peripherial clock
+
+ SC->PCONP |=0x00040000; // bit 18 of PCONP set to 1
+ SC->PCLKSEL1 |=0x00000001; /* bits 1,0 00 CCLK/4
+ 01 CCLK
+ 10 CCLK/2
+ 11 CCLK/8 */
+
+
+// Enable pins for QEI MCI0, MCI1 (PhA PhB, signals respectively)
+
+
+ PINCON->PINSEL3 = ((PINSEL3 &=0xFFFF3DFF) |= 0x00004100); /* 01 in bits 9:8 and 15:14*/
+ //PINCON->PINSEL3 = ((PINSEL3 &=0xFFFCFFFF) |= 0x00010000); /* Turns on MCI2 index input 17:16 = 01*/
+
+
+
+// Specify Quadrature phase mode
+
+ // QEI->QEICON &= 0xFFFFFFFE; /* Clears position counter */
+ QEI->QEICONF &= 0XFFFFFFFD; /* Sets bit 1 to 0, Signal mode to Quad Phase */
+ // QEI->QEICON |= 0X00000002; /* Bit 2 to 1 counts both PhA and PhB for higher resolution */
+
+
+
+ // QEI->FILTER = ****Whatever Sampling size we want*** /* Sets the number of consecutive pulses for a change in direction, etc to accepted */
+
+
+// Set Various Attributes
+
+ // QEI->QEIMAXPOS = *****insert max position*****;
+
+
/* Note: QEI->QEIPOS is a read only section that stores the current position */
\ No newline at end of file