Bugfixes and cleanups for the third robot.
- Fix a bug in the gyro board code that would cause it to
enable the ISR for the wrong timer.
- Fix a bug in the gyro board code that would cause it to
set a falling edge interrupts on the wrong pin.
- Cleanup some junk in gyp files.
- Break usb_receiver stuff into a separate gyp file so it would
build correctly for both robots.
- Fix the dip switch numbering in the gyro board code.
diff --git a/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
index bf5f4ff..bcd5e29 100644
--- a/bot3/autonomous/autonomous.gyp
+++ b/bot3/autonomous/autonomous.gyp
@@ -26,7 +26,6 @@
'<(AOS)/common/common.gyp:controls',
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
diff --git a/bot3/control_loops/drivetrain/drivetrain.gyp b/bot3/control_loops/drivetrain/drivetrain.gyp
index 7cc5191..cc93a9c 100644
--- a/bot3/control_loops/drivetrain/drivetrain.gyp
+++ b/bot3/control_loops/drivetrain/drivetrain.gyp
@@ -27,7 +27,6 @@
'dependencies': [
'drivetrain_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
],
diff --git a/bot3/control_loops/shooter/shooter.gyp b/bot3/control_loops/shooter/shooter.gyp
index 2be0439..f13a00a 100644
--- a/bot3/control_loops/shooter/shooter.gyp
+++ b/bot3/control_loops/shooter/shooter.gyp
@@ -27,7 +27,6 @@
'dependencies': [
'shooter_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
],
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index 0331d24..31863be 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -22,6 +22,9 @@
namespace bot3 {
namespace {
+const double kWheelRadius = 1.964;
+const double kTapeThickness = 2.696;
+
inline double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
(32.0 / 44.0 /*encoder gears*/) * // the encoders are on the wheels.
@@ -30,7 +33,7 @@
inline double shooter_translate(int32_t in) {
return 1.0 / (static_cast<double>(in) / (10 ^ 8)/*ticks per sec*/)
- * (2 * M_PI);
+ / (1 - (kTapeThickness / (2 * kWheelRadius * M_PI))) * (2 * M_PI);
}
inline double gyro_translate(int64_t in) {
@@ -61,7 +64,8 @@
.Send();*/
shooter.position.MakeWithBuilder()
- .velocity(shooter_translate(data()->bot3.shooter_cycle_ticks));
+ .velocity(shooter_translate(data()->bot3.shooter_cycle_ticks))
+ .Send();
}
};
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
index b43fc48..d0a0bd4 100644
--- a/bot3/input/input.gyp
+++ b/bot3/input/input.gyp
@@ -26,30 +26,11 @@
'dependencies': [
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:wrapping_counter',
- 'usb_receiver',
- ],
- },
- {
- 'target_name': 'usb_receiver',
- 'type': 'static_library',
- 'sources': [
- '<(DEPTH)/frc971/input/usb_receiver.cc',
- ],
- 'dependencies': [
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/util/util.gyp:wrapping_counter',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:controls',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/common/util/util.gyp:wrapping_counter',
- '<(AOS)/common/common.gyp:time',
+ '<(DEPTH)/frc971/input/usb.gyp:usb_receiver',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
],
},
],
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index b0bcb20..ef34096 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -37,33 +37,9 @@
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/util/util.gyp:wrapping_counter',
- 'usb_receiver',
+ '<(DEPTH)/frc971/input/usb.gyp:usb_receiver',
'<(DEPTH)/frc971/frc971.gyp:constants',
],
},
- {
- 'target_name': 'usb_receiver',
- 'type': 'static_library',
- 'sources': [
- 'usb_receiver.cc',
- ],
- 'dependencies': [
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:controls',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/common/common.gyp:time',
- ],
- 'variables': {
- # TODO(brians): Add dependency on this file too (or something).
- 'checksum': '<!(<(DEPTH)/gyro_board/src/usb/data_struct_checksum.sh)',
- },
- 'defines': [
- 'GYRO_BOARD_DATA_CHECKSUM=<(checksum)',
- ],
- },
],
}
diff --git a/frc971/input/usb.gyp b/frc971/input/usb.gyp
new file mode 100644
index 0000000..8cf1430
--- /dev/null
+++ b/frc971/input/usb.gyp
@@ -0,0 +1,31 @@
+#This file is needed because gyp is stupid, and if we put the usb_receiver
+#target in the same file as the other input stuff, it screws up the build
+#for the third robot.
+{
+ 'targets': [
+ {
+ 'target_name': 'usb_receiver',
+ 'type': 'static_library',
+ 'sources': [
+ 'usb_receiver.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:controls',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ 'variables': {
+ # TODO(brians): Add dependency on this file too (or something).
+ 'checksum': '<!(<(DEPTH)/gyro_board/src/usb/data_struct_checksum.sh)',
+ },
+ 'defines': [
+ 'GYRO_BOARD_DATA_CHECKSUM=<(checksum)',
+ ],
+ },
+ ],
+}
diff --git a/gyro_board/src/usb/digital.c b/gyro_board/src/usb/digital.c
index 83ef169..ed8522a 100644
--- a/gyro_board/src/usb/digital.c
+++ b/gyro_board/src/usb/digital.c
@@ -30,7 +30,7 @@
int is_bot3;
void digital_init(void) {
- if (dip_switch(0) || dip_switch(1) || dip_switch(2) || dip_switch(3)) {
+ if (dip_switch(1) || dip_switch(2) || dip_switch(3) || dip_switch(4)) {
is_bot3 = 1;
} else {
is_bot3 = 0;
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index c735126..f5ca745 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -13,6 +13,7 @@
// How long (in ms) to wait after a falling edge on the bottom indexer sensor
// before reading the indexer encoder.
static const int kBottomFallDelayTime = 32;
+static const int kWheelStopThreshold = 2.5;
#define ENC(gpio, a, b) readGPIO(gpio, a) * 2 + readGPIO(gpio, b)
int encoder_bits(int channel) {
@@ -434,35 +435,6 @@
static volatile uint32_t sensor_timing_wraps = 0;
void encoder_init(void) {
- // 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;
-
- // 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.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL1 &= ~(0x3 << 12);
- PINCON->PINSEL1 &= ~(0x3 << 10);
- encoder2_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.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL1 &= ~(0x3 << 8);
- PINCON->PINSEL1 &= ~(0x3 << 6);
- encoder3_val = 0;
-
// Enable interrupts from the GPIO pins.
NVIC_EnableIRQ(EINT3_IRQn);
@@ -483,12 +455,21 @@
TIM2->MR0 = kWheelStopThreshold * (10 ^ 8);
TIM2->MCR = 1;
// Enable timer IRQ, and make it lower priority than the encoders.
- NVIC_SetPriority(TIMER3_IRQn, 1);
- NVIC_EnableIRQ(TIMER3_IRQn);
+ NVIC_SetPriority(TIMER2_IRQn, 1);
+ NVIC_EnableIRQ(TIMER2_IRQn);
// Set up GPIO interrupt on other edge.
- GPIOINT->IO0IntEnF |= (1 << 23);
+ GPIOINT->IO0IntEnF |= (1 << 4);
} else { // is main robot
+ // 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;
+
// Set up encoder 1.
// Make GPIOs 2.11 and 2.12 trigger EINT1 and EINT2 (respectively).
// PINSEL4[23:22] = {0 1}
@@ -501,7 +482,27 @@
NVIC_EnableIRQ(EINT1_IRQn);
NVIC_EnableIRQ(EINT2_IRQn);
encoder1_val = 0;
+
+ // 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.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL1 &= ~(0x3 << 12);
+ PINCON->PINSEL1 &= ~(0x3 << 10);
+ encoder2_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.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL1 &= ~(0x3 << 8);
+ PINCON->PINSEL1 &= ~(0x3 << 6);
+ encoder3_val = 0;
+
// Set up encoder 4.
GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
@@ -522,7 +523,6 @@
PINCON->PINSEL4 &= ~(0x3 << 6);
encoder5_val = 0;
-
xTaskCreate(vDelayCapture,
(signed char *) "SENSORs",
configMINIMAL_STACK_SIZE + 100,
@@ -573,8 +573,8 @@
if (is_bot3) {
packet->robot_id = 1;
- packet->main.left_drive = encoder3_val;
- packet->main.right_drive = encoder2_val;
+ //packet->main.left_drive = encoder3_val;
+ //packet->main.right_drive = encoder2_val;
packet->bot3.shooter_cycle_ticks = shooter_cycle_ticks;
} else { // is main robot