merging in the useful parts of the bot3 code
diff --git a/aos/atom_code/output/motor_output.cc b/aos/atom_code/output/motor_output.cc
index 48acc38..e16bbb5 100644
--- a/aos/atom_code/output/motor_output.cc
+++ b/aos/atom_code/output/motor_output.cc
@@ -14,6 +14,8 @@
// going 2 to each side to make sure we get the full range
const MotorOutput::MotorControllerBounds MotorOutput::kTalonBounds
{213, 135, 132, 129, 50};
+const MotorOutput::MotorControllerBounds MotorOutput::kVictorBounds
+ {210, 138, 132, 126, 56};
uint8_t MotorOutput::MotorControllerBounds::Map(double value) const {
if (value == 0.0) return kCenter;
diff --git a/aos/atom_code/output/motor_output.h b/aos/atom_code/output/motor_output.h
index 8828f14..116e791 100644
--- a/aos/atom_code/output/motor_output.h
+++ b/aos/atom_code/output/motor_output.h
@@ -43,6 +43,8 @@
protected:
// Brian got the values here by trying values with hardware on 11/23/12.
static const MotorControllerBounds kTalonBounds;
+ // Taken from WPILib.
+ static const MotorControllerBounds kVictorBounds;
// Helper methods for filling out values_.
// All channels are the 1-indexed numbers that usually go into WPILib.
diff --git a/aos/build/build.sh b/aos/build/build.sh
index 4b36e7e..656e72d 100755
--- a/aos/build/build.sh
+++ b/aos/build/build.sh
@@ -9,10 +9,11 @@
PLATFORM=$1
GYP_MAIN=$2
DEBUG=$3
-ACTION=$4
+OUT_NAME=$4
+ACTION=$5
-shift 3
-shift || true # We might not have a 4th argument if ACTION is empty.
+shift 4
+shift || true # We might not have a 5th argument if ACTION is empty.
export WIND_BASE=${WIND_BASE:-"/usr/local/powerpc-wrs-vxworks/wind_base"}
@@ -21,7 +22,7 @@
AOS=`dirname $0`/..
-OUTDIR=${AOS}/../output/${PLATFORM}
+OUTDIR=${AOS}/../output/${OUT_NAME}
BUILD_NINJA=${OUTDIR}/build.ninja
${AOS}/build/download_externals.sh
@@ -40,7 +41,7 @@
{
'target_defaults': {
'configurations': {
- '${PLATFORM}': {}
+ '${OUT_NAME}': {}
}
}
}
@@ -71,7 +72,7 @@
fi
${NINJA} -C ${OUTDIR} ${NINJA_ACTION} "$@"
if [[ ${ACTION} == deploy || ${ACTION} == redeploy ]]; then
- [ ${PLATFORM} == atom ] && \
+ [[ ${PLATFORM} =~ .*atom ]] && \
rsync --progress -c -r \
${OUTDIR}/outputs/* \
driver@`${AOS}/build/get_ip fitpc`:/home/driver/robot_code/bin
diff --git a/frc971/atom_code/build.sh b/frc971/atom_code/build.sh
index 172b2d4..42229b2 100755
--- a/frc971/atom_code/build.sh
+++ b/frc971/atom_code/build.sh
@@ -2,4 +2,4 @@
cd $(dirname $0)
-../../aos/build/build.sh atom atom_code.gyp no "$@"
+../../aos/build/build.sh atom atom_code.gyp no atom "$@"
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 754ba62..fffbe0d 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -25,8 +25,11 @@
self._namespace_end = '\n'.join(
['} // namespace %s' % name for name in reversed(self._namespaces)])
+ def _TopDirectory(self):
+ return self._namespaces[0]
+
def _HeaderGuard(self, header_file):
- return ('FRC971_CONTROL_LOOPS_' +
+ return (self._TopDirectory().upper() + '_CONTROL_LOOPS_' +
header_file.upper().replace('.', '_').replace('/', '_') +
'_')
@@ -89,7 +92,8 @@
def WriteCC(self, header_file_name, cc_file):
"""Writes the cc file to the file named cc_file."""
with open(cc_file, 'w') as fd:
- fd.write('#include \"frc971/control_loops/%s\"\n' % header_file_name)
+ fd.write('#include \"%s/control_loops/%s\"\n' %
+ (self._TopDirectory(), header_file_name))
fd.write('\n')
fd.write('#include <vector>\n')
fd.write('\n')
diff --git a/frc971/crio/build.sh b/frc971/crio/build.sh
index 9e77e89..7b4f559 100755
--- a/frc971/crio/build.sh
+++ b/frc971/crio/build.sh
@@ -2,4 +2,4 @@
cd $(dirname $0)
-../../aos/build/build.sh crio crio.gyp no "$@"
+../../aos/build/build.sh crio crio.gyp no crio "$@"
diff --git a/frc971/input/usb.gyp b/frc971/input/usb.gyp
new file mode 100644
index 0000000..72e9b6b
--- /dev/null
+++ b/frc971/input/usb.gyp
@@ -0,0 +1,31 @@
+# This file is needed because gyp wants to drag in all of the targets in a file
+# which is a problem because the main and bot3 code contain some executables
+# with the same names.
+{
+ '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/data_struct.h b/gyro_board/src/usb/data_struct.h
index 975084a..0f54fe2 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -129,6 +129,7 @@
};
uint16_t booleans;
};
+ uint32_t shooter_cycle_ticks;
} bot3;
};
};
diff --git a/gyro_board/src/usb/digital.c b/gyro_board/src/usb/digital.c
index c468173..ed8522a 100644
--- a/gyro_board/src/usb/digital.c
+++ b/gyro_board/src/usb/digital.c
@@ -30,5 +30,9 @@
int is_bot3;
void digital_init(void) {
- is_bot3 = 0;
+ 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 c9155d4..b4688a8 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -1,3 +1,5 @@
+#include <string.h>
+
#include "fill_packet.h"
#include "encoder.h"
@@ -11,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 uint32_t kWheelStopThreshold = 250000000;
#define ENC(gpio, a, b) readGPIO(gpio, a) * 2 + readGPIO(gpio, b)
int encoder_bits(int channel) {
@@ -75,6 +78,39 @@
}
}
+static inline void reset_TC(void) {
+ TIM2->TCR = (1 << 1); // Put it into reset and disabled.
+ while (TIM2->TC != 0);
+ TIM2->TCR = 1; // Take it out of reset + make sure it's enabled.
+}
+
+// TIM2
+volatile uint32_t shooter_cycle_ticks;
+void TIMER2_IRQHandler(void) {
+ // Apparently, this handler runs regardless of a match or capture event.
+ if (TIM2->IR & (1 << 4)) {
+ // Capture
+ TIM2->IR = (1 << 4); // Clear the interrupt.
+
+ shooter_cycle_ticks = TIM2->CR0;
+
+ reset_TC();
+ } else if (TIM2->IR & 1) {
+ // Match
+ TIM2->IR = 1; // Clear the interrupt
+
+ // Assume shooter is stopped.
+ shooter_cycle_ticks = 0;
+
+ // Disable timer.
+ TIM2->TCR = 0;
+ }
+
+ // It will only handle one interrupt per run.
+ // If there is another interrupt pending, it won't be cleared, and the ISR
+ // will be run again to handle it.
+}
+
// TODO(brians): Have this indicate some kind of error instead of just looping
// infinitely in the ISR because it never clears it.
static void NoGPIO(void) {}
@@ -288,30 +324,25 @@
capture_shooter_angle_rise = encoder2_val;
}
-// Count leading zeros.
-// Returns 0 if bit 31 is set etc.
-__attribute__((always_inline)) static __INLINE uint32_t __clz(uint32_t value) {
- uint32_t result;
- __asm__("clz %0, %1" : "=r" (result) : "r" (value));
- return result;
+// Third robot shooter.
+static void ShooterPhotoFall(void) {
+ GPIOINT->IO0IntClr = (1 << 4);
+ // We reset TC to make sure we don't get a crap
+ // value from CR0 when the capture interrupt occurs
+ // if the shooter is just starting up again, and so
+ // that the match interrupt thing works right.
+ reset_TC();
}
-inline static void IRQ_Dispatch(void) {
- // There is no need to add a loop here to handle multiple interrupts at the
- // same time because the processor has tail chaining of interrupts which we
- // can't really beat with our own loop.
- // It would actually be bad because a loop here would block EINT1/2 for longer
- // lengths of time.
- uint32_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
- (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
-
- typedef void (*Handler)(void);
- const static Handler table[] = {
+typedef void (*Handler)(void);
+// Contains default pointers for ISR functions.
+// (These can be used without modifications on the comp/practice bots.)
+Handler ISRTable[] = {
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
+ 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
@@ -340,12 +371,31 @@
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]();
+};
+
+// Count leading zeros.
+// Returns 0 if bit 31 is set etc.
+__attribute__((always_inline)) static __INLINE uint32_t __clz(uint32_t value) {
+ uint32_t result;
+ __asm__("clz %0, %1" : "=r" (result) : "r" (value));
+ return result;
+}
+inline static void IRQ_Dispatch(void) {
+ // There is no need to add a loop here to handle multiple interrupts at the
+ // same time because the processor has tail chaining of interrupts which we
+ // can't really beat with our own loop.
+ // It would actually be bad because a loop here would block EINT1/2 for longer
+ // lengths of time.
+
+ uint32_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
+ (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
+
+ ISRTable[index]();
}
void EINT3_IRQHandler(void) {
IRQ_Dispatch();
}
+
int32_t encoder_val(int chan) {
int32_t val;
switch (chan) {
@@ -386,73 +436,92 @@
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 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;
-
- // 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.
- GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 0);
- PINCON->PINSEL4 &= ~(0x3 << 2);
- encoder4_val = 0;
-
- // 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.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 4);
- PINCON->PINSEL4 &= ~(0x3 << 6);
- encoder5_val = 0;
-
// Enable interrupts from the GPIO pins.
NVIC_EnableIRQ(EINT3_IRQn);
if (is_bot3) {
+ // Modify robot handler table for third robot.
+ ISRTable[23] = ShooterPhotoFall;
+
+ // Set up timer for bot3 photosensor.
+ // Make sure timer two is powered.
+ SC->PCONP |= (1 << 22);
+ // Select capture 2.0 function on pin 0.4.
+ PINCON->PINSEL0 |= (0x3 << 8);
+ // Set timer to capture and interrupt on rising edge.
+ TIM2->CCR = 0x5;
+ // Set up match interrupt.
+ TIM2->MR0 = kWheelStopThreshold;
+ TIM2->MCR = 1;
+ // Enable timer IRQ, and make it lower priority than the encoders.
+ NVIC_SetPriority(TIMER2_IRQn, 1);
+ NVIC_EnableIRQ(TIMER2_IRQn);
+ // Set up GPIO interrupt on other edge.
+ 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}
+ // 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;
+
+ // 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.
+ GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL4 &= ~(0x3 << 0);
+ PINCON->PINSEL4 &= ~(0x3 << 2);
+ encoder4_val = 0;
+
+ // 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.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL4 &= ~(0x3 << 4);
+ PINCON->PINSEL4 &= ~(0x3 << 6);
+ encoder5_val = 0;
+
xTaskCreate(vDelayCapture,
(signed char *) "SENSORs",
configMINIMAL_STACK_SIZE + 100,
@@ -502,6 +571,8 @@
if (is_bot3) {
packet->robot_id = 1;
+
+ packet->bot3.shooter_cycle_ticks = shooter_cycle_ticks;
} else { // is main robot
packet->robot_id = 2;