Added Parker's initial USB firmware changes to support us moving all sensors off the cRIO.
diff --git a/gyro_board/src/usb/CAN.c b/gyro_board/src/usb/CAN.c
index 74e0a62..6770328 100644
--- a/gyro_board/src/usb/CAN.c
+++ b/gyro_board/src/usb/CAN.c
@@ -202,11 +202,10 @@
xLastFlashTime = xTaskGetTickCount();
for (;;) {
- printf("hello\n");
int c = VCOM_getchar();
while (c != -1) {
- printf("hello\n");
+ printf("hello in write\n");
int j = c;
printf("Sending data 0x%x\n", j);
message.data[0] = j++;
diff --git a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c b/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
index af1eede..2920584 100644
--- a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
+++ b/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
@@ -55,10 +55,10 @@
static const unsigned char abDescriptors[] = {
-// device descriptor
+// Device descriptor
0x12,
DESC_DEVICE,
- LE_WORD(0x0101), // bcdUSB
+ LE_WORD(0x0200), // bcdUSB
0xFF, // bDeviceClass
0x00, // bDeviceSubClass
0x00, // bDeviceProtocol
@@ -67,20 +67,20 @@
LE_WORD(0xd243), // idProduct
LE_WORD(0x0153), // bcdDevice
0x03, // iManufacturer
- 0x36, // iProduct
- 0x33, // iSerialNumber
+ 0x02, // iProduct
+ 0x01, // iSerialNumber
0x01, // bNumConfigurations
-// configuration descriptor
+// Configuration descriptor
0x09,
DESC_CONFIGURATION,
- LE_WORD(67), // wTotalLength
+ LE_WORD(46), // wTotalLength
0x01, // bNumInterfaces
0x01, // bConfigurationValue
0x00, // iConfiguration
0xC0, // bmAttributes
0x32, // bMaxPower
-// data class interface descriptor
+// Data class interface descriptor
0x09,
DESC_INTERFACE,
0x00, // bInterfaceNumber
@@ -90,28 +90,28 @@
0x00, // bInterfaceSubClass
0x00, // bInterfaceProtocol
0x00, // iInterface
-// debug EP OUT
+// Debug EP OUT
0x07,
DESC_ENDPOINT,
BULK_OUT_EP, // bEndpointAddress
0x02, // bmAttributes = bulk
LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
0x00, // bInterval
-// debug EP in
+// Debug EP in
0x07,
DESC_ENDPOINT,
BULK_IN_EP, // bEndpointAddress
0x02, // bmAttributes = bulk
LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
0x00, // bInterval
-// data EP OUT
+// Data EP OUT
0x07,
DESC_ENDPOINT,
INT_OUT_EP, // bEndpointAddress
0x03, // bmAttributes = intr
LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
0x01, // bInterval
-// data EP in
+// Data EP in
0x07,
DESC_ENDPOINT,
INT_IN_EP, // bEndpointAddress
@@ -126,15 +126,15 @@
0x0E,
DESC_STRING,
- 'L', 0, 'P', 0, 'C', 0, 'U', 0, 'S', 0, 'B', 0,
+ '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, 'r', 0, 'i', 0, 'a', 0, 'l', 0,
+ 'U', 0, 'S', 0, 'B', 0, 'S', 0, 'e', 0, 'n', 0, 's', 0, 'o', 0, 'r', 0,
0x12,
DESC_STRING,
- 'A', 0, 'B', 0, 'S', 0, 'M', 0, 'o', 0, 't', 0, 'o', 0, 'r', 0,
+ 'A', 0, 'O', 0, 'S', 0, '_', 0, 'G', 0, 'y', 0, 'r', 0, 'o', 0,
// terminating zero
0
@@ -201,26 +201,29 @@
portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
}
-extern int64_t gyro_angle;
+
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;
- unsigned char buff[16];
- memcpy(buff, &gyro_angle, sizeof(gyro_angle));
- USBHwEPWrite(bEP, buff, sizeof(gyro_angle));
- portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
+ long lHigherPriorityTaskWoken = pdFALSE;
+ fillSensorPacket(&usbPacket);
+ USBHwEPWrite(bEP, (unsigned char *)&usbPacket, sizeof(usbPacket));
+ portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
}
/**
@@ -230,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;
+ }
}
@@ -259,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;
}
@@ -281,11 +284,12 @@
static void USBFrameHandler(unsigned short wFrame) {
(void) wFrame;
-
- if (uxQueueMessagesWaitingFromISR(xCharsForTx) > 0) {
- // data available, enable NAK interrupt on bulk in
- USBHwNakIntEnable(INACK_BI);
- }
+ if(uxQueueMessagesWaitingFromISR(xCharsForTx) > 0){
+ // data available, enable NAK interrupt on bulk in
+ USBHwNakIntEnable(INACK_BI | INACK_II);
+ }else{
+ USBHwNakIntEnable(INACK_BI);
+ }
}
void vUSBTask(void *pvParameters) {
diff --git a/gyro_board/src/usb/Makefile b/gyro_board/src/usb/Makefile
index a87936e..7b20a72 100644
--- a/gyro_board/src/usb/Makefile
+++ b/gyro_board/src/usb/Makefile
@@ -6,15 +6,16 @@
GCC_PATH=/usr/local/cortex-m3/bin
-PORT=/dev/ttyUSB0
+PORT=/dev/ttyS1
CC=$(GCC_PATH)/arm-eabi-gcc
LD=$(GCC_PATH)/arm-eabi-ld
OBJCOPY=$(GCC_PATH)/arm-eabi-objcopy
+OBJDUMP=$(GCC_PATH)/arm-eabi-objdump
AS=$(GCC_PATH)/arm-eabi-as
FLASHER=lpc21isp
-CFLAGS=-I. -I./FreeRTOS/include -I./FreeRTOS/portable/GCC/ARM_CM3/ -I./CommonDemoTasks/include -Os -mcpu=cortex-m3 -mthumb -Wl,--gc-sections -ffunction-sections -Wl,-static -Werror
+CFLAGS=-I. -I./FreeRTOS/include -I./FreeRTOS/portable/GCC/ARM_CM3/ -I./CommonDemoTasks/include -O3 -mcpu=cortex-m3 -mthumb -Wl,--gc-sections -ffunction-sections -Wl,-static -Werror
SPEED=38400
OSC=12000
@@ -56,6 +57,9 @@
cat:
@cd ../../bin; python serial_looper.py
+assm.S: $(NAME).elf
+ $(OBJDUMP) -D -S $(NAME).elf > assm.S
+
%.hex: %.elf Makefile
$(OBJCOPY) -O ihex $< $@
diff --git a/gyro_board/src/usb/analog.c b/gyro_board/src/usb/analog.c
index 9e67dd5..35c7fd3 100644
--- a/gyro_board/src/usb/analog.c
+++ b/gyro_board/src/usb/analog.c
@@ -7,6 +7,12 @@
// ****************************************************************************
#include "FreeRTOS.h"
+/* Scheduler includes. */
+#include "FreeRTOS.h"
+#include "queue.h"
+#include "task.h"
+
+#include "analog.h"
void analog_init (void)
{
@@ -145,9 +151,518 @@
}
volatile int32_t encoder1_val;
+volatile int32_t encoder2_val;
+volatile int32_t encoder3_val;
+volatile int32_t encoder4_val;
+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;
+}
+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 --;
+ }
+}
+
+void Encoder3ARise(){
+ GPIOINT->IO0IntClr |= (1<<20);
+ if(GPIO0->FIOPIN & (1<<19)){
+ encoder3_val ++;
+ }else{
+ encoder3_val --;
+ }
+}
+void Encoder3AFall(){
+ GPIOINT->IO0IntClr |= (1<<20);
+ if(GPIO0->FIOPIN & (1<<19)){
+ encoder3_val --;
+ }else{
+ encoder3_val ++;
+ }
+}
+void Encoder3BRise(){
+ GPIOINT->IO0IntClr |= (1<<19);
+ if(GPIO0->FIOPIN & (1<<20)){
+ encoder3_val --;
+ }else{
+ encoder3_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 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 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 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;
+}
+volatile int8_t 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;
+volatile int32_t dirty_delay;
+portTickType xDelayTimeFrom;
+static portTASK_FUNCTION(vDelayCapture, pvParameters)
+{
+ portTickType xSleepFrom;
+ xSleepFrom = xTaskGetTickCount();
+
+ for(;;){
+ NVIC_DisableIRQ(EINT3_IRQn);
+ if(dirty_delay){
+ xSleepFrom = xDelayTimeFrom;
+ dirty_delay = 0;
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+ 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);
+ }
+ }
+}
+
+
+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;
+}
+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;
+}
+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;
+}
+typedef void (*PFUNC)(void);
+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]();
+
+
+/*
+ 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 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 4: //NO GPIO #bit 27
+ NoGPIO();
+ 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 7: //P0.20 Fall #bit 24 //Encoder 3 A
+ Encoder3AFall();
+ 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 10: //P0.21 Rise #bit 21 //Encoder 2 B
+ Encoder2BRise();
+ 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 13: //NO GPIO #bit 18
+ NoGPIO();
+ return;
+
+ case 14: //NO GPIO #bit 17
+ NoGPIO();
+ return;
+
+ case 15: //NO GPIO #bit 16
+ NoGPIO();
+ return;
+
+ case 16: //NO GPIO #bit 15
+ NoGPIO();
+ return;
+
+ case 17: //NO GPIO #bit 14
+ NoGPIO();
+ return;
+
+ case 18: //NO GPIO #bit 13
+ NoGPIO();
+ return;
+
+ case 19: //NO GPIO #bit 12
+ NoGPIO();
+ return;
+
+ case 20: //NO GPIO #bit 11
+ NoGPIO();
+ return;
+
+ case 21: //NO GPIO #bit 10
+ NoGPIO();
+ 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 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 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 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 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 32: //NO BITS SET #False Alarm
+ NoGPIO();
+ return;
+ }
+ */
+}
+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;
+ }
+}
+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;
+
+ packet->capture_top_rise = capture_top_rise;
+ packet->top_rise_count = top_rise_count;
+
+ packet->capture_top_fall = capture_top_fall;
+ packet->top_fall_count = top_fall_count;
+
+ packet->bottom_rise_count = bottom_rise_count;
+
+ 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);
+}
+
void encoder_init(void)
{
-// port 0
+ // port 0
// Setup the encoder interface.
SC->PCONP |= PCONP_PCQEI;
PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
@@ -159,9 +674,6 @@
// Wrap back to 0 when we wrap the int...
QEI->QEIMAXPOS = 0xffffffff;
// port 1
- NVIC_EnableIRQ(EINT1_IRQn);
- NVIC_EnableIRQ(EINT2_IRQn);
- //NVIC_EnableIRQ(EINT3_IRQn);
//PINSEL4 23/22 0 1
//PINSEL4 25 24 0 1
PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
@@ -170,51 +682,71 @@
//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;
-//ports 2 and 3
+
+//port 2
+
+ 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);
+
+//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);
+
+//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);
+
+//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);
+
+
+//gpio pins:
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+
+// 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);
+
}
-void EINT1_IRQHandler(void){
- //ENC1A 2.11
- 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;
- SC->EXTINT = 0x2;
-}
-void EINT2_IRQHandler(void){
- //ENC1B 2.12
- 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;
- SC->EXTINT = 0x4;
-}
-void EINT3_IRQHandler(void){
-
-}
-int32_t encoder_val(int chan)
-{
- switch(chan){
- case 0:
- return (int32_t)QEI->QEIPOS;
- case 1:
- return encoder1_val;
- case 2:
- case 3:
- default:
- return -1;
- }
-}
-
diff --git a/gyro_board/src/usb/analog.h b/gyro_board/src/usb/analog.h
index e9704f6..7026af0 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -1,6 +1,39 @@
#ifndef __ANALOG_H__
#define __ANALOG_H__
+extern int64_t gyro_angle;
+struct DataStruct{
+ int64_t gyro_angle;
+
+ int32_t right_drive;
+ int32_t left_drive;
+ int32_t shooter_angle;
+ int32_t shooter;
+ int32_t indexer;
+ int32_t wrist;
+
+ int32_t capture_top_rise;
+ int32_t capture_top_fall;
+ int32_t capture_bottom_fall_delay;
+ int32_t capture_wrist_rise;
+ int32_t capture_shooter_angle_rise;
+
+ int8_t top_rise_count;
+
+ int8_t top_fall_count;
+
+ int8_t bottom_rise_count;
+
+ int8_t bottom_fall_delay_count;
+ int8_t bottom_fall_count;
+
+ int8_t wrist_rise_count;
+
+ int8_t shooter_angle_rise_count;
+}__attribute__((__packed__));
+void fillSensorPacket(struct DataStruct *packet);
+
+
void analog_init (void);
int analog(int chan);
int digital(int chan);
diff --git a/gyro_board/src/usb/main.c b/gyro_board/src/usb/main.c
index 11191a4..6bf1721 100644
--- a/gyro_board/src/usb/main.c
+++ b/gyro_board/src/usb/main.c
@@ -196,25 +196,32 @@
}
}
-#include "CAN.h"
-
-
-
-void motor(int32_t speed)
+static portTASK_FUNCTION(vSensorPoll, pvParameters)
{
- if (speed > 2047) speed = 2047;
- if (speed < -2047) speed = -2047;
- return;
- if (speed > 0) {
- MCPWM->MCMAT1 = 2047 - speed;
- MCPWM->MCMAT0 = 2048;
- } else {
- MCPWM->MCMAT1 = 2048;
- MCPWM->MCMAT0 = speed + 2047;
+ portTickType xLastFlashTime;
+ xLastFlashTime = xTaskGetTickCount();
+
+
+ vTaskDelayUntil(&xLastFlashTime, 1000 / portTICK_RATE_MS);
+
+ for(;;){
+
+ vTaskDelayUntil(&xLastFlashTime, 20 / portTICK_RATE_MS);
+ /*
+ printf("not realtime! e0:%d e1:%d e2:%d e3:%d e4:%d e5:%d angle:%d \n", (int)encoder_val(0),
+ (int)encoder_val(1),
+ (int)encoder_val(2),
+ (int)encoder_val(3),
+ (int)encoder_val(4),
+ (int)encoder_val(5),
+ (int)gyro_angle);
+*/
}
}
+#include "CAN.h"
+
/*-----------------------------------------------------------*/
@@ -232,6 +239,9 @@
xTaskCreate(vPrintPeriodic, (signed char *) "PRINTx", configMINIMAL_STACK_SIZE + 100, NULL, tskIDLE_PRIORITY + 2, NULL);
+
+ xTaskCreate(vSensorPoll, (signed char *) "SENSORs", configMINIMAL_STACK_SIZE + 100, NULL, tskIDLE_PRIORITY + 1, NULL);
+
initCAN();
// Start the scheduler.
diff --git a/gyro_board/src/usb_driver/90-aschuh.rules b/gyro_board/src/usb_driver/90-aschuh.rules
index 7e7699e..f4e07de 100644
--- a/gyro_board/src/usb_driver/90-aschuh.rules
+++ b/gyro_board/src/usb_driver/90-aschuh.rules
@@ -1 +1 @@
-KERNEL=="aschuh[0-9]*",GROUP="dialout"
+SUBSYSTEM=="usb", ACTION=="add", ATTR{idVendor}=="d243", ATTR{idProduct}=="1424", GROUP="dialout"