<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
- <name>VEX_1516</name>
+ <name>New</name>
<comment></comment>
<projects>
</projects>
+++ /dev/null
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?>
-
-<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
- <storageModule moduleId="org.eclipse.cdt.core.settings">
- <cconfiguration id="cdt.managedbuild.toolchain.gnu.cross.base.1066004663">
- <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.cross.base.1066004663" moduleId="org.eclipse.cdt.core.settings" name="Default">
- <externalSettings/>
- <extensions>
- <extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
- <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- </extensions>
- </storageModule>
- <storageModule moduleId="cdtBuildSystem" version="4.0.0">
- <configuration buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.cross.base.1066004663" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
- <folderInfo id="cdt.managedbuild.toolchain.gnu.cross.base.1066004663.216315519" name="/" resourcePath="">
- <toolChain id="cdt.managedbuild.toolchain.gnu.cross.base.375388531" name="cdt.managedbuild.toolchain.gnu.cross.base" resourceTypeBasedDiscovery="true" superClass="cdt.managedbuild.toolchain.gnu.cross.base">
- <option id="cdt.managedbuild.option.gnu.cross.prefix.599893882" name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix"/>
- <option id="cdt.managedbuild.option.gnu.cross.path.1759346474" name="Path" superClass="cdt.managedbuild.option.gnu.cross.path"/>
- <targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.targetPlatform.gnu.cross.2134707772" isAbstract="false" osList="all" superClass="cdt.managedbuild.targetPlatform.gnu.cross"/>
- <builder id="cdt.managedbuild.builder.gnu.cross.779610124" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.builder.gnu.cross"/>
- <tool id="cdt.managedbuild.tool.gnu.cross.c.compiler.1492636040" name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler">
- <inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1080172599" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.1535676614" name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler"/>
- <tool id="cdt.managedbuild.tool.gnu.cross.c.linker.666130241" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker">
- <inputType id="cdt.managedbuild.tool.gnu.c.linker.input.1115410559" superClass="cdt.managedbuild.tool.gnu.c.linker.input">
- <additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
- <additionalInput kind="additionalinput" paths="$(LIBS)"/>
- </inputType>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.cross.cpp.linker.274329887" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker"/>
- </toolChain>
- </folderInfo>
- </configuration>
- </storageModule>
- <storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
- </cconfiguration>
- </storageModule>
- <storageModule moduleId="scannerConfiguration">
- <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
- <scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.1066004663;cdt.managedbuild.toolchain.gnu.cross.base.1066004663.216315519;cdt.managedbuild.tool.gnu.cross.c.compiler.1492636040;cdt.managedbuild.tool.gnu.c.compiler.input.1080172599">
- <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"/>
- <profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
- <buildOutputProvider>
- <openAction enabled="true" filePath=""/>
- <parser enabled="false"/>
- </buildOutputProvider>
- <scannerInfoProvider id="specsFile">
- <runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c -mthumb -mcpu=cortex-m3 -mlittle-endian" command="arm-none-eabi-gcc" useDefault="true"/>
- <parser enabled="true"/>
- </scannerInfoProvider>
- </profile>
- </scannerConfigBuildInfo>
- <scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.1066004663">
- <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"/>
- <profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
- <buildOutputProvider>
- <openAction enabled="true" filePath=""/>
- <parser enabled="false"/>
- </buildOutputProvider>
- <scannerInfoProvider id="specsFile">
- <runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c -mthumb -mcpu=cortex-m3 -mlittle-endian" command="arm-none-eabi-gcc" useDefault="true"/>
- <parser enabled="true"/>
- </scannerInfoProvider>
- </profile>
- </scannerConfigBuildInfo>
- </storageModule>
- <storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
- <buildTargets>
- <target name="upload" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
- <buildCommand>make</buildCommand>
- <buildArguments/>
- <buildTarget>upload</buildTarget>
- <stopOnError>false</stopOnError>
- <useDefaultCommand>true</useDefaultCommand>
- <runAllBuilders>false</runAllBuilders>
- </target>
- </buildTargets>
- </storageModule>
-</cproject>
+++ /dev/null
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
- <name>Shooter Testing</name>
- <comment></comment>
- <projects>
- </projects>
- <buildSpec>
- <buildCommand>
- <name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
- <triggers>clean,full,incremental,</triggers>
- <arguments>
- <dictionary>
- <key>?name?</key>
- <value></value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.append_environment</key>
- <value>true</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.autoBuildTarget</key>
- <value>all</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.buildArguments</key>
- <value></value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.buildCommand</key>
- <value>make</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
- <value>clean</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.contents</key>
- <value>org.eclipse.cdt.make.core.activeConfigSettings</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.enableAutoBuild</key>
- <value>false</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.enableCleanBuild</key>
- <value>true</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.enableFullBuild</key>
- <value>true</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.fullBuildTarget</key>
- <value>all</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.stopOnError</key>
- <value>true</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
- <value>true</value>
- </dictionary>
- </arguments>
- </buildCommand>
- <buildCommand>
- <name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
- <triggers>full,incremental,</triggers>
- <arguments>
- </arguments>
- </buildCommand>
- </buildSpec>
- <natures>
- <nature>org.eclipse.cdt.core.cnature</nature>
- <nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
- <nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
- </natures>
-</projectDescription>
+++ /dev/null
-# Universal C Makefile for MCU targets\r
-\r
-# Path to project root (for top-level, so the project is in ./; first-level, ../; etc.)\r
-ROOT=.\r
-# Binary output directory\r
-BINDIR=$(ROOT)/bin\r
-# Subdirectories to include in the build\r
-SUBDIRS=src\r
-\r
-# Nothing below here needs to be modified by typical users\r
-\r
-# Include common aspects of this project\r
--include $(ROOT)/common.mk\r
-\r
-ASMSRC:=$(wildcard *.$(ASMEXT))\r
-ASMOBJ:=$(patsubst %.o,$(BINDIR)/%.o,$(ASMSRC:.$(ASMEXT)=.o))\r
-HEADERS:=$(wildcard *.$(HEXT))\r
-CSRC=$(wildcard *.$(CEXT))\r
-COBJ:=$(patsubst %.o,$(BINDIR)/%.o,$(CSRC:.$(CEXT)=.o))\r
-CPPSRC:=$(wildcard *.$(CPPEXT))\r
-CPPOBJ:=$(patsubst %.o,$(BINDIR)/%.o,$(CPPSRC:.$(CPPEXT)=.o))\r
-OUT:=$(BINDIR)/$(OUTNAME)\r
-\r
-.PHONY: all clean upload _force_look\r
-\r
-# By default, compile program\r
-all: $(BINDIR) $(OUT)\r
-\r
-# Remove all intermediate object files (remove the binary directory)\r
-clean:\r
- -rm -f $(OUT)\r
- -rm -rf $(BINDIR)\r
-\r
-# Uploads program to device\r
-upload: all\r
- $(UPLOAD)\r
-\r
-# Phony force-look target\r
-_force_look:\r
- @true\r
-\r
-# Looks in subdirectories for things to make\r
-$(SUBDIRS): %: _force_look\r
- @$(MAKE) --no-print-directory -C $@\r
-\r
-# Ensure binary directory exists\r
-$(BINDIR):\r
- -@mkdir -p $(BINDIR)\r
-\r
-# Compile program\r
-$(OUT): $(SUBDIRS) $(ASMOBJ) $(COBJ) $(CPPOBJ)\r
- @echo LN $(BINDIR)/*.o $(LIBRARIES) to $@\r
- @$(CC) $(LDFLAGS) $(BINDIR)/*.o $(LIBRARIES) -o $@\r
- @$(MCUPREFIX)size $(SIZEFLAGS) $(OUT)\r
- $(MCUPREPARE)\r
-\r
-# Assembly source file management\r
-$(ASMOBJ): $(BINDIR)/%.o: %.$(ASMEXT) $(HEADERS)\r
- @echo AS $<\r
- @$(AS) $(AFLAGS) -o $@ $<\r
-\r
-# Object management\r
-$(COBJ): $(BINDIR)/%.o: %.$(CEXT) $(HEADERS)\r
- @echo CC $(INCLUDE) $<\r
- @$(CC) $(INCLUDE) $(CFLAGS) -o $@ $<\r
-\r
-$(CPPOBJ): $(BINDIR)/%.o: %.$(CPPEXT) $(HEADERS)\r
- @echo CPC $(INCLUDE) $<\r
- @$(CPPCC) $(INCLUDE) $(CPPFLAGS) -o $@ $<\r
+++ /dev/null
-# Universal C Makefile for MCU targets\r
-# Top-level template file to configure build\r
-\r
-# Makefile for IFI VeX Cortex Microcontroller (STM32F103VD series)\r
-DEVICE=VexCortex\r
-# Libraries to include in the link (use -L and -l) e.g. -lm, -lmyLib\r
-LIBRARIES=$(ROOT)/firmware/libccos.a -lgcc -lm\r
-# Prefix for ARM tools (must be on the path)\r
-MCUPREFIX=arm-none-eabi-\r
-# Flags for the assembler\r
-MCUAFLAGS=-mthumb -mcpu=cortex-m3 -mlittle-endian\r
-# Flags for the compiler\r
-MCUCFLAGS=-mthumb -mcpu=cortex-m3 -mlittle-endian\r
-# Flags for the linker\r
-MCULFLAGS=-nostartfiles -Wl,-static -Bfirmware -Wl,-u,VectorTable -Wl,-T -Xlinker firmware/cortex.ld\r
-# Prepares the elf file by converting it to a binary that java can write\r
-MCUPREPARE=$(OBJCOPY) $(OUT) -O binary $(BINDIR)/$(OUTBIN)\r
-# Advanced sizing flags\r
-SIZEFLAGS=\r
-# Uploads program using java\r
-UPLOAD=@java -jar firmware/uniflash.jar vex $(BINDIR)/$(OUTBIN)\r
-\r
-# Advanced options\r
-ASMEXT=s\r
-CEXT=c\r
-CPPEXT=cpp\r
-HEXT=h\r
-INCLUDE=-I$(ROOT)/include -I$(ROOT)/src\r
-OUTBIN=output.bin\r
-OUTNAME=output.elf\r
-\r
-# Flags for programs\r
-AFLAGS:=$(MCUAFLAGS)\r
-ARFLAGS:=$(MCUCFLAGS)\r
-CCFLAGS:=-c -Wall $(MCUCFLAGS) -Os -ffunction-sections -fsigned-char -fomit-frame-pointer -fsingle-precision-constant\r
-CFLAGS:=$(CCFLAGS) -std=gnu99 -Werror=implicit-function-declaration\r
-CPPFLAGS:=$(CCFLAGS) -fno-exceptions -fno-rtti -felide-constructors\r
-LDFLAGS:=-Wall $(MCUCFLAGS) $(MCULFLAGS) -Wl,--gc-sections\r
-\r
-# Tools used in program\r
-AR:=$(MCUPREFIX)ar\r
-AS:=$(MCUPREFIX)as\r
-CC:=$(MCUPREFIX)gcc\r
-CPPCC:=$(MCUPREFIX)g++\r
-OBJCOPY:=$(MCUPREFIX)objcopy\r
+++ /dev/null
-/* Memory space definitions */\r
-MEMORY {\r
- RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 64K\r
- FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 384K\r
-}\r
-\r
-/* Higher address of the user mode stack */\r
-EXTERN ( _estack );\r
-PROVIDE ( _estack = ORIGIN(RAM) + LENGTH(RAM) );\r
-\r
-/* This sends all unreferenced IRQHandlers to reset. */\r
-PROVIDE ( ISR_SWI = 0 );\r
-PROVIDE ( ISR_IRQ = 0 );\r
-PROVIDE ( ISR_Prefetch = 0 );\r
-PROVIDE ( ISR_Abort = 0 );\r
-PROVIDE ( ISR_FIQ = 0 );\r
-\r
-PROVIDE ( ISR_NMI = 0 );\r
-PROVIDE ( ISR_HardFault = 0 );\r
-PROVIDE ( ISR_MemManage = 0 );\r
-PROVIDE ( ISR_BusFault = 0 );\r
-PROVIDE ( ISR_UsageFault = 0 );\r
-\r
-PROVIDE ( ISR_SVC = 0 );\r
-PROVIDE ( ISR_DebugMon = 0 );\r
-PROVIDE ( ISR_PendSV = 0 );\r
-PROVIDE ( ISR_SysTick = 0 );\r
-\r
-PROVIDE ( ISR_WWDG = 0 );\r
-PROVIDE ( ISR_PVD = 0 );\r
-PROVIDE ( ISR_TAMPER = 0 );\r
-PROVIDE ( ISR_RTC = 0 );\r
-PROVIDE ( ISR_FLASH = 0 );\r
-PROVIDE ( ISR_RCC = 0 );\r
-PROVIDE ( ISR_EXTI0 = 0 );\r
-PROVIDE ( ISR_EXTI1 = 0 );\r
-PROVIDE ( ISR_EXTI2 = 0 );\r
-PROVIDE ( ISR_EXTI3 = 0 );\r
-PROVIDE ( ISR_EXTI4 = 0 );\r
-PROVIDE ( ISR_DMAChannel1 = 0 );\r
-PROVIDE ( ISR_DMAChannel2 = 0 );\r
-PROVIDE ( ISR_DMAChannel3 = 0 );\r
-PROVIDE ( ISR_DMAChannel4 = 0 );\r
-PROVIDE ( ISR_DMAChannel5 = 0 );\r
-PROVIDE ( ISR_DMAChannel6 = 0 );\r
-PROVIDE ( ISR_DMAChannel7 = 0 );\r
-PROVIDE ( ISR_DMA1_Channel1 = 0 );\r
-PROVIDE ( ISR_DMA1_Channel2 = 0 );\r
-PROVIDE ( ISR_DMA1_Channel3 = 0 );\r
-PROVIDE ( ISR_DMA1_Channel4 = 0 );\r
-PROVIDE ( ISR_DMA1_Channel5 = 0 );\r
-PROVIDE ( ISR_DMA1_Channel6 = 0 );\r
-PROVIDE ( ISR_DMA1_Channel7 = 0 );\r
-PROVIDE ( ISR_ADC = 0 );\r
-PROVIDE ( ISR_ADC1_2 = 0 );\r
-PROVIDE ( ISR_USB_HP_CAN_TX = 0 );\r
-PROVIDE ( ISR_USB_HP_CAN1_TX = 0 );\r
-PROVIDE ( ISR_USB_LP_CAN_RX0 = 0 );\r
-PROVIDE ( ISR_USB_LP_CAN1_RX0 = 0 );\r
-PROVIDE ( ISR_CAN_RX1 = 0 );\r
-PROVIDE ( ISR_CAN1_RX1 = 0 );\r
-PROVIDE ( ISR_CAN_SCE = 0 );\r
-PROVIDE ( ISR_CAN1_SCE = 0 );\r
-PROVIDE ( ISR_EXTI9_5 = 0 );\r
-PROVIDE ( ISR_TIM1_BRK = 0 );\r
-PROVIDE ( ISR_TIM1_UP = 0 );\r
-PROVIDE ( ISR_TIM1_TRG_COM = 0 );\r
-PROVIDE ( ISR_TIM1_CC = 0 );\r
-PROVIDE ( ISR_TIM2 = 0 );\r
-PROVIDE ( ISR_TIM3 = 0 );\r
-PROVIDE ( ISR_TIM4 = 0 );\r
-PROVIDE ( ISR_I2C1_EV = 0 );\r
-PROVIDE ( ISR_I2C1_ER = 0 );\r
-PROVIDE ( ISR_I2C2_EV = 0 );\r
-PROVIDE ( ISR_I2C2_ER = 0 );\r
-PROVIDE ( ISR_SPI1 = 0 );\r
-PROVIDE ( ISR_SPI2 = 0 );\r
-PROVIDE ( ISR_USART1 = 0 );\r
-PROVIDE ( ISR_USART2 = 0 );\r
-PROVIDE ( ISR_USART3 = 0 );\r
-PROVIDE ( ISR_EXTI15_10 = 0 );\r
-PROVIDE ( ISR_RTCAlarm = 0 );\r
-PROVIDE ( ISR_USBWakeUp = 0 );\r
-PROVIDE ( ISR_TIM8_BRK = 0 );\r
-PROVIDE ( ISR_TIM8_UP = 0 );\r
-PROVIDE ( ISR_TIM8_TRG_COM = 0 );\r
-PROVIDE ( ISR_TIM8_CC = 0 );\r
-PROVIDE ( ISR_ADC3 = 0 );\r
-PROVIDE ( ISR_FSMC = 0 );\r
-PROVIDE ( ISR_SDIO = 0 );\r
-PROVIDE ( ISR_TIM5 = 0 );\r
-PROVIDE ( ISR_SPI3 = 0 );\r
-PROVIDE ( ISR_UART4 = 0 );\r
-PROVIDE ( ISR_UART5 = 0 );\r
-PROVIDE ( ISR_TIM6 = 0 );\r
-PROVIDE ( ISR_TIM7 = 0 );\r
-PROVIDE ( ISR_DMA2_Channel1 = 0 );\r
-PROVIDE ( ISR_DMA2_Channel2 = 0 );\r
-PROVIDE ( ISR_DMA2_Channel3 = 0 );\r
-PROVIDE ( ISR_DMA2_Channel4_5 = 0 );\r
+++ /dev/null
-/* Include STM32F10X linker scripts */\r
-INCLUDE "STM32F10x.ld"\r
-\r
-EXTERN ( _heapbegin );\r
-\r
-/* Section definitions for Cortex firmware flashing */\r
-SECTIONS {\r
- .isr_vector : {\r
- /* startup code, prevents garbage collection from eating everything */\r
- KEEP(*(.isr_vector))\r
- . = ALIGN(4);\r
- } >FLASH\r
- .text : {\r
- . = ALIGN(4);\r
- *(.text)\r
- *(.text.*)\r
- /* Preinit array of functions */\r
- __preinit_array_start = .;\r
- KEEP (*(.preinit_array))\r
- __preinit_array_end = .;\r
- . = ALIGN(4);\r
- /* Init array of functions */\r
- KEEP(*(.init))\r
- . = ALIGN(4);\r
- __init_array_start = .;\r
- KEEP (*(SORT(.init_array.*)))\r
- KEEP (*(.init_array))\r
- __init_array_end = .;\r
- . = ALIGN(4);\r
- /* C++ constructors */\r
- KEEP (*crtbegin.o(.ctors))\r
- KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))\r
- KEEP (*(SORT(.ctors.*)))\r
- KEEP (*crtend.o(.ctors))\r
- . = ALIGN(4);\r
- /* Finalizer array of functions */\r
- KEEP(*(.fini))\r
- . = ALIGN(4);\r
- __fini_array_start = .;\r
- KEEP (*(.fini_array))\r
- KEEP (*(SORT(.fini_array.*)))\r
- __fini_array_end = .;\r
- . = ALIGN(4);\r
- /* C++ destructors */\r
- KEEP (*crtbegin.o(.dtors))\r
- KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))\r
- KEEP (*(SORT(.dtors.*)))\r
- KEEP (*crtend.o(.dtors))\r
- _etext = .;\r
- _srdata = .;\r
- . = ALIGN(4);\r
- *(.rodata)\r
- *(.rodata*)\r
- *(.glue_7)\r
- *(.glue_7t)\r
- . = ALIGN(4);\r
- _erdata = .;\r
- _sidata = .;\r
- } >FLASH\r
- /* The program executes knowing that the data is in the RAM, but the loader puts the\r
- * initial values in the FLASH (inidata). The startup copies the initial values over */\r
- .data : AT ( _sidata ) {\r
- . = ALIGN(4);\r
- _sdata = .;\r
- *(.data)\r
- *(.data.*)\r
- *(.RAMtext)\r
- . = ALIGN(4);\r
- _edata = .;\r
- } >RAM\r
- /* Uninitialized data (zero-fill) section */\r
- .bss : {\r
- . = ALIGN(4);\r
- _sbss = .;\r
- *(.bss)\r
- *(COMMON)\r
- . = ALIGN(4);\r
- _ebss = .;\r
- . = ALIGN(8);\r
- _heapbegin = .;\r
- } >RAM\r
-\r
- /DISCARD/ : {\r
- libc.a ( * )\r
- libg.a ( * )\r
- libm.a ( * )\r
- libgcc.a ( * )\r
- libstdc++.a ( * )\r
- libsupc++.a ( * )\r
- }\r
- /* ARM exception unwinding, mandated by ARM EABI C++ standard (with -fno-exceptions?) */\r
- .ARM.exidx 0 : { *(.ARM.exidx*) }\r
- /* Stabs debugging sections */\r
- .stab 0 : { *(.stab) }\r
- .stabstr 0 : { *(.stabstr) }\r
- .stab.excl 0 : { *(.stab.excl) }\r
- .stab.exclstr 0 : { *(.stab.exclstr) }\r
- .stab.index 0 : { *(.stab.index) }\r
- .stab.indexstr 0 : { *(.stab.indexstr) }\r
- .comment 0 : { *(.comment) }\r
- /* DWARF 1 */\r
- .debug 0 : { *(.debug) }\r
- .line 0 : { *(.line) }\r
- /* GNU DWARF 1 extensions */\r
- .debug_srcinfo 0 : { *(.debug_srcinfo) }\r
- .debug_sfnames 0 : { *(.debug_sfnames) }\r
- /* DWARF 1.1 and DWARF 2 */\r
- .debug_aranges 0 : { *(.debug_aranges) }\r
- .debug_pubnames 0 : { *(.debug_pubnames) }\r
- /* DWARF 2 */\r
- .debug_info 0 : { *(.debug_info .gnu.linkonce.wi.*) }\r
- .debug_abbrev 0 : { *(.debug_abbrev) }\r
- .debug_line 0 : { *(.debug_line) }\r
- .debug_frame 0 : { *(.debug_frame) }\r
- .debug_str 0 : { *(.debug_str) }\r
- .debug_loc 0 : { *(.debug_loc) }\r
- .debug_macinfo 0 : { *(.debug_macinfo) }\r
- /* SGI/MIPS DWARF 2 extensions */\r
- .debug_weaknames 0 : { *(.debug_weaknames) }\r
- .debug_funcnames 0 : { *(.debug_funcnames) }\r
- .debug_typenames 0 : { *(.debug_typenames) }\r
- .debug_varnames 0 : { *(.debug_varnames) }\r
-}\r
+++ /dev/null
-/** @file API.h\r
- * @brief Provides the high-level user functionality intended for use by typical VEX Cortex\r
- * programmers.\r
- *\r
- * This file should be included for you in the predefined stubs in each new VEX Cortex PROS\r
- * project through the inclusion of "main.h". In any new C source file, it is advisable to\r
- * include main.h instead of referencing API.h by name, to better handle any nomenclature\r
- * changes to this file or its contents.\r
- *\r
- * Copyright (c) 2011-2014, Purdue University ACM SIG BOTS.\r
- * All rights reserved.\r
- *\r
- * Redistribution and use in source and binary forms, with or without\r
- * modification, are permitted provided that the following conditions are met:\r
- * * Redistributions of source code must retain the above copyright\r
- * notice, this list of conditions and the following disclaimer.\r
- * * Redistributions in binary form must reproduce the above copyright\r
- * notice, this list of conditions and the following disclaimer in the\r
- * documentation and/or other materials provided with the distribution.\r
- * * Neither the name of Purdue University ACM SIG BOTS nor the\r
- * names of its contributors may be used to endorse or promote products\r
- * derived from this software without specific prior written permission.\r
- *\r
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND\r
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\r
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\r
- * DISCLAIMED. IN NO EVENT SHALL PURDUE UNIVERSITY ACM SIG BOTS BE LIABLE FOR ANY\r
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\r
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\r
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\r
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\r
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
- *\r
- * Purdue Robotics OS contains FreeRTOS (http://www.freertos.org) whose source code may be\r
- * obtained from http://sourceforge.net/projects/freertos/files/ or on request.\r
- */\r
-\r
-#ifndef API_H_\r
-#define API_H_\r
-\r
-// System includes\r
-#include <stdlib.h>\r
-#include <stdbool.h>\r
-#include <stdarg.h>\r
-\r
-// Begin C++ extern to C\r
-#ifdef __cplusplus\r
-extern "C" {\r
-#endif\r
-\r
-// -------------------- VEX competition functions --------------------\r
-\r
-/**\r
- * DOWN button (valid on channels 5, 6, 7, 8)\r
- */\r
-#define JOY_DOWN 1\r
-/**\r
- * LEFT button (valid on channels 7, 8)\r
- */\r
-#define JOY_LEFT 2\r
-/**\r
- * UP button (valid on channels 5, 6, 7, 8)\r
- */\r
-#define JOY_UP 4\r
-/**\r
- * RIGHT button (valid on channels 7, 8)\r
- */\r
-#define JOY_RIGHT 8\r
-/**\r
- * Analog axis for the X acceleration from the VEX Joystick.\r
- */\r
-#define ACCEL_X 5\r
-/**\r
- * Analog axis for the Y acceleration from the VEX Joystick.\r
- */\r
-#define ACCEL_Y 6\r
-\r
-/**\r
- * Returns true if the robot is in autonomous mode, or false otherwise.\r
- *\r
- * While in autonomous mode, joystick inputs will return a neutral value, but serial port\r
- * communications (even over VexNET) will still work properly.\r
- */\r
-bool isAutonomous();\r
-/**\r
- * Returns true if the robot is enabled, or false otherwise.\r
- *\r
- * While disabled via the VEX Competition Switch or VEX Field Controller, motors will not\r
- * function. However, the digital I/O ports can still be changed, which may indirectly affect\r
- * the robot state (e.g. solenoids). Avoid performing externally visible actions while\r
- * disabled (the kernel should take care of this most of the time).\r
- */\r
-bool isEnabled();\r
-/**\r
- * Returns true if a joystick is connected to the specified slot number (1 or 2), or false\r
- * otherwise.\r
- *\r
- * Useful for automatically merging joysticks for one operator, or splitting for two. This\r
- * function does not work properly during initialize() or initializeIO() and can return false\r
- * positives. It should be checked once and stored at the beginning of operatorControl().\r
- *\r
- * @param joystick the joystick slot to check\r
- */\r
-bool isJoystickConnected(unsigned char joystick);\r
-/**\r
- * Returns true if a VEX field controller or competition switch is connected, or false\r
- * otherwise.\r
- *\r
- * When in online mode, the switching between autonomous() and operatorControl() tasks is\r
- * managed by the PROS kernel.\r
- */\r
-bool isOnline();\r
-/**\r
- * Gets the value of a control axis on the VEX joystick. Returns the value from -127 to 127,\r
- * or 0 if no joystick is connected to the requested slot.\r
- *\r
- * @param joystick the joystick slot to check\r
- * @param axis one of 1, 2, 3, 4, ACCEL_X, or ACCEL_Y\r
- */\r
-int joystickGetAnalog(unsigned char joystick, unsigned char axis);\r
-/**\r
- * Gets the value of a button on the VEX joystick. Returns true if that button is pressed, or\r
- * false otherwise. If no joystick is connected to the requested slot, returns false.\r
- *\r
- * @param joystick the joystick slot to check\r
- * @param buttonGroup one of 5, 6, 7, or 8 to request that button as labelled on the joystick\r
- * @param button one of JOY_UP, JOY_DOWN, JOY_LEFT, or JOY_RIGHT; requesting JOY_LEFT or\r
- * JOY_RIGHT for groups 5 or 6 will cause an undefined value to be returned\r
- */\r
-bool joystickGetDigital(unsigned char joystick, unsigned char buttonGroup,\r
- unsigned char button);\r
-/**\r
- * Returns the backup battery voltage in millivolts.\r
- *\r
- * If no backup battery is connected, returns 0.\r
- */\r
-unsigned int powerLevelBackup();\r
-/**\r
- * Returns the main battery voltage in millivolts.\r
- *\r
- * In rare circumstances, this method might return 0. Check the output value for reasonability\r
- * before blindly blasting the user.\r
- */\r
-unsigned int powerLevelMain();\r
-/**\r
- * Sets the team name displayed to the VEX field control and VEX Firmware Upgrade.\r
- *\r
- * @param name a string containing the team name; only the first eight characters will be shown\r
- */\r
-void setTeamName(const char *name);\r
-\r
-// -------------------- Pin control functions --------------------\r
-\r
-/**\r
- * There are 8 available analog I/O on the Cortex.\r
- */\r
-#define BOARD_NR_ADC_PINS 8\r
-/**\r
- * There are 27 available I/O on the Cortex that can be used for digital communication.\r
- *\r
- * This excludes the crystal ports but includes the Communications, Speaker, and Analog ports.\r
- *\r
- * The motor ports are not on the Cortex and are thus excluded from this count. Pin 0 is the\r
- * Speaker port, pins 1-12 are the standard Digital I/O, 13-20 are the Analog I/O, 21+22 are\r
- * UART1, 23+24 are UART2, and 25+26 are the I2C port.\r
- */\r
-#define BOARD_NR_GPIO_PINS 27\r
-/**\r
- * Used for digitalWrite() to specify a logic HIGH state to output.\r
- *\r
- * In reality, using any non-zero expression or "true" will work to set a pin to HIGH.\r
- */\r
-#define HIGH 1\r
-/**\r
- * Used for digitalWrite() to specify a logic LOW state to output.\r
- *\r
- * In reality, using a zero expression or "false" will work to set a pin to LOW.\r
- */\r
-#define LOW 0\r
-\r
-/**\r
- * pinMode() state for digital input, with pullup.\r
- *\r
- * This is the default state for the 12 Digital pins. The pullup causes the input to read as\r
- * "HIGH" when unplugged, but is fairly weak and can safely be driven by most sources. Many VEX\r
- * digital sensors rely on this behavior and cannot be used with INPUT_FLOATING.\r
- */\r
-#define INPUT 0x0A\r
-/**\r
- * pinMode() state for analog inputs.\r
- *\r
- * This is the default state for the 8 Analog pins and the Speaker port. This only works on\r
- * pins with analog input capabilities; use anywhere else results in undefined behavior.\r
- */\r
-#define INPUT_ANALOG 0x00\r
-/**\r
- * pinMode() state for digital input, without pullup.\r
- *\r
- * Beware of power consumption, as digital inputs left "floating" may switch back and forth\r
- * and cause spurious interrupts.\r
- */\r
-#define INPUT_FLOATING 0x04\r
-/**\r
- * pinMode() state for digital output, push-pull.\r
- *\r
- * This is the mode which should be used to output a digital HIGH or LOW value from the Cortex.\r
- * This mode is useful for pneumatic solenoid valves and VEX LEDs.\r
- */\r
-#define OUTPUT 0x01\r
-/**\r
- * pinMode() state for open-drain outputs.\r
- *\r
- * This is useful in a few cases for external electronics and should not be used for the VEX\r
- * solenoid or LEDs.\r
- */\r
-#define OUTPUT_OD 0x05\r
-\r
-/**\r
- * Calibrates the analog sensor on the specified channel.\r
- *\r
- * This method assumes that the true sensor value is not actively changing at this time and\r
- * computes an average from approximately 500 samples, 1 ms apart, for a 0.5 s period of\r
- * calibration. The average value thus calculated is returned and stored for later calls to the\r
- * analogReadCalibrated() and analogReadCalibratedHR() functions. These functions will return\r
- * the difference between this value and the current sensor value when called.\r
- *\r
- * Do not use this function in initializeIO(), or when the sensor value might be unstable\r
- * (gyro rotation, accelerometer movement).\r
- *\r
- * This function may not work properly if the VEX Cortex is tethered to a PC using the orange\r
- * USB A to A cable and has no VEX 7.2V Battery connected and powered on, as the VEX Battery\r
- * provides power to sensors.\r
- *\r
- * @param channel the channel to calibrate from 1-8\r
- * @return the average sensor value computed by this function\r
- */\r
-int analogCalibrate(unsigned char channel);\r
-/**\r
- * Reads an analog input channel and returns the 12-bit value.\r
- *\r
- * The value returned is undefined if the analog pin has been switched to a different mode.\r
- * This function is Wiring-compatible with the exception of the larger output range. The\r
- * meaning of the returned value varies depending on the sensor attached.\r
- *\r
- * This function may not work properly if the VEX Cortex is tethered to a PC using the orange\r
- * USB A to A cable and has no VEX 7.2V Battery connected and powered on, as the VEX Battery\r
- * provides power to sensors.\r
- *\r
- * @param channel the channel to read from 1-8\r
- * @return the analog sensor value, where a value of 0 reflects an input voltage of nearly 0 V\r
- * and a value of 4095 reflects an input voltage of nearly 5 V\r
- */\r
-int analogRead(unsigned char channel);\r
-/**\r
- * Reads the calibrated value of an analog input channel.\r
- *\r
- * The analogCalibrate() function must be run first on that channel. This function is\r
- * inappropriate for sensor values intended for integration, as round-off error can accumulate\r
- * causing drift over time. Use analogReadCalibratedHR() instead.\r
- *\r
- * This function may not work properly if the VEX Cortex is tethered to a PC using the orange\r
- * USB A to A cable and has no VEX 7.2V Battery connected and powered on, as the VEX Battery\r
- * provides power to sensors.\r
- *\r
- * @param channel the channel to read from 1-8\r
- * @return the difference of the sensor value from its calibrated default from -4095 to 4095\r
- */\r
-int analogReadCalibrated(unsigned char channel);\r
-/**\r
- * Reads the calibrated value of an analog input channel 1-8 with enhanced precision.\r
- *\r
- * The analogCalibrate() function must be run first. This is intended for integrated sensor\r
- * values such as gyros and accelerometers to reduce drift due to round-off, and should not be\r
- * used on a sensor such as a line tracker or potentiometer.\r
- *\r
- * The value returned actually has 16 bits of "precision", even though the ADC only reads\r
- * 12 bits, so that errors induced by the average value being between two values come out\r
- * in the wash when integrated over time. Think of the value as the true value times 16.\r
- *\r
- * This function may not work properly if the VEX Cortex is tethered to a PC using the orange\r
- * USB A to A cable and has no VEX 7.2V Battery connected and powered on, as the VEX Battery\r
- * provides power to sensors.\r
- *\r
- * @param channel the channel to read from 1-8\r
- * @return the difference of the sensor value from its calibrated default from -16384 to 16384\r
- */\r
-int analogReadCalibratedHR(unsigned char channel);\r
-/**\r
- * Gets the digital value (1 or 0) of a pin configured as a digital input.\r
- *\r
- * If the pin is configured as some other mode, the digital value which reflects the current\r
- * state of the pin is returned, which may or may not differ from the currently set value. The\r
- * return value is undefined for pins configured as Analog inputs, or for ports in use by a\r
- * Communications interface. This function is Wiring-compatible.\r
- *\r
- * This function may not work properly if the VEX Cortex is tethered to a PC using the orange\r
- * USB A to A cable and has no VEX 7.2V Battery connected and powered on, as the VEX Battery\r
- * provides power to sensors.\r
- *\r
- * @param pin the pin to read from 1-26\r
- * @return true if the pin is HIGH, or false if it is LOW\r
- */\r
-bool digitalRead(unsigned char pin);\r
-/**\r
- * Sets the digital value (1 or 0) of a pin configured as a digital output.\r
- *\r
- * If the pin is configured as some other mode, behavior is undefined. This function is\r
- * Wiring-compatible.\r
- *\r
- * @param pin the pin to write from 1-26\r
- * @param value an expression evaluating to "true" or "false" to set the output to HIGH or LOW\r
- * respectively, or the constants HIGH or LOW themselves\r
- */\r
-void digitalWrite(unsigned char pin, bool value);\r
-/**\r
- * Configures the pin as an input or output with a variety of settings.\r
- *\r
- * Do note that INPUT by default turns on the pull-up resistor, as most VEX sensors are\r
- * open-drain active low. It should not be a big deal for most push-pull sources. This function\r
- * is Wiring-compatible.\r
- *\r
- * @param pin the pin to modify from 1-26\r
- * @param mode one of INPUT, INPUT_ANALOG, INPUT_FLOATING, OUTPUT, or OUTPUT_OD\r
- */\r
-void pinMode(unsigned char pin, unsigned char mode);\r
-\r
-/*\r
- * Digital port 10 cannot be used as an interrupt port, or for an encoder. Plan accordingly.\r
- */\r
-\r
-/**\r
- * When used in ioSetInterrupt(), triggers an interrupt on rising edges (LOW to HIGH).\r
- */\r
-#define INTERRUPT_EDGE_RISING 1\r
-/**\r
- * When used in ioSetInterrupt(), triggers an interrupt on falling edges (HIGH to LOW).\r
- */\r
-#define INTERRUPT_EDGE_FALLING 2\r
-/**\r
- * When used in ioSetInterrupt(), triggers an interrupt on both rising and falling edges\r
- * (LOW to HIGH or HIGH to LOW).\r
- */\r
-#define INTERRUPT_EDGE_BOTH 3\r
-/**\r
- * Type definition for interrupt handlers. Such functions must accept one argument indicating\r
- * the pin which changed.\r
- */\r
-typedef void (*InterruptHandler)(unsigned char pin);\r
-\r
-/**\r
- * Disables interrupts on the specified pin.\r
- *\r
- * Disabling interrupts on interrupt pins which are not in use conserves processing time.\r
- *\r
- * @param pin the pin on which to reset interrupts from 1-9,11-12\r
- */\r
-void ioClearInterrupt(unsigned char pin);\r
-/**\r
- * Sets up an interrupt to occur on the specified pin, and resets any counters or timers\r
- * associated with the pin.\r
- *\r
- * Each time the specified change occurs, the function pointer passed in will be called with\r
- * the pin that changed as an argument. Enabling pin-change interrupts consumes processing\r
- * time, so it is best to only enable necessary interrupts and to keep the InterruptHandler\r
- * function short. Pin change interrupts can only be enabled on pins 1-9 and 11-12.\r
- *\r
- * Do not use API functions such as delay() inside the handler function, as the function will\r
- * run in an ISR where the scheduler is paused and no other interrupts can execute. It is best\r
- * to quickly update some state and allow a task to perform the work.\r
- *\r
- * Do not use this function on pins that are also being used by the built-in ultrasonic or\r
- * shaft encoder drivers, or on pins which have been switched to output mode.\r
- *\r
- * @param pin the pin on which to enable interrupts from 1-9,11-12\r
- * @param edges one of INTERRUPT_EDGE_RISING, INTERRUPT_EDGE_FALLING, or INTERRUPT_EDGE_BOTH\r
- * @param handler the function to call when the condition is satisfied\r
- */\r
-void ioSetInterrupt(unsigned char pin, unsigned char edges, InterruptHandler handler);\r
-\r
-// -------------------- Physical output control functions --------------------\r
-\r
-/**\r
- * Gets the last set speed of the specified motor channel.\r
- *\r
- * This speed may have been set by any task or the PROS kernel itself. This is not guaranteed\r
- * to be the speed that the motor is actually running at, or even the speed currently being\r
- * sent to the motor, due to latency in the Motor Controller 29 protocol and physical loading.\r
- * To measure actual motor shaft revolution speed, attach a VEX Integrated Motor Encoder or\r
- * VEX Quadrature Encoder and use the velocity functions associated with each.\r
- *\r
- * @param channel the motor channel to fetch from 1-10\r
- * @return the speed last sent to this channel; -127 is full reverse and 127 is full forward,\r
- * with 0 being off\r
- */\r
-int motorGet(unsigned char channel);\r
-/**\r
- * Sets the speed of the specified motor channel.\r
- *\r
- * Do not use motorSet() with the same channel argument from two different tasks. It is safe to\r
- * use motorSet() with different channel arguments from different tasks.\r
- *\r
- * @param channel the motor channel to modify from 1-10\r
- * @param speed the new signed speed; -127 is full reverse and 127 is full forward, with 0\r
- * being off\r
- */\r
-void motorSet(unsigned char channel, int speed);\r
-/**\r
- * Stops the motor on the specified channel, equivalent to calling motorSet() with an argument\r
- * of zero.\r
- *\r
- * This performs a coasting stop, not an active brake. Since motorStop is similar to\r
- * motorSet(0), see the note for motorSet() about use from multiple tasks.\r
- *\r
- * @param channel the motor channel to stop from 1-10\r
- */\r
-void motorStop(unsigned char channel);\r
-/**\r
- * Stops all motors; significantly faster than looping through all motor ports and calling\r
- * motorSet(channel, 0) on each one.\r
- */\r
-void motorStopAll();\r
-\r
-/**\r
- * Initializes VEX speaker support.\r
- *\r
- * The VEX speaker is not thread safe; it can only be used from one task at a time. Using the\r
- * VEX speaker may impact robot performance. Teams may benefit from an if statement that only\r
- * enables sound if isOnline() returns false.\r
- */\r
-void speakerInit();\r
-/**\r
- * Plays up to three RTTTL (Ring Tone Text Transfer Language) songs simultaneously over the\r
- * VEX speaker. The audio is mixed to allow polyphonic sound to be played. Many simple songs\r
- * are available in RTTTL format online, or compose your own.\r
- *\r
- * The song must not be NULL, but unused tracks within the song can be set to NULL. If any of\r
- * the three song tracks is invalid, the result of this function is undefined.\r
- *\r
- * The VEX speaker is not thread safe; it can only be used from one task at a time. Using the\r
- * VEX speaker may impact robot performance. Teams may benefit from an if statement that only\r
- * enables sound if isOnline() returns false.\r
- *\r
- * @param songs an array of up to three (3) RTTTL songs as string values to play\r
- */\r
-void speakerPlayArray(const char * * songs);\r
-/**\r
- * Plays an RTTTL (Ring Tone Text Transfer Language) song over the VEX speaker. Many simple\r
- * songs are available in RTTTL format online, or compose your own.\r
- *\r
- * The song must not be NULL. If an invalid song is specified, the result of this function is\r
- * undefined.\r
- *\r
- * The VEX speaker is not thread safe; it can only be used from one task at a time. Using the\r
- * VEX speaker may impact robot performance. Teams may benefit from an if statement that only\r
- * enables sound if isOnline() returns false.\r
- *\r
- * @param song the RTTTL song as a string value to play\r
- */\r
-void speakerPlayRtttl(const char *song);\r
-/**\r
- * Powers down and disables the VEX speaker.\r
- *\r
- * If a song is currently being played in another task, the behavior of this function is\r
- * undefined, since the VEX speaker is not thread safe.\r
- */\r
-void speakerShutdown();\r
-\r
-// -------------------- VEX sensor control functions --------------------\r
-\r
-/**\r
- * IME addresses end at 0x1F. Actually using more than 10 (address 0x1A) encoders will cause\r
- * unreliable communications.\r
- */\r
-#define IME_ADDR_MAX 0x1F\r
-\r
-/**\r
- * Initializes all IMEs.\r
- *\r
- * IMEs are assigned sequential incrementing addresses, beginning with the first IME on the\r
- * chain (closest to the VEX Cortex I2C port). Therefore, a given configuration of IMEs will\r
- * always have the same ID assigned to each encoder. The addresses range from 0 to\r
- * IME_ADDR_MAX, so the first encoder gets 0, the second gets 1, ...\r
- *\r
- * This function should most likely be used in initialize(). Do not use it in initializeIO() or\r
- * at any other time when the scheduler is paused (like an interrupt). Checking the return\r
- * value of this function is important to ensure that all IMEs are plugged in and responding as\r
- * expected.\r
- *\r
- * This function, unlike the other IME functions, is not thread safe. If using imeInitializeAll\r
- * to re-initialize encoders, calls to other IME functions might behave unpredictably during\r
- * this function's execution.\r
- *\r
- * @return the number of IMEs successfully initialized.\r
- */\r
-unsigned int imeInitializeAll();\r
-/**\r
- * Gets the current 32-bit count of the specified IME.\r
- *\r
- * Much like the count for a quadrature encoder, the tick count is signed and cumulative.\r
- * The value reflects total counts since the last reset. Different VEX Motor Encoders have a\r
- * different number of counts per revolution:\r
- *\r
- * * \c 240.448 for the 269 IME\r
- * * \c 627.2 for the 393 IME in high torque mode (factory default)\r
- * * \c 392 for the 393 IME in high speed mode\r
- *\r
- * If the IME address is invalid, or the IME has not been reset or initialized, the value\r
- * stored in *value is undefined.\r
- *\r
- * @param address the IME address to fetch from 0 to IME_ADDR_MAX\r
- * @param value a pointer to the location where the value will be stored (obtained using the\r
- * "&" operator on the target variable name e.g. <code>imeGet(2, &counts)</code>)\r
- * @return true if the count was successfully read and the value stored in *value is valid;\r
- * false otherwise\r
- */\r
-bool imeGet(unsigned char address, int *value);\r
-/**\r
- * Gets the current rotational velocity of the specified IME.\r
- *\r
- * In this version of PROS, the velocity is positive if the IME count is increasing and\r
- * negative if the IME count is decreasing. The velocity is in RPM of the internal encoder\r
- * wheel. Since checking the IME for its type cannot reveal whether the motor gearing is\r
- * high speed or high torque (in the 2-Wire Motor 393 case), the user must divide the return\r
- * value by the number of output revolutions per encoder revolution:\r
- *\r
- * * \c 30.056 for the 269 IME\r
- * * \c 39.2 for the 393 IME in high torque mode (factory default)\r
- * * \c 24.5 for the 393 IME in high speed mode\r
- *\r
- * If the IME address is invalid, or the IME has not been reset or initialized, the value\r
- * stored in *value is undefined.\r
- *\r
- * @param address the IME address to fetch from 0 to IME_ADDR_MAX\r
- * @param value a pointer to the location where the value will be stored (obtained using the\r
- * "&" operator on the target variable name e.g. <code>imeGetVelocity(2, &counts)</code>)\r
- * @return true if the velocity was successfully read and the value stored in *value is valid;\r
- * false otherwise\r
- */\r
-bool imeGetVelocity(unsigned char address, int *value);\r
-/**\r
- * Resets the specified IME's counters to zero.\r
- *\r
- * This method can be used while the IME is rotating.\r
- *\r
- * @param address the IME address to reset from 0 to IME_ADDR_MAX\r
- * @return true if the reset succeeded; false otherwise\r
- */\r
-bool imeReset(unsigned char address);\r
-/**\r
- * Shuts down all IMEs on the chain; their addresses return to the default and the stored\r
- * counts and velocities are lost. This function, unlike the other IME functions, is not\r
- * thread safe.\r
- *\r
- * To use the IME chain again, wait at least 0.25 seconds before using imeInitializeAll again.\r
- */\r
-void imeShutdown();\r
-\r
-/**\r
- * Reference type for an initialized gyro.\r
- *\r
- * Gyro information is stored as an opaque pointer to a structure in memory; as this is a\r
- * pointer type, it can be safely passed or stored by value.\r
- */\r
-typedef void * Gyro;\r
-\r
-/**\r
- * Gets the current gyro angle in degrees, rounded to the nearest degree.\r
- *\r
- * There are 360 degrees in a circle.\r
- *\r
- * @param gyro the Gyro object from gyroInit() to read\r
- * @return the signed and cumulative number of degrees rotated around the gyro's vertical axis\r
- * since the last start or reset\r
- */\r
-int gyroGet(Gyro gyro);\r
-/**\r
- * Initializes and enables a gyro on an analog port.\r
- *\r
- * NULL will be returned if the port is invalid or the gyro is already in use. Initializing a\r
- * gyro implicitly calibrates it and resets its count. Do not move the robot while the gyro is\r
- * being calibrated. It is suggested to call this function in initialize() and to place the\r
- * robot in its final position before powering it on.\r
- *\r
- * The multiplier parameter can tune the gyro to adapt to specific sensors. The default value\r
- * at this time is 196; higher values will increase the number of degrees reported for a fixed\r
- * actual rotation, while lower values will decrease the number of degrees reported. If your\r
- * robot is consistently turning too far, increase the multiplier, and if it is not turning\r
- * far enough, decrease the multiplier.\r
- *\r
- * @param port the analog port to use from 1-8\r
- * @param multiplier an optional constant to tune the gyro readings; use 0 for the default\r
- * value\r
- * @return a Gyro object to be stored and used for later calls to gyro functions\r
- */\r
-Gyro gyroInit(unsigned char port, unsigned short multiplier);\r
-/**\r
- * Resets the gyro to zero.\r
- *\r
- * It is safe to use this method while a gyro is enabled. It is not necessary to call this\r
- * method before stopping or starting a gyro.\r
- *\r
- * @param gyro the Gyro object from gyroInit() to reset\r
- */\r
-void gyroReset(Gyro gyro);\r
-/**\r
- * Stops and disables the gyro.\r
- *\r
- * Gyros use processing power, so disabling unused gyros increases code performance.\r
- * The gyro's position will be retained.\r
- *\r
- * @param gyro the Gyro object from gyroInit() to stop\r
- */\r
-void gyroShutdown(Gyro gyro);\r
-\r
-/**\r
- * Reference type for an initialized encoder.\r
- *\r
- * Encoder information is stored as an opaque pointer to a structure in memory; as this is a\r
- * pointer type, it can be safely passed or stored by value.\r
- */\r
-typedef void * Encoder;\r
-/**\r
- * Gets the number of ticks recorded by the encoder.\r
- *\r
- * There are 360 ticks in one revolution.\r
- *\r
- * @param enc the Encoder object from encoderInit() to read\r
- * @return the signed and cumulative number of counts since the last start or reset\r
- */\r
-int encoderGet(Encoder enc);\r
-/**\r
- * Initializes and enables a quadrature encoder on two digital ports.\r
- *\r
- * Neither the top port nor the bottom port can be digital port 10. NULL will be returned if\r
- * either port is invalid or the encoder is already in use. Initializing an encoder implicitly\r
- * resets its count.\r
- *\r
- * @param portTop the "top" wire from the encoder sensor with the removable cover side UP\r
- * @param portBottom the "bottom" wire from the encoder sensor\r
- * @param reverse if "true", the sensor will count in the opposite direction\r
- * @return an Encoder object to be stored and used for later calls to encoder functions\r
- */\r
-Encoder encoderInit(unsigned char portTop, unsigned char portBottom, bool reverse);\r
-/**\r
- * Resets the encoder to zero.\r
- *\r
- * It is safe to use this method while an encoder is enabled. It is not necessary to call this\r
- * method before stopping or starting an encoder.\r
- *\r
- * @param enc the Encoder object from encoderInit() to reset\r
- */\r
-void encoderReset(Encoder enc);\r
-/**\r
- * Stops and disables the encoder.\r
- *\r
- * Encoders use processing power, so disabling unused encoders increases code performance.\r
- * The encoder's count will be retained.\r
- *\r
- * @param enc the Encoder object from encoderInit() to stop\r
- */\r
-void encoderShutdown(Encoder enc);\r
-\r
-/**\r
- * Reference type for an initialized ultrasonic sensor.\r
- *\r
- * Ultrasonic information is stored as an opaque pointer to a structure in memory; as this is a\r
- * pointer type, it can be safely passed or stored by value.\r
- */\r
-typedef void * Ultrasonic;\r
-/**\r
- * Gets the current ultrasonic sensor value in centimeters.\r
- *\r
- * If no object was found, zero is returned. If the ultrasonic sensor was never started, the\r
- * return value is undefined. Round and fluffy objects can cause inaccurate values to be\r
- * returned.\r
- *\r
- * @param ult the Ultrasonic object from ultrasonicInit() to read\r
- * @return the distance to the nearest object in centimeters\r
- */\r
-int ultrasonicGet(Ultrasonic ult);\r
-/**\r
- * Initializes an ultrasonic sensor on the specified digital ports.\r
- *\r
- * The ultrasonic sensor will be polled in the background in concert with the other sensors\r
- * registered using this method. NULL will be returned if either port is invalid or the\r
- * ultrasonic sensor port is already in use.\r
- *\r
- * @param portEcho the port connected to the orange cable from 1-9,11-12\r
- * @param portPing the port connected to the yellow cable from 1-12\r
- * @return an Ultrasonic object to be stored and used for later calls to ultrasonic functions\r
- */\r
-Ultrasonic ultrasonicInit(unsigned char portEcho, unsigned char portPing);\r
-/**\r
- * Stops and disables the ultrasonic sensor.\r
- *\r
- * The last distance it had before stopping will be retained. One more ping operation may occur\r
- * before the sensor is fully disabled.\r
- *\r
- * @param ult the Ultrasonic object from ultrasonicInit() to stop\r
- */\r
-void ultrasonicShutdown(Ultrasonic ult);\r
-\r
-/**\r
- * FILE is an integer referring to a stream for the standard I/O functions.\r
- *\r
- * FILE * is the standard library method of referring to a file pointer, even though there is\r
- * actually nothing there.\r
- */\r
-typedef int FILE;\r
-/**\r
- * Bit mask for usartInit() for 8 data bits (typical)\r
- */\r
-#define SERIAL_DATABITS_8 0x0000\r
-/**\r
- * Bit mask for usartInit() for 9 data bits\r
- */\r
-#define SERIAL_DATABITS_9 0x1000\r
-/**\r
- * Bit mask for usartInit() for 1 stop bit (typical)\r
- */\r
-#define SERIAL_STOPBITS_1 0x0000\r
-/**\r
- * Bit mask for usartInit() for 2 stop bits\r
- */\r
-#define SERIAL_STOPBITS_2 0x2000\r
-/**\r
- * Bit mask for usartInit() for No parity (typical)\r
- */\r
-#define SERIAL_PARITY_NONE 0x0000\r
-/**\r
- * Bit mask for usartInit() for Even parity\r
- */\r
-#define SERIAL_PARITY_EVEN 0x0400\r
-/**\r
- * Bit mask for usartInit() for Odd parity\r
- */\r
-#define SERIAL_PARITY_ODD 0x0600\r
-/**\r
- * Specifies the default serial settings when used in usartInit()\r
- */\r
-#define SERIAL_8N1 0x0000\r
-\r
-/**\r
- * Initialize the specified serial interface with the given connection parameters.\r
- *\r
- * I/O to the port is accomplished using the "standard" I/O functions such as fputs(),\r
- * fprintf(), and fputc().\r
- *\r
- * Re-initializing an open port may cause loss of data in the buffers. This routine may be\r
- * safely called from initializeIO() or when the scheduler is paused. If I/O is attempted on a\r
- * serial port which has never been opened, the behavior will be the same as if the port had\r
- * been disabled.\r
- *\r
- * @param usart the port to open, either "uart1" or "uart2"\r
- * @param baud the baud rate to use from 2400 to 1000000 baud\r
- * @param flags a bit mask combination of the SERIAL_* flags specifying parity, stop, and data\r
- * bits\r
- */\r
-void usartInit(FILE *usart, unsigned int baud, unsigned int flags);\r
-/**\r
- * Disables the specified USART interface.\r
- *\r
- * Any data in the transmit and receive buffers will be lost. Attempts to read from the port\r
- * when it is disabled will deadlock, and attempts to write to it may deadlock depending on\r
- * the state of the buffer.\r
- *\r
- * @param usart the port to close, either "uart1" or "uart2"\r
- */\r
-void usartShutdown(FILE *usart);\r
-\r
-// -------------------- Character input and output --------------------\r
-\r
-/**\r
- * The standard output stream uses the PC debug terminal.\r
- */\r
-#define stdout ((FILE *)3)\r
-/**\r
- * The standard input stream uses the PC debug terminal.\r
- */\r
-#define stdin ((FILE *)3)\r
-/**\r
- * UART 1 on the Cortex; must be opened first using usartInit().\r
- */\r
-#define uart1 ((FILE *)1)\r
-/**\r
- * UART 2 on the Cortex; must be opened first using usartInit().\r
- */\r
-#define uart2 ((FILE *)2)\r
-\r
-#ifndef EOF\r
-/**\r
- * EOF is a value evaluating to -1.\r
- */\r
-#define EOF ((int)-1)\r
-#endif\r
-\r
-#ifndef SEEK_SET\r
-/**\r
- * SEEK_SET is used in fseek() to denote an absolute position in bytes from the start of the\r
- * file.\r
- */\r
-#define SEEK_SET 0\r
-#endif\r
-#ifndef SEEK_CUR\r
-/**\r
- * SEEK_CUR is used in fseek() to denote an relative position in bytes from the current file\r
- * location.\r
- */\r
-#define SEEK_CUR 1\r
-#endif\r
-#ifndef SEEK_END\r
-/**\r
- * SEEK_END is used in fseek() to denote an absolute position in bytes from the end of the\r
- * file. The offset will most likely be negative in this case.\r
- */\r
-#define SEEK_END 2\r
-#endif\r
-\r
-/**\r
- * Closes the specified file descriptor. This function does not work on communication ports;\r
- * use usartShutdown() instead.\r
- *\r
- * @param stream the file descriptor to close from fopen()\r
- */\r
-void fclose(FILE *stream);\r
-/**\r
- * Returns the number of characters that can be read without blocking (the number of\r
- * characters available) from the specified stream. This only works for communication ports and\r
- * files in Read mode; for files in Write mode, 0 is always returned.\r
- *\r
- * This function may underestimate, but will not overestimate, the number of characters which\r
- * meet this criterion.\r
- *\r
- * @param stream the stream to read (stdin, uart1, uart2, or an open file in Read mode)\r
- * @return the number of characters which meet this criterion; if this number cannot be\r
- * determined, returns 0\r
- */\r
-int fcount(FILE *stream);\r
-/**\r
- * Delete the specified file if it exists and is not currently open.\r
- *\r
- * The file will actually be erased from memory on the next re-boot. A physical power cycle is\r
- * required to purge deleted files and free their allocated space for new files to be written.\r
- * Deleted files are still considered inaccessible to fopen() in Read mode.\r
- *\r
- * @param file the file name to erase\r
- * @return 0 if the file was deleted, or 1 if the file could not be found\r
- */\r
-int fdelete(const char *file);\r
-/**\r
- * Checks to see if the specified stream is at its end. This only works for communication ports\r
- * and files in Read mode; for files in Write mode, 1 is always returned.\r
- *\r
- * @param stream the channel to check (stdin, uart1, uart2, or an open file in Read mode)\r
- * @return 0 if the stream is not at EOF, or 1 otherwise.\r
- */\r
-int feof(FILE *stream);\r
-/**\r
- * Flushes the data on the specified file channel open in Write mode. This function has no\r
- * effect on a communication port or a file in Read mode, as these streams are always flushed as\r
- * quickly as possible by the kernel.\r
- *\r
- * Successful completion of an fflush function on a file in Write mode cannot guarantee that\r
- * the file is vaild until fclose() is used on that file descriptor.\r
- *\r
- * @param stream the channel to flush (an open file in Write mode)\r
- * @return 0 if the data was successfully flushed, EOF otherwise\r
- */\r
-int fflush(FILE *stream);\r
-/**\r
- * Reads and returns one character from the specified stream, blocking until complete.\r
- *\r
- * Do not use fgetc() on a VEX LCD port; deadlock may occur.\r
- *\r
- * @param stream the stream to read (stdin, uart1, uart2, or an open file in Read mode)\r
- * @return the next character from 0 to 255, or -1 if no character can be read\r
- */\r
-int fgetc(FILE *stream);\r
-/**\r
- * Reads a string from the specified stream, storing the characters into the memory at str.\r
- * Characters will be read until the specified limit is reached, a new line is found, or the\r
- * end of file is reached.\r
- *\r
- * If the stream is already at end of file (for files in Read mode), NULL will be returned;\r
- * otherwise, at least one character will be read and stored into str.\r
- *\r
- * @param str the location where the characters read will be stored\r
- * @param num the maximum number of characters to store; at most (num - 1) characters will be\r
- * read, with a null terminator ('\0') automatically appended\r
- * @param stream the channel to read (stdin, uart1, uart2, or an open file in Read mode)\r
- * @return str, or NULL if zero characters could be read\r
- */\r
-char* fgets(char *str, int num, FILE *stream);\r
-/**\r
- * Opens the given file in the specified mode. The file name is truncated to eight characters.\r
- * Only four files can be in use simultaneously in any given time, with at most one of those\r
- * files in Write mode. This function does not work on communication ports; use usartInit()\r
- * instead.\r
- *\r
- * mode can be "r" or "w". Due to the nature of the VEX Cortex memory, the "r+", "w+", and "a"\r
- * modes are not supported by the file system.\r
- *\r
- * Opening a file that does not exist in Read mode will fail and return NULL, but opening a new\r
- * file in Write mode will create it if there is space. Opening a file that already exists in\r
- * Write mode will destroy the contents and create a new blank file if space is available.\r
- *\r
- * There are important considerations when using of the file system on the VEX Cortex. Reading\r
- * from files is safe, but writing to files should only be performed when robot actuators have\r
- * been stopped. PROS will attempt to continue to handle events during file writes, but most\r
- * user tasks cannot execute during file writing. Powering down the VEX Cortex mid-write may\r
- * cause file system corruption.\r
- *\r
- * @param file the file name\r
- * @param mode the file mode\r
- * @return a file descriptor pointing to the new file, or NULL if the file could not be opened\r
- */\r
-FILE * fopen(const char *file, const char *mode);\r
-/**\r
- * Prints the simple string to the specified stream.\r
- *\r
- * This method is much, much faster than fprintf() and does not add a new line like fputs().\r
- * Do not use fprint() on a VEX LCD port. Use lcdSetText() instead.\r
- *\r
- * @param string the string to write\r
- * @param stream the stream to write (stdout, uart1, uart2, or an open file in Write mode)\r
- */\r
-void fprint(const char *string, FILE *stream);\r
-/**\r
- * Writes one character to the specified stream.\r
- *\r
- * Do not use fputc() on a VEX LCD port. Use lcdSetText() instead.\r
- *\r
- * @param value the character to write (a value of type "char" can be used)\r
- * @param stream the stream to write (stdout, uart1, uart2, or an open file in Write mode)\r
- * @return the character written\r
- */\r
-int fputc(int value, FILE *stream);\r
-/**\r
- * Behaves the same as the "fprint" function, and appends a trailing newline ("\n").\r
- *\r
- * Do not use fputs() on a VEX LCD port. Use lcdSetText() instead.\r
- *\r
- * @param string the string to write\r
- * @param stream the stream to write (stdout, uart1, uart2, or an open file in Write mode)\r
- * @return the number of characters written, excluding the new line\r
- */\r
-int fputs(const char *string, FILE *stream);\r
-/**\r
- * Reads data from a stream into memory. Returns the number of bytes thus read.\r
- *\r
- * If the memory at ptr cannot store (size * count) bytes, undefined behavior occurs.\r
- *\r
- * @param ptr a pointer to where the data will be stored\r
- * @param size the size of each data element to read in bytes\r
- * @param count the number of data elements to read\r
- * @param stream the stream to read (stdout, uart1, uart2, or an open file in Read mode)\r
- * @return the number of bytes successfully read\r
- */\r
-size_t fread(void *ptr, size_t size, size_t count, FILE *stream);\r
-/**\r
- * Seeks within a file open in Read mode. This function will fail when used on a file in Write\r
- * mode or on any communications port.\r
- *\r
- * @param stream the stream to seek within\r
- * @param offset the location within the stream to seek\r
- * @param origin the reference location for offset: SEEK_CUR, SEEK_SET, or SEEK_END\r
- * @return 0 if the seek was successful, or 1 otherwise\r
- */\r
-int fseek(FILE *stream, long int offset, int origin);\r
-/**\r
- * Returns the current position of the stream. This function works on files in either Read or\r
- * Write mode, but will fail on communications ports.\r
- *\r
- * @param stream the stream to check\r
- * @return the offset of the stream, or -1 if the offset could not be determined\r
- */\r
-long int ftell(FILE *stream);\r
-/**\r
- * Writes data from memory to a stream. Returns the number of bytes thus written.\r
- *\r
- * If the memory at ptr is not as long as (size * count) bytes, undefined behavior occurs.\r
- *\r
- * @param ptr a pointer to the data to write\r
- * @param size the size of each data element to write in bytes\r
- * @param count the number of data elements to write\r
- * @param stream the stream to write (stdout, uart1, uart2, or an open file in Write mode)\r
- * @return the number of bytes successfully written\r
- */\r
-size_t fwrite(const void *ptr, size_t size, size_t count, FILE *stream);\r
-/**\r
- * Reads and returns one character from "stdin", which is the PC debug terminal.\r
- *\r
- * @return the next character from 0 to 255, or -1 if no character can be read\r
- */\r
-int getchar();\r
-/**\r
- * Prints the simple string to the debug terminal without formatting.\r
- *\r
- * This method is much, much faster than printf().\r
- *\r
- * @param string the string to write\r
- */\r
-void print(const char *string);\r
-/**\r
- * Writes one character to "stdout", which is the PC debug terminal, and returns the input\r
- * value.\r
- *\r
- * When using a wireless connection, one may need to press the spacebar before the input is\r
- * visible on the terminal.\r
- *\r
- * @param value the character to write (a value of type "char" can be used)\r
- * @return the character written\r
- */\r
-int putchar(int value);\r
-/**\r
- * Behaves the same as the "print" function, and appends a trailing newline ("\n").\r
- *\r
- * @param string the string to write\r
- * @return the number of characters written, excluding the new line\r
- */\r
-int puts(const char *string);\r
-\r
-/**\r
- * Prints the formatted string to the specified output stream.\r
- *\r
- * The specifiers supported by this minimalistic printf() function are:\r
- * * @c \%d: Signed integer in base 10 (int)\r
- * * @c \%u: Unsigned integer in base 10 (unsigned int)\r
- * * @c \%x, @c \%X: Integer in base 16 (unsigned int, int)\r
- * * @c \%p: Pointer (void *, int *, ...)\r
- * * @c \%c: Character (char)\r
- * * @c \%s: Null-terminated string (char *)\r
- * * @c \%%: Single literal percent sign\r
- * * @c \%f: Floating-point number\r
- *\r
- * Specifiers can be modified with:\r
- * * @c 0: Zero-pad, instead of space-pad\r
- * * @c a.b: Make the field at least "a" characters wide. If "b" is specified for "%f", changes the\r
- * number of digits after the decimal point\r
- * * @c -: Left-align, instead of right-align\r
- * * @c +: Always display the sign character (displays a leading "+" for positive numbers)\r
- * * @c l: Ignored for compatibility\r
- *\r
- * Invalid format specifiers, or mismatched parameters to specifiers, cause undefined behavior.\r
- * Other characters are written out verbatim. Do not use fprintf() on a VEX LCD port.\r
- * Use lcdPrint() instead.\r
- *\r
- * @param stream the stream to write (stdout, uart1, or uart2)\r
- * @param formatString the format string as specified above\r
- * @return the number of characters written\r
- */\r
-int fprintf(FILE *stream, const char *formatString, ...);\r
-/**\r
- * Prints the formatted string to the debug stream (the PC terminal).\r
- *\r
- * @param formatString the format string as specified in fprintf()\r
- * @return the number of characters written\r
- */\r
-int printf(const char *formatString, ...);\r
-/**\r
- * Prints the formatted string to the string buffer with the specified length limit.\r
- *\r
- * The length limit, as per the C standard, includes the trailing null character, so an\r
- * argument of 256 will cause a maximum of 255 non-null characters to be printed, and one null\r
- * terminator in all cases.\r
- *\r
- * @param buffer the string buffer where characters can be placed\r
- * @param limit the maximum number of characters to write\r
- * @param formatString the format string as specified in fprintf()\r
- * @return the number of characters stored\r
- */\r
-int snprintf(char *buffer, size_t limit, const char *formatString, ...);\r
-/**\r
- * Prints the formatted string to the string buffer.\r
- *\r
- * If the buffer is not big enough to contain the complete formatted output, undefined behavior\r
- * occurs. See snprintf() for a safer version of this function.\r
- *\r
- * @param buffer the string buffer where characters can be placed\r
- * @param formatString the format string as specified in fprintf()\r
- * @return the number of characters stored\r
- */\r
-int sprintf(char *buffer, const char *formatString, ...);\r
-\r
-/**\r
- * LEFT button on LCD for use with lcdReadButtons()\r
- */\r
-#define LCD_BTN_LEFT 1\r
-/**\r
- * CENTER button on LCD for use with lcdReadButtons()\r
- */\r
-#define LCD_BTN_CENTER 2\r
-/**\r
- * RIGHT button on LCD for use with lcdReadButtons()\r
- */\r
-#define LCD_BTN_RIGHT 4\r
-\r
-/**\r
- * Clears the LCD screen on the specified port.\r
- *\r
- * Printing to a line implicitly overwrites the contents, so clearing should only be required\r
- * at startup.\r
- *\r
- * @param lcdPort the LCD to clear, either uart1 or uart2\r
- */\r
-void lcdClear(FILE *lcdPort);\r
-/**\r
- * Initializes the LCD port, but does not change the text or settings.\r
- *\r
- * If the LCD was not initialized before, the text currently on the screen will be undefined.\r
- * The port will not be usable with standard serial port functions until the LCD is stopped.\r
- *\r
- * @param lcdPort the LCD to initialize, either uart1 or uart2\r
- */\r
-void lcdInit(FILE *lcdPort);\r
-/**\r
- * Prints the formatted string to the attached LCD.\r
- *\r
- * The output string will be truncated as necessary to fit on the LCD screen, 16 characters\r
- * wide. It is probably better to generate the string in a local buffer and use lcdSetText()\r
- * but this method is provided for convenience.\r
- *\r
- * @param lcdPort the LCD to write, either uart1 or uart2\r
- * @param line the LCD line to write, either 1 or 2\r
- * @param formatString the format string as specified in fprintf()\r
- */\r
-#ifdef DOXYGEN\r
-void lcdPrint(FILE *lcdPort, unsigned char line, const char *formatString, ...);\r
-#else\r
-void __attribute__ ((format (printf, 3, 4))) lcdPrint(FILE *lcdPort, unsigned char line,\r
- const char *formatString, ...);\r
-#endif\r
-/**\r
- * Reads the user button status from the LCD display.\r
- *\r
- * For example, if the left and right buttons are pushed, (1 | 4) = 5 will be returned. 0 is\r
- * returned if no buttons are pushed.\r
- *\r
- * @param lcdPort the LCD to poll, either uart1 or uart2\r
- * @return the buttons pressed as a bit mask\r
- */\r
-unsigned int lcdReadButtons(FILE *lcdPort);\r
-/**\r
- * Sets the specified LCD backlight to be on or off.\r
- *\r
- * Turning it off will save power but may make it more difficult to read in dim conditions.\r
- *\r
- * @param lcdPort the LCD to adjust, either uart1 or uart2\r
- * @param backlight true to turn the backlight on, or false to turn it off\r
- */\r
-void lcdSetBacklight(FILE *lcdPort, bool backlight);\r
-/**\r
- * Prints the string buffer to the attached LCD.\r
- *\r
- * The output string will be truncated as necessary to fit on the LCD screen, 16 characters\r
- * wide. This function, like fprint(), is much, much faster than a formatted routine such as\r
- * lcdPrint() and consumes less memory.\r
- *\r
- * @param lcdPort the LCD to write, either uart1 or uart2\r
- * @param line the LCD line to write, either 1 or 2\r
- * @param buffer the string to write\r
- */\r
-void lcdSetText(FILE *lcdPort, unsigned char line, const char *buffer);\r
-/**\r
- * Shut down the specified LCD port.\r
- *\r
- * @param lcdPort the LCD to stop, either uart1 or uart2\r
- */\r
-void lcdShutdown(FILE *lcdPort);\r
-\r
-// -------------------- Real-time scheduler functions --------------------\r
-/**\r
- * Only this many tasks can exist at once. Attempts to create further tasks will not succeed\r
- * until tasks end or are destroyed, AND the idle task cleans them up.\r
- *\r
- * Changing this value will not change the limit without a kernel recompile. The idle task\r
- * and VEX daemon task count against the limit. The user autonomous() or teleop() also counts\r
- * against the limit, so 12 tasks usually remain for other uses.\r
- */\r
-#define TASK_MAX 16\r
-/**\r
- * The maximum number of available task priorities, which run from 0 to 5.\r
- *\r
- * Changing this value will not change the priority count without a kernel recompile.\r
- */\r
-#define TASK_MAX_PRIORITIES 6\r
-/**\r
- * The lowest priority that can be assigned to a task, which puts it on a level with the idle\r
- * task. This may cause severe performance problems and is generally not recommended.\r
- */\r
-#define TASK_PRIORITY_LOWEST 0\r
-/**\r
- * The default task priority, which should be used for most tasks.\r
- *\r
- * Default tasks such as autonomous() inherit this priority.\r
- */\r
-#define TASK_PRIORITY_DEFAULT 2\r
-/**\r
- * The highest priority that can be assigned to a task. Unlike the lowest priority, this\r
- * priority can be safely used without hampering interrupts. Beware of deadlock.\r
- */\r
-#define TASK_PRIORITY_HIGHEST (TASK_MAX_PRIORITIES - 1)\r
-/**\r
- * The recommended stack size for a new task that does an average amount of work. This stack\r
- * size is used for default tasks such as autonomous().\r
- *\r
- * This is probably OK for 4-5 levels of function calls and the use of printf() with several\r
- * arguments. Tasks requiring deep recursion or large local buffers will need a bigger stack.\r
- */\r
-#define TASK_DEFAULT_STACK_SIZE 512\r
-/**\r
- * The minimum stack depth for a task. Scheduler state is stored on the stack, so even if the\r
- * task never uses the stack, at least this much space must be allocated.\r
- *\r
- * Function calls and other seemingly innocent constructs may place information on the stack.\r
- * Err on the side of a larger stack when possible.\r
- */\r
-#define TASK_MINIMAL_STACK_SIZE 64\r
-\r
-/**\r
- * Constant returned from taskGetState() when the task is dead or nonexistant.\r
- */\r
-#define TASK_DEAD 0\r
-/**\r
- * Constant returned from taskGetState() when the task is actively executing.\r
- */\r
-#define TASK_RUNNING 1\r
-/**\r
- * Constant returned from taskGetState() when the task is exists and is available to run, but\r
- * not currently running.\r
- */\r
-#define TASK_RUNNABLE 2\r
-/**\r
- * Constant returned from taskGetState() when the task is delayed or blocked waiting for a\r
- * semaphore, mutex, or I/O operation.\r
- */\r
-#define TASK_SLEEPING 3\r
-/**\r
- * Constant returned from taskGetState() when the task is suspended using taskSuspend().\r
- */\r
-#define TASK_SUSPENDED 4\r
-\r
-/**\r
- * Type by which tasks are referenced.\r
- *\r
- * As this is a pointer type, it can be safely passed or stored by value.\r
- */\r
-typedef void * TaskHandle;\r
-/**\r
- * Type by which mutexes are referenced.\r
- *\r
- * As this is a pointer type, it can be safely passed or stored by value.\r
- */\r
-typedef void * Mutex;\r
-/**\r
- * Type by which semaphores are referenced.\r
- *\r
- * As this is a pointer type, it can be safely passed or stored by value.\r
- */\r
-typedef void * Semaphore;\r
-/**\r
- * Type for defining task functions. Task functions must accept one parameter of type\r
- * "void *"; they need not use it.\r
- *\r
- * For example:\r
- *\r
- * void MyTask(void *ignore) {\r
- * while (1);\r
- * }\r
- */\r
-typedef void (*TaskCode)(void *);\r
-\r
-/**\r
- * Creates a new task and add it to the list of tasks that are ready to run.\r
- *\r
- * @param taskCode the function to execute in its own task\r
- * @param stackDepth the number of variables available on the stack (4 * stackDepth bytes will\r
- * be allocated on the Cortex)\r
- * @param parameters an argument passed to the taskCode function\r
- * @param priority a value from TASK_PRIORITY_LOWEST to TASK_PRIORITY_HIGHEST determining the\r
- * initial priority of the task\r
- * @return a handle to the created task, or NULL if an error occurred\r
- */\r
-TaskHandle taskCreate(TaskCode taskCode, const unsigned int stackDepth, void *parameters,\r
- const unsigned int priority);\r
-/**\r
- * Delays the current task for a given number of milliseconds.\r
- *\r
- * Delaying for a period of zero will force a reschedule, where tasks of equal priority may be\r
- * scheduled if available. The calling task will still be available for immediate rescheduling\r
- * once the other tasks have had their turn or if nothing of equal or higher priority is\r
- * available to be scheduled.\r
- *\r
- * This is not the best method to have a task execute code at predefined intervals, as the\r
- * delay time is measured from when the delay is requested. To delay cyclically, use\r
- * taskDelayUntil().\r
- *\r
- * @param msToDelay the number of milliseconds to wait, with 1000 milliseconds per second\r
- */\r
-void taskDelay(const unsigned long msToDelay);\r
-/**\r
- * Delays the current task until a specified time. The task will be unblocked\r
- * at the time *previousWakeTime + cycleTime, and *previousWakeTime will be changed to reflect\r
- * the time at which the task will unblock.\r
- *\r
- * If the target time is in the past, no delay occurs, but a reschedule is forced, as if\r
- * taskDelay() was called with an argument of zero. If the sum of cycleTime and\r
- * *previousWakeTime overflows or underflows, undefined behavior occurs.\r
- *\r
- * This function should be used by cyclical tasks to ensure a constant execution frequency.\r
- * While taskDelay() specifies a wake time relative to the time at which the function is\r
- * called, taskDelayUntil() specifies the absolute future time at which it wishes to unblock.\r
- * Calling taskDelayUntil with the same cycleTime parameter value in a loop, with \r
- * previousWakeTime referring to a local variable initialized to millis(), will cause the\r
- * loop to execute with a fixed period.\r
- *\r
- * @param previousWakeTime a pointer to the location storing the last unblock time, obtained\r
- * by using the "&" operator on a variable (e.g. "taskDelayUntil(&now, 50);")\r
- * @param cycleTime the number of milliseconds to wait, with 1000 milliseconds per second\r
- */\r
-void taskDelayUntil(unsigned long *previousWakeTime, const unsigned long cycleTime);\r
-/**\r
- * Kills and removes the specified task from the kernel task list.\r
- *\r
- * Deleting the last task will end the program, possibly leading to undesirable states as\r
- * some outputs may remain in their last set configuration.\r
- *\r
- * NOTE: The idle task is responsible for freeing the kernel allocated memory from tasks that\r
- * have been deleted. It is therefore important that the idle task is not starved of\r
- * processing time. Memory allocated by the task code is not automatically freed, and should be\r
- * freed before the task is deleted.\r
- *\r
- * @param taskToDelete the task to kill; passing NULL kills the current task\r
- */\r
-void taskDelete(TaskHandle taskToDelete);\r
-/**\r
- * Determines the number of tasks that are currently being managed.\r
- *\r
- * This includes all ready, blocked and suspended tasks. A task that has been deleted but not\r
- * yet freed by the idle task will also be included in the count. Tasks recently created may\r
- * take one context switch to be counted.\r
- *\r
- * @return the number of tasks that are currently running, waiting, or suspended\r
- */\r
-unsigned int taskGetCount();\r
-/**\r
- * Retrieves the state of the specified task. Note that the state of tasks which have died may\r
- * be re-used for future tasks, causing the value returned by this function to reflect a\r
- * different task than possibly intended in this case.\r
- *\r
- * @param task Handle to the task to query. Passing NULL will query the current task status\r
- * (which will, by definition, be TASK_RUNNING if this call returns)\r
- *\r
- * @return A value reflecting the task's status, one of the constants TASK_DEAD, TASK_RUNNING,\r
- * TASK_RUNNABLE, TASK_SLEEPING, or TASK_SUSPENDED\r
- */\r
-unsigned int taskGetState(TaskHandle task);\r
-/**\r
- * Obtains the priority of the specified task.\r
- *\r
- * @param task the task to check; passing NULL checks the current task\r
- * @return the priority of that task from 0 to TASK_MAX_PRIORITIES\r
- */\r
-unsigned int taskPriorityGet(const TaskHandle task);\r
-/**\r
- * Sets the priority of the specified task.\r
- *\r
- * A context switch may occur before the function returns if the priority being set is higher\r
- * than the currently executing task and the task being mutated is available to be scheduled.\r
- *\r
- * @param task the task to change; passing NULL changes the current task\r
- * @param newPriority a value between TASK_PRIORITY_LOWEST and TASK_PRIORITY_HIGHEST inclusive\r
- * indicating the new task priority\r
- */\r
-void taskPrioritySet(TaskHandle task, const unsigned int newPriority);\r
-/**\r
- * Resumes the specified task.\r
- *\r
- * A task that has been suspended by one or more calls to taskSuspend() will be made available\r
- * for scheduling again by a call to taskResume(). If the task was not suspended at the time\r
- * of the call to taskResume(), undefined behavior occurs.\r
- *\r
- * @param taskToResume the task to change; passing NULL is not allowed as the current task\r
- * cannot be suspended (it is obviously running if this function is called)\r
- */\r
-void taskResume(TaskHandle taskToResume);\r
-/**\r
- * Starts a task which will periodically call the specified function.\r
- *\r
- * Intended for use as a quick-start skeleton for cyclic tasks with higher priority than the\r
- * "main" tasks. The created task will have priority TASK_PRIORITY_DEFAULT + 1 with the default\r
- * stack size. To customize behavior, create a task manually with the specified function.\r
- *\r
- * This task will automatically terminate after one further function invocation when the robot\r
- * is disabled or when the robot mode is switched.\r
- *\r
- * @param fn the function to call in this loop\r
- * @param increment the delay between successive calls in milliseconds; the taskDelayUntil()\r
- * function is used for accurate cycle timing\r
- * @return a handle to the task, or NULL if an error occurred\r
- */\r
-TaskHandle taskRunLoop(void (*fn)(void), const unsigned long increment);\r
-/**\r
- * Suspends the specified task.\r
- *\r
- * When suspended a task will not be scheduled, regardless of whether it might be otherwise\r
- * available to run.\r
- *\r
- * @param taskToSuspend the task to suspend; passing NULL suspends the current task\r
- */\r
-void taskSuspend(TaskHandle taskToSuspend);\r
-\r
-/**\r
- * Creates a semaphore intended for synchronizing tasks. To prevent some critical code from\r
- * simultaneously modifying a shared resource, use mutexes instead.\r
- *\r
- * Semaphores created using this function can be accessed using the semaphoreTake() and\r
- * semaphoreGive() functions. The mutex functions must not be used on objects of this type.\r
- *\r
- * This type of object does not need to have balanced take and give calls, so priority\r
- * inheritance is not used. Semaphores can be signalled by an interrupt routine.\r
- *\r
- * @return a handle to the created semaphore\r
- */\r
-Semaphore semaphoreCreate();\r
-/**\r
- * Signals a semaphore. Tasks waiting for a signal using semaphoreTake() will be unblocked by\r
- * this call and can continue execution.\r
- *\r
- * Slow processes can give semaphores when ready, and fast processes waiting to take the\r
- * semaphore will continue at that point.\r
- *\r
- * @param semaphore the semaphore to signal\r
- * @return true if the semaphore was successfully given, or false if the semaphore was not\r
- * taken since the last give\r
- */\r
-bool semaphoreGive(Semaphore semaphore);\r
-/**\r
- * Waits on a semaphore. If the semaphore is already in the "taken" state, the current task\r
- * will wait for the semaphore to be signaled. Other tasks can run during this time.\r
- *\r
- * @param semaphore the semaphore to wait\r
- * @param blockTime the maximum time to wait for the semaphore to be given, where -1\r
- * specifies an infinite timeout\r
- * @return true if the semaphore was successfully taken, or false if the timeout expired\r
- */\r
-bool semaphoreTake(Semaphore semaphore, const unsigned long blockTime);\r
-/**\r
- * Deletes the specified semaphore. This function can be dangerous; deleting semaphores being\r
- * waited on by a task may cause deadlock or a crash.\r
- *\r
- * @param semaphore the semaphore to destroy\r
- */\r
-void semaphoreDelete(Semaphore semaphore);\r
-\r
-/**\r
- * Creates a mutex intended to allow only one task to use a resource at a time. For signalling\r
- * and synchronization, try using semaphores.\r
- *\r
- * Mutexes created using this function can be accessed using the mutexTake() and mutexGive()\r
- * functions. The semaphore functions must not be used on objects of this type.\r
- *\r
- * This type of object uses a priority inheritance mechanism so a task 'taking' a mutex MUST\r
- * ALWAYS 'give' the mutex back once the mutex is no longer required.\r
- *\r
- * @return a handle to the created mutex\r
- */\r
-Mutex mutexCreate();\r
-/**\r
- * Relinquishes a mutex so that other tasks can use the resource it guards. The mutex must be\r
- * held by the current task using a corresponding call to mutexTake.\r
- *\r
- * @param mutex the mutex to release\r
- * @return true if the mutex was released, or false if the mutex was not already held\r
- */\r
-bool mutexGive(Mutex mutex);\r
-/**\r
- * Requests a mutex so that other tasks cannot simultaneously use the resource it guards.\r
- * The mutex must not already be held by the current task. If another task already\r
- * holds the mutex, the function will wait for the mutex to be released. Other tasks can run\r
- * during this time.\r
- *\r
- * @param mutex the mutex to request\r
- * @param blockTime the maximum time to wait for the mutex to be available, where -1\r
- * specifies an infinite timeout\r
- * @return true if the mutex was successfully taken, or false if the timeout expired\r
- */\r
-bool mutexTake(Mutex mutex, const unsigned long blockTime);\r
-/**\r
- * Deletes the specified mutex. This function can be dangerous; deleting semaphores being\r
- * waited on by a task may cause deadlock or a crash.\r
- *\r
- * @param mutex the mutex to destroy\r
- */\r
-void mutexDelete(Mutex mutex);\r
-\r
-/**\r
- * Wiring-compatible alias of taskDelay().\r
- *\r
- * @param time the duration of the delay in milliseconds (1 000 milliseconds per second)\r
- */\r
-void delay(const unsigned long time);\r
-/**\r
- * Wait for approximately the given number of microseconds.\r
- *\r
- * The method used for delaying this length of time may vary depending on the argument.\r
- * The current task will always be delayed by at least the specified period, but possibly much\r
- * more depending on CPU load. In general, this function is less reliable than delay(). Using\r
- * this function in a loop may hog processing time from other tasks.\r
- *\r
- * @param us the duration of the delay in microseconds (1 000 000 microseconds per second)\r
- */\r
-void delayMicroseconds(const unsigned long us);\r
-/**\r
- * Returns the number of microseconds since Cortex power-up. There are 10^6 microseconds in a\r
- * second, so as a 32-bit integer, this will overflow and wrap back to zero every two hours or\r
- * so.\r
- *\r
- * This function is Wiring-compatible.\r
- *\r
- * @return the number of microseconds since the Cortex was turned on or the last overflow\r
- */\r
-unsigned long micros();\r
-/**\r
- * Returns the number of milliseconds since Cortex power-up. There are 1000 milliseconds in a\r
- * second, so as a 32-bit integer, this will not overflow for 50 days.\r
- *\r
- * This function is Wiring-compatible.\r
- *\r
- * @return the number of milliseconds since the Cortex was turned on\r
- */\r
-unsigned long millis();\r
-/**\r
- * Alias of taskDelay() intended to help EasyC users.\r
- *\r
- * @param time the duration of the delay in milliseconds (1 000 milliseconds per second)\r
- */\r
-void wait(const unsigned long time);\r
-/**\r
- * Alias of taskDelayUntil() intended to help EasyC users.\r
- *\r
- * @param previousWakeTime a pointer to the last wakeup time\r
- * @param time the duration of the delay in milliseconds (1 000 milliseconds per second)\r
- */\r
-void waitUntil(unsigned long *previousWakeTime, const unsigned long time);\r
-\r
-// End C++ extern to C\r
-#ifdef __cplusplus\r
-}\r
-#endif\r
-\r
-#endif\r
+++ /dev/null
-/** @file main.h\r
- * @brief Header file for global functions\r
- * \r
- * Any experienced C or C++ programmer knows the importance of header files. For those who\r
- * do not, a header file allows multiple files to reference functions in other files without\r
- * necessarily having to see the code (and therefore causing a multiple definition). To make\r
- * a function in "opcontrol.c", "auto.c", "main.c", or any other C file visible to the core\r
- * implementation files, prototype it here.\r
- *\r
- * This file is included by default in the predefined stubs in each VEX Cortex PROS Project.\r
- *\r
- * Copyright (c) 2011-2014, Purdue University ACM SIG BOTS.\r
- * All rights reserved.\r
- *\r
- * Redistribution and use in source and binary forms, with or without\r
- * modification, are permitted provided that the following conditions are met:\r
- * * Redistributions of source code must retain the above copyright\r
- * notice, this list of conditions and the following disclaimer.\r
- * * Redistributions in binary form must reproduce the above copyright\r
- * notice, this list of conditions and the following disclaimer in the\r
- * documentation and/or other materials provided with the distribution.\r
- * * Neither the name of Purdue University ACM SIG BOTS nor the\r
- * names of its contributors may be used to endorse or promote products\r
- * derived from this software without specific prior written permission.\r
- *\r
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND\r
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\r
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\r
- * DISCLAIMED. IN NO EVENT SHALL PURDUE UNIVERSITY ACM SIG BOTS BE LIABLE FOR ANY\r
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\r
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\r
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\r
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\r
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
- *\r
- * Purdue Robotics OS contains FreeRTOS (http://www.freertos.org) whose source code may be\r
- * obtained from http://sourceforge.net/projects/freertos/files/ or on request.\r
- */\r
-#ifndef MAIN_H_\r
-\r
-// This prevents multiple inclusion, which isn't bad for this file but is good practice\r
-#define MAIN_H_\r
-\r
-#include <API.h>\r
-\r
-// Allow usage of this file in C++ programs\r
-#ifdef __cplusplus\r
-extern "C" {\r
-#endif\r
-// A function prototype looks exactly like its declaration, but with a semicolon instead of\r
-// actual code. If a function does not match a prototype, compile errors will occur.\r
-\r
-// Prototypes for initialization, operator control and autonomous\r
-\r
-/**\r
- * Runs the user autonomous code. This function will be started in its own task with the default\r
- * priority and stack size whenever the robot is enabled via the Field Management System or the\r
- * VEX Competition Switch in the autonomous mode. If the robot is disabled or communications is\r
- * lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart\r
- * the task, not re-start it from where it left off.\r
- *\r
- * Code running in the autonomous task cannot access information from the VEX Joystick. However,\r
- * the autonomous function can be invoked from another task if a VEX Competition Switch is not\r
- * available, and it can access joystick information if called in this way.\r
- *\r
- * The autonomous task may exit, unlike operatorControl() which should never exit. If it does\r
- * so, the robot will await a switch to another mode or disable/enable cycle.\r
- */\r
-void autonomous();\r
-/**\r
- * Runs pre-initialization code. This function will be started in kernel mode one time while the\r
- * VEX Cortex is starting up. As the scheduler is still paused, most API functions will fail.\r
- *\r
- * The purpose of this function is solely to set the default pin modes (pinMode()) and port\r
- * states (digitalWrite()) of limit switches, push buttons, and solenoids. It can also safely\r
- * configure a UART port (usartOpen()) but cannot set up an LCD (lcdInit()).\r
- */\r
-void initializeIO();\r
-/**\r
- * Runs user initialization code. This function will be started in its own task with the default\r
- * priority and stack size once when the robot is starting up. It is possible that the VEXnet\r
- * communication link may not be fully established at this time, so reading from the VEX\r
- * Joystick may fail.\r
- *\r
- * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global\r
- * variables, and IMEs.\r
- *\r
- * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks\r
- * will not start. An autonomous mode selection menu like the pre_auton() in other environments\r
- * can be implemented in this task if desired.\r
- */\r
-void initialize();\r
-/**\r
- * Runs the user operator control code. This function will be started in its own task with the\r
- * default priority and stack size whenever the robot is enabled via the Field Management System\r
- * or the VEX Competition Switch in the operator control mode. If the robot is disabled or\r
- * communications is lost, the operator control task will be stopped by the kernel. Re-enabling\r
- * the robot will restart the task, not resume it from where it left off.\r
- *\r
- * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will\r
- * run the operator control task. Be warned that this will also occur if the VEX Cortex is\r
- * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.\r
- *\r
- * Code running in this task can take almost any action, as the VEX Joystick is available and\r
- * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly\r
- * recommended to give other tasks (including system tasks such as updating LCDs) time to run.\r
- *\r
- * This task should never exit; it should end with some kind of infinite loop, even if empty.\r
- */\r
-extern Encoder encoder1,encoder2;\r
-void operatorControl();\r
-\r
-// End C++ export structure\r
-#ifdef __cplusplus\r
-}\r
-#endif\r
-\r
-#endif\r
+++ /dev/null
-# Makefile for compiling PROS projects\r
-\r
-# Path to project root (NO trailing slash!)\r
-ROOT=..\r
-# Binary output directory\r
-BINDIR=$(ROOT)/bin\r
-\r
-# Nothing below here needs to be modified by typical users\r
-\r
-# Include common aspects of this project\r
--include $(ROOT)/common.mk\r
-\r
-ASMSRC:=$(wildcard *.$(ASMEXT))\r
-ASMOBJ:=$(patsubst %.o,$(BINDIR)/%.o,$(ASMSRC:.$(ASMEXT)=.o))\r
-HEADERS:=$(wildcard *.$(HEXT))\r
-### Special section for Cortex projects ###\r
-HEADERS_2:=$(wildcard ../include/*.$(HEXT))\r
-### End special section ###\r
-CSRC=$(wildcard *.$(CEXT))\r
-COBJ:=$(patsubst %.o,$(BINDIR)/%.o,$(CSRC:.$(CEXT)=.o))\r
-CPPSRC:=$(wildcard *.$(CPPEXT))\r
-CPPOBJ:=$(patsubst %.o,$(BINDIR)/%.o,$(CPPSRC:.$(CPPEXT)=.o))\r
-OUT:=$(BINDIR)/$(OUTNAME)\r
-\r
-.PHONY: all\r
-\r
-# By default, compile program\r
-all: .\r
-\r
-# Compiles the program if anything is changed\r
-.: $(ASMOBJ) $(COBJ) $(CPPOBJ)\r
- @touch .\r
-\r
-# Assembly source file management\r
-$(ASMOBJ): $(BINDIR)/%.o: %.$(ASMEXT)\r
- @echo AS $<\r
- @$(AS) $(AFLAGS) -o $@ $<\r
-\r
-### Special section for Cortex projects ###\r
-\r
-# Object management\r
-$(COBJ): $(BINDIR)/%.o: %.$(CEXT) $(HEADERS) $(HEADERS_2)\r
- @echo CC $(INCLUDE) $<\r
- @$(CC) $(INCLUDE) $(CFLAGS) -o $@ $<\r
-\r
-$(CPPOBJ): $(BINDIR)/%.o: %.$(CPPEXT) $(HEADERS) $(HEADERS_2)\r
- @echo CPC $(INCLUDE) $<\r
- @$(CPPCC) $(INCLUDE) $(CPPFLAGS) -o $@ $<\r
-\r
-### End special section ###\r
+++ /dev/null
-/** @file auto.c\r
- * @brief File for autonomous code\r
- *\r
- * This file should contain the user autonomous() function and any functions related to it.\r
- *\r
- * Copyright (c) 2011-2014, Purdue University ACM SIG BOTS.\r
- * All rights reserved.\r
- *\r
- * Redistribution and use in source and binary forms, with or without\r
- * modification, are permitted provided that the following conditions are met:\r
- * * Redistributions of source code must retain the above copyright\r
- * notice, this list of conditions and the following disclaimer.\r
- * * Redistributions in binary form must reproduce the above copyright\r
- * notice, this list of conditions and the following disclaimer in the\r
- * documentation and/or other materials provided with the distribution.\r
- * * Neither the name of Purdue University ACM SIG BOTS nor the\r
- * names of its contributors may be used to endorse or promote products\r
- * derived from this software without specific prior written permission.\r
- *\r
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND\r
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\r
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\r
- * DISCLAIMED. IN NO EVENT SHALL PURDUE UNIVERSITY ACM SIG BOTS BE LIABLE FOR ANY\r
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\r
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\r
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\r
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\r
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
- *\r
- * Purdue Robotics OS contains FreeRTOS (http://www.freertos.org) whose source code may be\r
- * obtained from http://sourceforge.net/projects/freertos/files/ or on request.\r
- */\r
-\r
-#include "main.h"\r
-\r
-/*\r
- * Runs the user autonomous code. This function will be started in its own task with the default\r
- * priority and stack size whenever the robot is enabled via the Field Management System or the\r
- * VEX Competition Switch in the autonomous mode. If the robot is disabled or communications is\r
- * lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart\r
- * the task, not re-start it from where it left off.\r
- *\r
- * Code running in the autonomous task cannot access information from the VEX Joystick. However,\r
- * the autonomous function can be invoked from another task if a VEX Competition Switch is not\r
- * available, and it can access joystick information if called in this way.\r
- *\r
- * The autonomous task may exit, unlike operatorControl() which should never exit. If it does\r
- * so, the robot will await a switch to another mode or disable/enable cycle.\r
- */\r
-void autonomous() {\r
-}\r
+++ /dev/null
-/** @file init.c\r
- * @brief File for initialization code\r
- *\r
- * This file should contain the user initialize() function and any functions related to it.\r
- *\r
- * Copyright (c) 2011-2014, Purdue University ACM SIG BOTS.\r
- * All rights reserved.\r
- *\r
- * Redistribution and use in source and binary forms, with or without\r
- * modification, are permitted provided that the following conditions are met:\r
- * * Redistributions of source code must retain the above copyright\r
- * notice, this list of conditions and the following disclaimer.\r
- * * Redistributions in binary form must reproduce the above copyright\r
- * notice, this list of conditions and the following disclaimer in the\r
- * documentation and/or other materials provided with the distribution.\r
- * * Neither the name of Purdue University ACM SIG BOTS nor the\r
- * names of its contributors may be used to endorse or promote products\r
- * derived from this software without specific prior written permission.\r
- *\r
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND\r
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\r
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\r
- * DISCLAIMED. IN NO EVENT SHALL PURDUE UNIVERSITY ACM SIG BOTS BE LIABLE FOR ANY\r
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\r
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\r
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\r
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\r
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
- *\r
- * Purdue Robotics OS contains FreeRTOS (http://www.freertos.org) whose source code may be\r
- * obtained from http://sourceforge.net/projects/freertos/files/ or on request.\r
- */\r
-\r
-#include "main.h"\r
-Encoder encoder1,encoder2;\r
-/*\r
- * Runs pre-initialization code. This function will be started in kernel mode one time while the\r
- * VEX Cortex is starting up. As the scheduler is still paused, most API functions will fail.\r
- *\r
- * The purpose of this function is solely to set the default pin modes (pinMode()) and port\r
- * states (digitalWrite()) of limit switches, push buttons, and solenoids. It can also safely\r
- * configure a UART port (usartOpen()) but cannot set up an LCD (lcdInit()).\r
- */\r
-void initializeIO() {\r
-\r
-\r
- pinMode(1,INPUT);\r
- pinMode(2,INPUT);\r
- pinMode(3,INPUT);\r
- pinMode(4,INPUT);\r
- pinMode(5,INPUT);\r
- pinMode(6,INPUT);\r
- pinMode(7,INPUT);\r
- pinMode(8,INPUT);\r
-\r
-\r
-\r
-\r
-// Setup sensors.\r
-\r
-\r
-\r
-\r
-}/*\r
- * Runs user initialization code. This function will be started in its own task with the default\r
- * priority and stack size once when the robot is starting up. It is possible that the VEXnet\r
- * communication link may not be fully established at this time, so reading from the VEX\r
- * Joystick may fail.\r
- *\r
- * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global\r
- * variables, and IMEs.\r
- *\r
- * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks\r
- * will not start. An autonomous mode selection menu like the pre_auton() in other environments\r
- * can be implemented in this task if desired.\r
- */\r
-void initialize() {\r
- lcdInit(uart2);\r
- lcdClear(uart2);\r
- lcdSetBacklight(uart2,1);\r
- encoder1 = encoderInit(3,4,true);\r
- encoder2 = encoderInit(5,6,true);\r
-}\r
+++ /dev/null
-#include "main.h"\r
-\r
-#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))\r
-#define PI =3.14159\r
-/*\r
- * Runs the user operator control code. This function will be started in its own task with the\r
- * default priority and stack size whenever the robot is enabled via the Field Management System\r
- * or the VEX Competition Switch in the operator control mode. If the robot is disabled or\r
- * communications is lost, the operator control task will be stopped by the kernel. Re-enabling\r
- * the robot will restart the task, not resume it from where it left off.\r
- *\r
- * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will\r
- * run the operator control task. Be warned that this will also occur if the VEX Cortex is\r
- * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.\r
- *\r
- * Code running in this task can take almost any action, as the VEX Joystick is available and\r
- * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly\r
- * recommended to give other tasks (including system tasks such as updating LCDs) time to run.\r
- *\r
- * This task should never exit; it should end with some kind of infinite loop, even if empty.\r
- *\r
-*/\r
-\r
-/* notes for Clyne:\r
- * Other processes should not interupt the collection of data:\r
- * rotation calculation should be and data collection should be a high priority/primary task\r
- * Main task= manages tasks, init variables, reset functions (sensors, etc)\r
- * Primary = sensor data collections and calculations\r
- * Secondary = drive tasks\r
- *\r
- *\r
- *\r
- */\r
-static long lVel,rVel,deltaPos,deltaTime,startTheta,\r
- heading,lDist,rDist,xPos,yPos,\r
- track=15;\r
-void operatorControl() {\r
- heading=startTheta;\r
- while (1) {\r
-\r
- //get time 1 the first time\r
- heading=((lDist-rDist)/(track*2*PI))*360+startTheta;//heading in degrees. might need to be radians for cosine functions\r
- //get time 2\r
- deltaPos=(lVel+rVel)/2*deltaTime;\r
- xPos+=cos(heading)*deltaPos;\r
- yPos+=sin(heading)*deltaPos;\r
- //get time 1\r
- delay(20);\r
-\r
- }\r
-}\r
+++ /dev/null
-{\rtf1\ansi\ansicpg1252\cocoartf1404\cocoasubrtf130
-{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
-{\colortbl;\red255\green255\blue255;}
-\margl1440\margr1440\vieww10800\viewh8400\viewkind0
-\pard\tx720\tx1440\tx2160\tx2880\tx3600\tx4320\tx5040\tx5760\tx6480\tx7200\tx7920\tx8640\pardirnatural\partightenfactor0
-
-\f0\fs24 \cf0 Articles/PDFs to Read:\
-{\field{\*\fldinst{HYPERLINK "http://www.ridgesoft.com/articles/trackingposition/TrackingPosition.pdf"}}{\fldrslt http://www.ridgesoft.com/articles/trackingposition/TrackingPosition.pdf}}}
\ No newline at end of file
# Flags for programs\r
AFLAGS:=$(MCUAFLAGS)\r
ARFLAGS:=$(MCUCFLAGS)\r
-CCFLAGS:=-s -c -Wall $(MCUCFLAGS) -Os -ffunction-sections -fsigned-char -fomit-frame-pointer -fsingle-precision-constant\r
+CCFLAGS:=-c -Wall $(MCUCFLAGS) -Os -ffunction-sections -fsigned-char -fomit-frame-pointer -fsingle-precision-constant\r
CFLAGS:=$(CCFLAGS) -std=gnu99 -Werror=implicit-function-declaration\r
CPPFLAGS:=$(CCFLAGS) -fno-exceptions -fno-rtti -felide-constructors\r
LDFLAGS:=-Wall $(MCUCFLAGS) $(MCULFLAGS) -Wl,--gc-sections\r
* * \c 240.448 for the 269 IME\r
* * \c 627.2 for the 393 IME in high torque mode (factory default)\r
* * \c 392 for the 393 IME in high speed mode\r
- * * \c 261.333 for the 393 IME in turbo speed mode\r
*\r
* If the IME address is invalid, or the IME has not been reset or initialized, the value\r
* stored in *value is undefined.\r
* * \c 30.056 for the 269 IME\r
* * \c 39.2 for the 393 IME in high torque mode (factory default)\r
* * \c 24.5 for the 393 IME in high speed mode\r
- * * \c 16.3333125 for the 393 IME in turbo speed mode\r
*\r
* If the IME address is invalid, or the IME has not been reset or initialized, the value\r
* stored in *value is undefined.\r
#define MAIN_H_\r
\r
#include <API.h>\r
-#include <zephyr.h>\r
+#include <stdint.h>\r
+\r
+typedef struct{\r
+ int x;\r
+ int y;\r
+}vec2;\r
+\r
+typedef enum {\r
+ UP,\r
+ DOWN,\r
+ KEY_UP\r
+} Button;\r
+\r
+typedef struct {\r
+ unsigned int num;\r
+ struct Side {\r
+ struct Group {\r
+ Button l;\r
+ Button r;\r
+ Button u;\r
+ Button d;\r
+ } front, back;\r
+ vec2 stick;\r
+ } left, right;\r
+} Controller;\r
+\r
+typedef struct {\r
+ enum type {\r
+ DIGITAL,\r
+ ANALOG,\r
+ GYRO,\r
+ ULTRASONIC\r
+ } type;\r
+ union data {\r
+ Gyro gyro;\r
+ Ultrasonic sonic;\r
+ } data;\r
+ unsigned int port;\r
+ int value;\r
+ int initial;\r
+} Sensor;\r
+\r
+typedef struct {\r
+ bool kill_req;\r
+ bool exiting;\r
+ TaskCode code;\r
+ TaskHandle handle;\r
+ void *param;\r
+} Process;\r
\r
#ifdef __cplusplus\r
extern "C" {\r
#endif\r
\r
+#define DEFAULT_TRPM trpm = 1850;\r
+#define EXTRA_TRPM trpm = 1900;\r
+\r
+#define PI 3.14159265L\r
+\r
+#define LCD_PORT uart2\r
+\r
+/**\r
+ * Be sure that getIMEPort() matches these values (see sensor.c).\r
+ */\r
+\r
+enum MOTOR_MAP {\r
+ CANNON_LEFT = 1,\r
+ CANNON_RIGHT,\r
+ LIFT_PUSHER,\r
+ INTAKE_1,\r
+ INTAKE_2,\r
+ DRIVE_RIGHT,\r
+ DRIVE_LEFT,\r
+ LIFT_1,\r
+ LIFT_2,\r
+ LIFT_ROTATER\r
+};\r
+\r
+enum MOTOR_IME_MAP {\r
+ DRIVE_RIGHT_IME = 0,\r
+ DRIVE_LEFT_IME,\r
+ LIFT_ROTATER_IME,\r
+ LIFT_1_IME,\r
+ LIFT_2_IME,\r
+ CANNON_LEFT_IME,\r
+ CANNON_RIGHT_IME\r
+};\r
+\r
+extern unsigned char MOTOR_USE_MAP[10];\r
+\r
+#define motorTake(p,k) MOTOR_USE_MAP[p-1] = k;\r
+#define motorFree(p) MOTOR_USE_MAP[p-1] = 0;\r
+#define motorSetK(p,s,k) if(!MOTOR_USE_MAP[p-1] || MOTOR_USE_MAP[p-1] == k){ motorSet(p,s); }\r
+#define motorSetN(p,s) motorSetK(p,s,0)\r
+\r
+int getIMEPort(unsigned int port);\r
+int getIME(unsigned int port);\r
+int getIMEVelocity(unsigned int port);\r
+\r
+/**\r
+ * Controller library functions\r
+ */\r
+\r
+#define keyUp(b) (b == KEY_UP)\r
+#define keyDown(b) (b == DOWN)\r
+\r
+void setEvent(Controller *c);\r
+\r
+/**\r
+ * Sensor library functions\r
+ */\r
+\r
+#define LIGHT_THRESH_DEFAULT 50\r
+#define SONIC_THRESH_DEFAULT 8\r
+\r
+#define initUltrasonic(p1,p2) initSensor((p2<<16)|p1,ULTRASONIC)\r
+Sensor initSensor(uint32_t port,unsigned char type);\r
+\r
+#define getSensor(s) (s.value)\r
+int readSensor(Sensor *s);\r
+\r
+#define diffSensor(s) (s.value - s.initial)\r
+#define underSensor(s,t) (diffSensor(s) < -t)\r
+#define overSensor(s,t) (diffSensor(s) > t)\r
+\r
+/**\r
+ * Process library functions\r
+ */\r
+\r
+#define taskInit(h,p) if(!h) h = taskCreate(h##Code,TASK_DEFAULT_STACK_SIZE,p,TASK_PRIORITY_DEFAULT);\r
+\r
+/**\r
+ * Main function declarations\r
+ */\r
+\r
+extern void softwareReset(void);\r
+\r
void autonomous();\r
void initializeIO();\r
void initialize();\r
+++ /dev/null
-#ifndef ZEPHYR_H_
-#define ZEPHYR_H_
-
-#include <main.h>
-
-#define PI 3.14159265L
-
-#define APPLY_THRESH(n,t) if(n < t && n > -t){ n = 0;}
-
-/*
- * Comment to disable LCD support.
-*/
-
-#define LCD_PORT uart1
-
-#define LCD_RATE 500
-
-#ifdef LCD_PORT
-#endif // LCD_PORT
-
-/*
- * Comment to disable gyro support.
-*/
-
-#define GYRO_PORT 2
-
-#ifdef GYRO_PORT
-
-void zGyroInit(void);
-int zGyroGet(void);
-void zGyroReset(void);
-
-#endif // GYRO_PORT
-
-/*
- * Comment to disable IME support.
-*/
-
-#define IME_ENABLE
-
-#ifdef IME_ENABLE
-
-void zIMEInit(void);
-
-#endif // IME_ENABLE
-
-/*
- * DRIVE_NORMAL will override tank drive options.
-*/
-
-#define DRIVE_JOY 1
-#define DRIVE_THRESHOLD 10
-
-#define DRIVE_NORMAL 3
-
-//#define DRIVE_TANK_LEFT 3
-//#define DRIVE_TANK_RIGHT 2
-
-#define zJoyDigital(j,g,b) joystickGetDigital(j,g,b)
-#define zJoyAnalog(j,a) joystickGetAnalog(j,a)
-
-void zMotorSet(const char *, // Motor Name
- int, // Desired Speed
- unsigned int // Caller ID
- );
-char zMotorGet(const char *); // Motor Name
-
-void zMotorTake(const char *,unsigned int);
-void zMotorReturn(const char *);
-
-#ifdef IME_ENABLE
-
-int zMotorIMEGet(const char *); // Motor Name
-int zMotorIMEGetVelocity(const char *); // Motor Name
-bool zMotorIMEReset(const char *motor);
-
-#endif // IME_ENABLE
-
-void zDriveUpdate(void);
-
-char zGetDigitalMotorSpeed(unsigned char, // Joystick No.
- unsigned char, // Button Group
- unsigned char, // Positive Button
- unsigned char, // Negative Button
- char // Desired Speed
- );
-
-#endif // ZEPHYR_H_
+++ /dev/null
-#include <main.h>\r
-#include <zephyr.h>\r
-\r
-//#define TARGET_RPM 1700\r
-\r
-/*void autonomous(){\r
- static double cl,cr,ca;\r
- static char speed;\r
-\r
- speed = 30;\r
-\r
- do{\r
- speed += 10;\r
- zMotorSet("Left cannon",-speed);\r
- zMotorSet("Right cannon",speed);\r
- delay(800);\r
- cl = zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9;\r
- cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;\r
- ca = (cl + cr) / 2;\r
- }while(ca < TARGET_RPM);\r
-\r
- zMotorSet("Misc",127);\r
- delay(900);\r
- zMotorSet("Misc",-127);\r
- delay(900);\r
- zMotorSet("Misc",0);\r
-\r
- motorStopAll();\r
-}*/\r
--- /dev/null
+#include <main.h>
+
+void setEvent(Controller *c){
+ c->left.stick.x = joystickGetAnalog(c->num, 4);
+ c->left.stick.y = joystickGetAnalog(c->num, 3);
+
+ c->right.stick.x = joystickGetAnalog(c->num, 1);
+ c->right.stick.y = joystickGetAnalog(c->num, 2);
+
+ /**
+ * BACK RIGHT BUTTONS
+ */
+
+ if(c->right.back.d == KEY_UP)c->right.back.d = UP;
+ else if(c->right.back.d == DOWN && !joystickGetDigital(c->num, 6, JOY_DOWN))c->right.back.d = KEY_UP;
+ else c->right.back.d = joystickGetDigital(c->num, 6, JOY_DOWN);
+
+ if(c->right.back.u == KEY_UP)c->right.back.u = UP;
+ else if(c->right.back.u == DOWN && !joystickGetDigital(c->num, 6, JOY_UP))c->right.back.u = KEY_UP;
+ else c->right.back.u = joystickGetDigital(c->num, 6, JOY_UP);
+
+
+ /*
+ * BACK LEFT BUTTONS
+ */
+ if(c->left.back.d == KEY_UP)c->left.back.d = UP;
+ else if(c->left.back.d == DOWN && !joystickGetDigital(c->num, 5, JOY_DOWN))c->left.back.d = KEY_UP;
+ else c->left.back.d = joystickGetDigital(c->num, 5, JOY_DOWN);
+
+ if(c->left.back.u == KEY_UP)c->left.back.u = UP;
+ else if(c->left.back.u == DOWN && !joystickGetDigital(c->num, 5, JOY_UP))c->left.back.u = KEY_UP;
+ else c->left.back.u = joystickGetDigital(c->num, 5, JOY_UP);
+
+
+ /*
+ * LEFT FRONT BUTTONS
+ */
+ if(c->left.front.l == KEY_UP)c->left.front.l = UP;
+ else if(c->left.front.l == DOWN && !joystickGetDigital(c->num, 7, JOY_LEFT))c->left.front.l = KEY_UP;
+ else c->left.front.l = joystickGetDigital(c->num, 7, JOY_LEFT);
+
+ if(c->left.front.r == KEY_UP)c->left.front.r = UP;
+ else if(c->left.front.r == DOWN && !joystickGetDigital(c->num, 7, JOY_RIGHT))c->left.front.r = KEY_UP;
+ else c->left.front.r = joystickGetDigital(c->num, 7, JOY_RIGHT);
+
+ if(c->left.front.u == KEY_UP)c->left.front.u = UP;
+ else if(c->left.front.u == DOWN && !joystickGetDigital(c->num, 7, JOY_UP))c->left.front.u = KEY_UP;
+ else c->left.front.u = joystickGetDigital(c->num, 7, JOY_UP);
+
+ if(c->left.front.d == KEY_UP)c->left.front.d = UP;
+ else if(c->left.front.d == DOWN && !joystickGetDigital(c->num, 7, JOY_DOWN))c->left.front.d = KEY_UP;
+ else c->left.front.d = joystickGetDigital(c->num, 7, JOY_DOWN);
+
+ /*
+ * RIGHT FRONT BUTTONS
+ */
+ if(c->right.front.d == KEY_UP)c->right.front.d = UP;
+ else if(c->right.front.d == DOWN && !joystickGetDigital(c->num, 8, JOY_DOWN))c->right.front.d = KEY_UP;
+ else c->right.front.d = joystickGetDigital(c->num, 8, JOY_DOWN);
+
+ if(c->right.front.l == KEY_UP)c->right.front.l = UP;
+ else if(c->right.front.l == DOWN && !joystickGetDigital(c->num, 8, JOY_LEFT))c->right.front.l = KEY_UP;
+ else c->right.front.l = joystickGetDigital(c->num, 8, JOY_LEFT);
+
+ if(c->right.front.u == KEY_UP)c->right.front.u = UP;
+ else if(c->right.front.u == DOWN && !joystickGetDigital(c->num, 8, JOY_UP))c->right.front.u = KEY_UP;
+ else c->right.front.u = joystickGetDigital(c->num, 8, JOY_UP);
+
+ if(c->right.front.r == KEY_UP)c->right.front.r = UP;
+ else if(c->right.front.r == DOWN && !joystickGetDigital(c->num, 8, JOY_RIGHT))c->right.front.r = KEY_UP;
+ else c->right.front.r = joystickGetDigital(c->num, 8, JOY_RIGHT);
+}
#include <main.h>\r
\r
-void initializeIO(){\r
-}\r
+Sensor intakeFrontLeft,\r
+ intakeFrontRight,\r
+ intakeLiftBase,\r
+ intakeLiftTop,\r
+ robotGyro;\r
+\r
+Gyro gyro;\r
\r
-void initialize(){\r
+bool initializeDone = false;\r
+\r
+void initializeIO(void){\r
+ intakeFrontLeft = initSensor(7,ANALOG);\r
+ intakeFrontRight = initSensor(6,ANALOG);\r
+ intakeLiftBase = initUltrasonic(1,2);\r
+ intakeLiftTop = initSensor(8,ANALOG);\r
+ //robotGyro = initSensor(2,GYRO);\r
+}\r
\r
- pinMode(20,INPUT_ANALOG);\r
- pinMode(13,INPUT_ANALOG);\r
+void initialize(void){\r
+ lcdInit(uart2);\r
+ lcdClear(uart2);\r
+ lcdSetBacklight(uart2,true);\r
\r
- lcdInit(LCD_PORT);\r
- lcdClear(LCD_PORT);\r
- lcdSetBacklight(LCD_PORT,true);\r
+ gyro = gyroInit(2,0);\r
\r
- zGyroInit();\r
- zIMEInit();\r
+ imeInitializeAll();\r
\r
- delay(1000);\r
+ lcdPrint(uart2,1," 5106 ZEPHYR ");\r
\r
+ initializeDone = true;\r
+ delay(2000);\r
}\r
#include <main.h>\r
-#include <stdint.h>\r
#include <math.h>\r
\r
-/**\r
- * The distance in inches the robot starts from the goal.\r
- */\r
+#define AUTO_SKILLS\r
\r
-#define GOAL_DISTANCE 120\r
+extern Sensor intakeFrontLeft,\r
+ intakeFrontRight,\r
+ intakeLiftBase,\r
+ intakeLiftTop;//,\r
+ //robotGyro;\r
\r
-/**\r
- * RTTTL songs for speaker usage.\r
- */\r
+extern Gyro gyro;\r
\r
-const char *song = "John Cena:d=4,o=5,b=125:4p,4g5,8a5,8f5,8p,1g5,4p,4a#5,8a5,8f5,8p,1g5";\r
-const char *xpst = "WinXP Login:d=4,o=5,b=160:4d#6.,4a#5.,2g#5.,4d#6,4a#5";\r
-const char *xpst2 = "WinXP Shutdown:d=4,o=5,b=125:4g#6,4d#6,4g#5,2a#5";\r
-const char *bound = "Nobody to Love:d=4,o=5,b=125:4d#5,2g5,4p,4g5,4f5,2g5,4d#5,4f5,2g5,4d#5,4f5,4g5,4d#5,4d#5,4f5,4g5,4a#4,1c5,4d#5,4f5,2g5,\\r
-4a#5,8g5,8f5,4d#5";\r
+extern bool initializeDone;\r
\r
-/**\r
- * Contains how many milliseconds it took to enter operatorControl(), used to\r
- * measure how long we've been in operatorControl().\r
- */\r
+static TaskHandle taskLCD = NULL,\r
+ taskPos = NULL,\r
+ taskCan = NULL,\r
+ taskArm = NULL,\r
+ taskLift = NULL,\r
+ taskAim = NULL;\r
\r
-static unsigned long opmillis = 0;\r
+static float xpos = 0,\r
+ ypos = 0;\r
\r
-/**\r
- * Contains a light sensor value collected at init time for object detection.\r
- */\r
+static double cangle = 0;\r
\r
-static int lightThresh = 0;\r
-\r
-/**\r
- * The value passed to motorSet() for the cannons. This variable is declared\r
- * in a global scope so that the 'Speed Adjust' buttons can modify raw speed\r
- * when not in RPM Tracking Mode.\r
- */\r
-\r
-static char cann = 0;\r
-\r
-/**\r
- * 'rpm' is the current RPM of the cannon motors when in RPM Tracking Mode.\r
- * 'trpm' contains the target RPM for the cannon to reach, which is adjusted\r
- * when the robot moves.\r
- * 'arpm' stands for Adjust RPM, and contains a value modifiable by the\r
- * operator and added to the target RPM.\r
- */\r
-\r
-static double rpm = 0,\r
- trpm = 1850,\r
- arpm = 0;\r
+static double rpm = 0, trpm = 1850, arpm = 0;\r
static bool cannReady = false;\r
\r
-/**\r
- * Contains the current X and Y position in inches. The X axis extends from\r
- * the front of the robot, with the Y axis perpendicular to the X axis.\r
- */\r
-\r
-static double xpos=0,ypos=0;\r
-\r
-/**\r
- * These bools are set by their corresponding tasks when those tasks are ran or\r
- * stopped. These prevent multiple instances of the same task.\r
- */\r
-\r
-static bool cannonProcRun = false,\r
- armProcRun = false,\r
- aimProcRun = false;\r
-\r
-/**\r
- * Task handles for the tasks, should they be needed.\r
- */\r
-\r
-TaskHandle taskLCD,\r
- taskCannon,\r
- taskArm,\r
- taskMove,\r
- taskAim;\r
-\r
-/**\r
- * Task function prototypes so that they can be spawned from operatorControl().\r
- */\r
-\r
-void lcdUpdateFunc(void *);\r
-void cannonProc(void *);\r
-void armProc(void *);\r
-void moveProc(void *);\r
-void aimProc(void *);\r
-\r
-/**\r
- * Cause the Cortex to reset, which results in the Cortex restarting the\r
- * operator control code.\r
- *\r
- * This reset is accomplished through setting two bits, SYSRESETREQ and\r
- * VECTRESET, in the Application Interrupt and Reset Control Register (AIRCR),\r
- * which is at a known location in memory. SYSRESETREQ will actually request\r
- * the system reset, while VECTRESET is 'reserved for debugging'. This second\r
- * bit may not need to be set; it's only set here because others have found it\r
- * necessary.\r
- */\r
-\r
-#define AIRCR_ADDR 0xE000ED0C\r
-#define VECTKEY 0x05FA\r
-#define SYSRESETREQ (1<<2)\r
-#define VECTRESET (1<<0)\r
-\r
-void softwareReset(void){\r
-\r
- /*\r
- * Read the current value of the AIRCR, since some flags currently set in\r
- * the register need to remain the same in order for the reset to work.\r
- */\r
+static Controller c[2];\r
\r
- uint32_t AIRCR = *((uint32_t *)AIRCR_ADDR);\r
+static Button No = UP;\r
\r
- /*\r
- * Here we save the lower 16 bits of the register, write a special key to\r
- * the higher 16 bits that'll allow the write to occur, and then set the\r
- * reset flags.\r
- */\r
-\r
- AIRCR = (AIRCR & 0xFFFF) | (VECTKEY << 16) | SYSRESETREQ | VECTRESET;\r
-\r
- // Update the AIRCR.\r
-\r
- *((uint32_t *)0xE000ED0C) = AIRCR;\r
-\r
- /*\r
- * This instruction causes the program to wait until the previous memory\r
- * write is complete, ensuring it is taken into effect and the reset\r
- * request is made.\r
- */\r
+void taskLCDCode(void *);\r
+void taskPosCode(void *);\r
+void taskCanCode(void *);\r
+void taskLiftCode(void *);\r
+void taskArmCode(void *);\r
+void taskAimCode(void *);\r
\r
- asm("DSB");\r
+void operatorControl(void){\r
+ static bool invert = false;\r
\r
- // Wait until the system reset completes.\r
-\r
- while(1);\r
-}\r
-\r
-/******************************************************************************\r
- * OPERATOR CONTROL *\r
- ******************************************************************************/\r
-\r
-void operatorControl(){\r
-\r
- /**\r
- * 'ui_inc' is used to have button reads happen at a certain interval.\r
- * 'cyc' contains what song to play next off of the speaker.\r
- */\r
-\r
- static unsigned char ui_inc = 0;\r
- static unsigned char cyc = 0;\r
+ DEFAULT_TRPM;\r
+ c[0].num = 1;\r
+ c[1].num = 2;\r
\r
/**\r
- * The raw speed to set the lift motors to.\r
+ * Insure that the initialization functions were executed.\r
*/\r
\r
- static char lift;\r
+ if(!initializeDone){\r
+ initializeIO();\r
+ initialize();\r
+ }\r
\r
/**\r
- * Collected init-time variables.\r
+ * Get initial readings from each sensor.\r
*/\r
\r
- opmillis = millis();\r
- lightThresh = analogRead(8) - 60;\r
+ intakeFrontLeft.initial = readSensor(&intakeFrontLeft);\r
+ intakeFrontRight.initial = readSensor(&intakeFrontRight);\r
+ intakeLiftBase.initial = readSensor(&intakeLiftBase);\r
+ intakeLiftTop.initial = readSensor(&intakeLiftTop);\r
+ //robotGyro.initial = readSensor(&robotGyro);\r
\r
- /**\r
- * Spawn the LCD task and the position-tracking task.\r
- */\r
+ taskInit(taskLCD,NULL);\r
+ taskInit(taskPos,NULL);\r
\r
- taskLCD=taskCreate(lcdUpdateFunc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
- taskMove=taskCreate(moveProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT+1);\r
+ if(taskCan)\r
+ taskDelete(taskCan);\r
\r
while(1){\r
\r
- /*\r
- * Handle drive controls and update drive motor speeds.\r
+ /**\r
+ * Update sensor values.\r
*/\r
\r
- zDriveUpdate();\r
-\r
- // Set the rotating motor speed.\r
-\r
- if(!aimProcRun)\r
- zMotorSet("Rotater",-zJoyAnalog(1,1) / 4,0);\r
-\r
- // Set the intake's speed.\r
+ readSensor(&intakeFrontLeft);\r
+ readSensor(&intakeFrontRight);\r
+ readSensor(&intakeLiftBase);\r
+ readSensor(&intakeLiftTop);\r
+ //readSensor(&robotGyro);\r
\r
- zMotorSet("Intake",\r
- zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127)/*|\r
- zGetDigitalMotorSpeed(2,5,JOY_UP,JOY_DOWN,127)*/,\r
- 0\r
- );\r
-\r
- // Set the lift's speed.\r
+ /**\r
+ * Read the joystick!!!\r
+ */\r
\r
- if(!armProcRun){\r
- lift = zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127);\r
- zMotorSet("Lift 1",lift,0);\r
- zMotorSet("Lift 2",lift,0);\r
+ setEvent(&c[0]);\r
+ setEvent(&c[1]);\r
+\r
+ if(keyUp(c[1].left.front.l)){\r
+ taskInit(taskCan,&c[1].left.front.l);\r
+ }else if(keyUp(c[1].left.front.u)){\r
+ /*if(!taskLift){\r
+ c[1].left.front.u = DOWN;\r
+ taskLift = taskCreate(taskLiftCode,TASK_DEFAULT_STACK_SIZE,&c[1].left.front.u,TASK_PRIORITY_DEFAULT);\r
+ }*/\r
+ taskInit(taskAim,&c[1].left.front.u);\r
+ }else if(keyUp(c[1].left.front.d)){\r
+ taskInit(taskArm,&c[1].left.front.d);\r
}\r
\r
- // Set the cannon's speed.\r
+ if(keyUp(c[1].right.front.u)){\r
+ arpm += 50;\r
+ }else if(keyUp(c[1].right.front.d)){\r
+ arpm -= 50;\r
+ }else if(keyUp(c[1].right.front.l) | keyDown(c[0].right.front.l)){\r
+ softwareReset();\r
+ }\r
\r
- if(!cannonProcRun){\r
- zMotorSet("Left cannon" ,-cann,0);\r
- zMotorSet("Right cannon", cann,0);\r
+ if(keyUp(c[0].right.front.r) | keyUp(c[1].right.front.r)){\r
+ invert ^= true;\r
}\r
\r
- if(++ui_inc == 50){\r
- ui_inc = 0;\r
+ motorSetN(DRIVE_LEFT ,c[0].left.stick.y);\r
+ motorSetN(DRIVE_RIGHT,c[0].right.stick.y);\r
\r
- // 1-5 CPU reset\r
+ motorSetN(INTAKE_2,(c[0].left.back.u | c[1].left.back.u) ? 127 : (c[0].left.back.d | c[1].left.back.d) ? -127 : 0);\r
+ motorSetN(INTAKE_1,invert ? -motorGet(INTAKE_2) : motorGet(INTAKE_2));\r
\r
- if(zJoyDigital(1,5,JOY_UP) && zJoyDigital(1,5,JOY_DOWN))\r
- softwareReset();\r
+ motorSetN(LIFT_1,c[1].right.back.u ? 127 : c[1].right.back.d ? -127 : 0);\r
+ motorSetN(LIFT_2,motorGet(LIFT_1));\r
\r
- // 1-7L Speaker function\r
+ motorSetN(LIFT_ROTATER,-c[1].right.stick.x / 4);\r
\r
- /*if(zJoyDigital(1,7,JOY_LEFT)){\r
- speakerInit();\r
- switch(cyc){\r
- case 0:speakerPlayRtttl(song );break;\r
- case 1:speakerPlayRtttl(xpst );break;\r
- case 2:speakerPlayRtttl(xpst2);break;\r
- case 3:speakerPlayRtttl(bound);break;\r
- }\r
- if(++cyc == 4) cyc = 0;\r
- speakerShutdown();\r
-\r
- // 2-8R Gyroscope reset\r
+ delay(20);\r
+ }\r
+}\r
\r
- }else if(zJoyDigital(2,8,JOY_RIGHT)){\r
- zGyroReset();\r
- zMotorIMEReset("Rotater");\r
+static unsigned int ballPos = 0;\r
\r
- // 2-8U Auto-Aim start\r
+void taskLiftCode(void *unused){\r
+ Button *kill = (Button *)unused;\r
\r
- }else if(zJoyDigital(2,8,JOY_UP))\r
- taskAim = taskCreate(aimProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+ motorTake(INTAKE_1,1);\r
+ motorTake(INTAKE_2,1);\r
\r
- // 2-8D Auto-Aim kill\r
+ do{\r
+ if(!underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT)){\r
+ if(cangle < 20 && cangle > -20){\r
+ motorTake(LIFT_1,1);\r
+ motorTake(LIFT_2,1);\r
+ motorSetK(LIFT_1,127,1);\r
+ motorSetK(LIFT_2,127,1);\r
+ }else{\r
+ motorSetK(LIFT_1,0,1);\r
+ motorSetK(LIFT_2,0,1);\r
+ motorFree(LIFT_1);\r
+ motorFree(LIFT_2);\r
+\r
+ if(!underSensor(intakeFrontLeft,LIGHT_THRESH_DEFAULT) &&\r
+ !underSensor(intakeFrontRight,LIGHT_THRESH_DEFAULT) ){\r
+ motorSetK(INTAKE_1,127,1);\r
+ motorSetK(INTAKE_2,127,1);\r
+ }else{\r
+ motorSetK(INTAKE_1,0,1);\r
+ motorSetK(INTAKE_2,0,1);\r
+ }\r
+ }\r
+ }else{\r
+ motorSetK(INTAKE_1,0,1);\r
+ motorSetK(INTAKE_2,0,1);\r
+ motorSetN(LIFT_1,0);\r
+ motorSetN(LIFT_2,0);\r
+ }\r
+ }while(!keyDown(*kill));\r
\r
- else if(zJoyDigital(2,8,JOY_DOWN))\r
- aimProcRun = false;*/\r
+ motorFree(INTAKE_1);\r
+ motorFree(INTAKE_2);\r
\r
- // 2-7U Increase cannon (speed/target RPM)\r
+ while(keyDown(*kill))\r
+ delay(100);\r
\r
- if(zJoyDigital(1,7,JOY_UP)){\r
- if(cannonProcRun) arpm += 50;\r
- else if(cann < 120)cann += 10;\r
+ taskLift = NULL;\r
+}\r
\r
- // 2-7D Decrease cannon\r
+void taskAimCode(void *unused){\r
+ Button *kill = (Button *)unused;\r
\r
- }else if(zJoyDigital(1,7,JOY_DOWN)){\r
- if(cannonProcRun) arpm -= 50;\r
- else if(cann > -120)cann -= 10;\r
+ static double target = 0;\r
\r
- // 2-7L Toggle RPM Tracking task.\r
+ motorTake(LIFT_ROTATER,4);\r
\r
- }else if(zJoyDigital(1,7,JOY_LEFT)){\r
- if(!cannonProcRun)\r
- taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
- else\r
- cannonProcRun = false;\r
- }\r
+ target = cangle;\r
\r
- // 2-7R Start Ball Pusher task.\r
+ do{\r
\r
- if(zJoyDigital(1,7,JOY_RIGHT))\r
- taskArm = taskCreate(armProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+ if(cangle > target){\r
+ motorSetK(LIFT_ROTATER,30,4);\r
+ }else if(cangle < target){\r
+ motorSetK(LIFT_ROTATER,-30,4);\r
+ }else{\r
+ motorSetK(LIFT_ROTATER,0,4);\r
}\r
\r
- delay(10); // Short delay to allow task switching\r
- }\r
-}\r
-\r
-/**\r
- * The Position-Tracker process.\r
- *\r
- * This process tries to track the position of the robot, using a combination\r
- * of motor encoders and the gyroscope. This process is created directly, and\r
- * is expected to run at a higher priority than other tasks.\r
- */\r
-\r
-void moveProc(void *unused_param){\r
+ delay(100);\r
+ }while(!keyDown(*kill));\r
\r
- /**\r
- * 'l' and 'r' contain IMEGet() values, 'lv' and 'rv' contain\r
- * IMEGetVelocity() values.\r
- */\r
+ motorFree(LIFT_ROTATER);\r
\r
- static double l,r,lv,rv;\r
+ while(keyDown(*kill))\r
+ delay(100);\r
\r
- /**\r
- * 'dist' might contain the total distance traveled in the past 10\r
- * milliseconds.\r
- * 'head' contains the angle the robot is facing at relative to its origin.\r
- * 'dpos' is the delta position.?\r
- */\r
+ taskAim = NULL;\r
+}\r
\r
- static double dist,head;\r
+void taskPosCode(void *unused){\r
+ static unsigned int dlime,drime;\r
+ static double /*l,r,*/lv,rv;\r
+ static double /*dist,*/head;\r
static double dpos;\r
\r
+ dlime = getIMEPort(DRIVE_LEFT);\r
+ drime = getIMEPort(DRIVE_RIGHT);\r
+\r
while(1){\r
\r
+ cangle = (int)floor(getIME(getIMEPort(LIFT_ROTATER)) / 627.2L * 112.5);\r
+ cangle += 0.28L * (cangle / 25);\r
+\r
/**\r
- * Get IMEGet values.\r
+ * Get IME values.\r
*/\r
\r
- l = zMotorIMEGet("Left drive") / 627.2L;\r
- r = -zMotorIMEGet("Right drive") / 627.2L;\r
+ //l = getIME(dlime) / 627.2L;\r
+ //r = getIME(drime) / 627.2L;\r
\r
/**\r
- * Get IMEGetVelocity values and convert to inches per millisecond:\r
+ * Get IME velocity values and convert to inches per millisecond:\r
*\r
* random # ->\r
* motor RPM ->\r
* inches per millisecond\r
*/\r
\r
- lv = zMotorIMEGetVelocity("Left drive") / 39.2L * 12.566L / 60000;\r
- rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 12.566L / 60000;\r
+ lv = getIMEVelocity(dlime) / 39.2L * 12.566L / 60000;\r
+ rv = -getIMEVelocity(drime) / 39.2L * 12.566L / 60000;\r
\r
/**\r
* Get the distance thing.\r
*/\r
\r
- dist = (l - r) * 8.64L;\r
+ //dist = (l - r) * 8.64L;\r
\r
/**\r
* Calculate heading, then trash the result in favor of the gyroscope.\r
*/\r
\r
- head = fmod(dist / 15,360.0L) / 2 * 90;\r
- head = /*(head +*/ zGyroGet()/*) / 2*/;\r
+ //head = fmod(dist / 15,360.0L) / 2 * 90;\r
+ //head = 0;//getSensor(robotGyro);\r
+ head = gyroGet(gyro);\r
\r
/**\r
* Calculate the delta position thing.\r
}\r
}\r
\r
-/**\r
- * The Auto-Aim process.\r
- *\r
- * This function will attempt to keep the cannon aimed at a constant angle,\r
- * adjusting its position when the robot's orientation changes by using the\r
- * gyroscope and the encoder build into the rotater motor.\r
- */\r
-\r
-void aimProc(void *procPtr){\r
-\r
- static double cangle, // Current angle (of rotater)\r
- rangle; // 'Robot' angle (target angle\r
-\r
- // Tell others we're running.\r
-\r
- aimProcRun = true;\r
+void taskCanCode(void *unused){\r
+ Button *kill = (Button *)unused;\r
\r
- /**\r
- * Claim necessary motors to prevent others from using them.\r
- */\r
-\r
- zMotorTake("Rotater",1);\r
-\r
- /**\r
- * Run until a stop is requested.\r
- */\r
-\r
- while(aimProcRun){\r
-\r
- /**\r
- * Read orientation sensors.\r
- */\r
-\r
- cangle = (int)floor(zMotorIMEGet("Rotater") / 627.2L * 112.5);\r
- rangle = zGyroGet() + (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);\r
-\r
- lcdPrint(uart1,1,"%.3lf, %.3lf",cangle,rangle);\r
-\r
- /**\r
- * Adjust aim if necessary.\r
- */\r
-\r
- if(cangle > rangle + 3)\r
- zMotorSet("Rotater",30,1);\r
- else if(cangle < rangle - 3)\r
- zMotorSet("Rotater",-30,1);\r
- else\r
- zMotorSet("Rotater",0,1);\r
-\r
- delay(100);\r
- }\r
-\r
- /**\r
- * Free motors.\r
- */\r
-\r
- zMotorReturn("Rotater");\r
-}\r
-\r
-/**\r
- * The RPM-Targeter process.\r
- *\r
- * This will attempt to keep the cannon motors running at a constant speed,\r
- * determined through a target RPM set by the user.\r
- */\r
-\r
-void cannonProc(void *procPtr){\r
+ static unsigned int clime,crime;\r
static double cl,cr,ca;\r
- static int speed;//,ispeed = 0;\r
-\r
- cannonProcRun = true;\r
+ static int speed;\r
\r
- /*\r
- * Initialize variables.\r
- * These need to be static so that their values are preserved through\r
- * task switchs but will retain their values when the function is\r
- * re-called, so here we reset them.\r
- */\r
+ clime = getIMEPort(CANNON_LEFT);\r
+ crime = getIMEPort(CANNON_RIGHT);\r
\r
speed = 20;\r
rpm = cl = cr = ca = 0;\r
\r
- /*\r
- * Reserve needed motors.\r
- */\r
-\r
- zMotorTake("Left cannon",2);\r
- zMotorTake("Right cannon",2);\r
-\r
- /*\r
- * Here we increase the power provided to the motors until our target\r
- * RPM is reached.\r
- */\r
+ motorTake(CANNON_LEFT,2);\r
+ motorTake(CANNON_RIGHT,2);\r
\r
do{\r
\r
- /*\r
- * Bring up the speed ...\r
- * The only reasonable explanation for an error such as the speed\r
- * getting too high is a hardware fault in the robot (bad motor,\r
- * bad IME, ...).\r
- */\r
-\r
speed += 10;\r
if(speed > 120)\r
speed = 127;\r
\r
- /*\r
- * Set the power levels, and allow the motors to adjust.\r
- */\r
-\r
- zMotorSet("Left cannon" ,-speed,2);\r
- zMotorSet("Right cannon", speed,2);\r
+ motorSetK(CANNON_LEFT,-speed,2);\r
+ motorSetK(CANNON_RIGHT,speed,2);\r
\r
delay(300);\r
\r
- /*\r
- * Calculate the average RPM, and continue if our target is met.\r
- */\r
-\r
- cl = zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9;\r
- cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;\r
+ cl = getIMEVelocity(clime) / 16.3333125L * 9;\r
+ cr = -getIMEVelocity(crime) / 16.3333125L * 9;\r
rpm = ca = cr;//(cl + cr) / 2;\r
\r
- }while(cannonProcRun && ca < trpm);\r
-\r
- if(!cannonProcRun)\r
- return;\r
+ }while(ca < trpm);\r
\r
- /*\r
- * Once we reach our target, we 'idle' at the sped and adjust as\r
- * necessary. `cannInUse` will be cleared by any user attempt to use the\r
- * motor.\r
- */\r
-\r
- while(cannonProcRun){\r
+ while(!keyDown(*kill)){\r
\r
/*\r
* Update RPM values.\r
*/\r
\r
- cl = zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9;\r
- cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;\r
+ cl = getIMEVelocity(clime) / 16.3333125L * 9;\r
+ cr = -getIMEVelocity(crime) / 16.3333125L * 9;\r
rpm = ca = cr;//(cl + cr) / 2;\r
\r
/*\r
*/\r
\r
if(ca < trpm - 40){\r
- speed += 2;\r
- zMotorSet("Left cannon" ,-speed,2);\r
- zMotorSet("Right cannon", speed,2);\r
+ speed += 2 * ((trpm - rpm) / 60);\r
+ motorSetK(CANNON_LEFT,-speed,2);\r
+ motorSetK(CANNON_RIGHT,speed,2);\r
cannReady = false;\r
}else if(ca > trpm + 40){\r
- speed -= 2;\r
- //if(speed < ispeed) speed = ispeed;\r
- zMotorSet("Left cannon" ,-speed,2);\r
- zMotorSet("Right cannon", speed,2);\r
+ speed --;\r
+ motorSetK(CANNON_LEFT,-speed,2);\r
+ motorSetK(CANNON_RIGHT,speed,2);\r
cannReady = false;\r
- }else{\r
+ }else\r
cannReady = true;\r
- //ispeed = speed;\r
- }\r
-\r
- lcdPrint(uart1,2,"%.0lf|%.3lf\n",trpm,rpm);\r
- //printf("%.0lf,%.3lf,%d\n",trpm,rpm,speed);\r
\r
delay(100);\r
}\r
\r
- zMotorSet("Right cannon",0,2);\r
+ motorSetK(CANNON_LEFT,0,2);\r
+ motorSetK(CANNON_RIGHT,0,2);\r
+\r
+ motorFree(CANNON_LEFT);\r
+ motorFree(CANNON_RIGHT);\r
+\r
+ while(keyDown(*kill))\r
+ delay(100);\r
\r
- zMotorReturn("Left cannon");\r
- zMotorReturn("Right cannon");\r
+ taskCan = NULL;\r
}\r
\r
-/**\r
- * The Ball-Pusher process.\r
- *\r
- * This process will handle the pusher that actually sends the ball into the\r
- * cannon through either an instant push or a detected one, in which the lift\r
- * is ran until a ball is seen via a light sensor.\r
- */\r
+void taskArmCode(void *unused){\r
+ if(keyDown(c[1].left.front.r))\r
+ goto PUSH;\r
\r
-void armProc(void *procPtr){\r
- static unsigned int start;\r
+ while(!underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT) && ((rpm < trpm - 30) | (rpm > trpm + 50)))\r
+ delay(100);\r
\r
- armProcRun = true;\r
+ /*while(ballPos != 5)\r
+ delay(100);*/\r
\r
- /*\r
- * Claim necessary motors.\r
- */\r
+PUSH:\r
\r
- zMotorTake("Lift 1",3);\r
- zMotorTake("Lift 2",3);\r
- zMotorTake("Misc",3);\r
+ motorTake(LIFT_PUSHER,3);\r
\r
- /*\r
- * Collect the time we started this operation, but negate it so we can add\r
- * the finish millis() call to collect a difference in milliseconds.\r
- */\r
+ motorSetK(LIFT_PUSHER,127,3);\r
+ delay(500);\r
+ motorSetK(LIFT_PUSHER,-127,3);\r
+ delay(800);\r
+ motorSetK(LIFT_PUSHER,0,3);\r
\r
- start = -millis();\r
+ motorFree(LIFT_PUSHER);\r
\r
- /*\r
- * Just run the pusher if it was requested by the user.\r
- */\r
+ taskArm = NULL;\r
+}\r
\r
- if(zJoyDigital(1,8,JOY_DOWN))\r
- goto PUSH;\r
\r
- /*\r
- * Run the lift and wait until a ball is detected by the light sensor. Once\r
- * a ball is seen, stop the lift so that the pusher can do its job.\r
- */\r
+void taskLCDCode(void *unused){\r
+ static unsigned int pos = 1;\r
+ unsigned int lcdInput;\r
+ static double cangle = 0;\r
\r
- zMotorSet("Lift 1",127,3);\r
- zMotorSet("Lift 2",127,3);\r
+ while(1){\r
\r
- while(armProcRun && analogRead(8) > lightThresh)\r
- delay(10);\r
+ switch(pos){\r
+ case 0:\r
+ lcdPrint(LCD_PORT,1,"(%.2lf,%.2lf)",xpos,ypos);\r
+ lcdPrint(LCD_PORT,2,"%s: %u",taskGetState(taskLift) == TASK_RUNNING ? "Running" : "Stopped",ballPos);\r
+ break;\r
+ case 1:\r
+ lcdPrint(LCD_PORT,1,"%.0lf | %.2lf\n",trpm,rpm);\r
+ lcdPrint(LCD_PORT,2," ");\r
+ break;\r
+ case 2:\r
+ lcdPrint(LCD_PORT,1,"%u %u %u %u",\r
+ taskGetState(taskPos),\r
+ taskGetState(taskCan),\r
+ taskGetState(taskArm),\r
+ taskGetState(taskLift));\r
+ break;\r
+ case 3:\r
+ lcdPrint(LCD_PORT,1,"%lf",cangle);\r
+ lcdPrint(LCD_PORT,2," ");\r
+ break;\r
+ }\r
\r
- if(!armProcRun)\r
- return;\r
+ lcdInput = lcdReadButtons(LCD_PORT);\r
\r
- delay(300);\r
+ if((lcdInput & LCD_BTN_LEFT) && pos)\r
+ pos--;\r
+ else if((lcdInput & LCD_BTN_RIGHT) && pos < 3)\r
+ pos++;\r
+ else if(lcdInput & LCD_BTN_CENTER)\r
+ softwareReset();\r
\r
- zMotorSet("Lift 1",0,3);\r
- zMotorSet("Lift 2",0,3);\r
+ delay(500);\r
+ }\r
+}\r
\r
- /*\r
- * Push a ball into the cannon.\r
- */\r
-PUSH:\r
+#ifdef AUTO_SKILLS\r
\r
- zMotorSet("Misc",127,3);\r
- delay(500);\r
- zMotorSet("Misc",-127,3);\r
- delay(500);\r
- zMotorSet("Misc",0,3);\r
+/*****************************************************************************\r
+ * AUTONOMOUS - SKILLS *\r
+ *****************************************************************************/\r
\r
- /*\r
- * 'Stop' the timer and print the result. Do this on the first line in case\r
- * the RPM tracker or another process is using the second.\r
- */\r
+void autonomous(){\r
+ static unsigned long inc = 0;\r
\r
- start += millis();\r
- lcdPrint(uart1,1,"%ums",start);\r
+ EXTRA_TRPM;\r
+ intakeLiftTop.initial = readSensor(&intakeLiftTop);\r
\r
- /*\r
- * Release the used motors.\r
- */\r
+ taskInit(taskPos,&No);\r
+ taskInit(taskAim,&No);\r
+ taskInit(taskCan,&No);\r
\r
- zMotorReturn("Lift 1");\r
- zMotorReturn("Lift 2");\r
- zMotorReturn("Misc");\r
+ while(1){\r
\r
- armProcRun = false;\r
-}\r
+ if(++inc == 50){\r
+ inc = 0;\r
+ lcdPrint(LCD_PORT,1,"%.0lf RPM",rpm);\r
+ }\r
\r
-/**\r
- * The LCD update function, registered at init time to be called by libZephyr's\r
- * LCD code.\r
- */\r
+ //readSensor(&intakeLiftTop);\r
+ //if(rpm >= trpm){// && underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT)){\r
+ if(rpm > trpm - 30 && rpm < trpm + 50){\r
\r
-void lcdUpdateFunc(void *unused_param){\r
- while(1){\r
- /*\r
- * Track elapsed time since operatorControl() entrance.\r
- */\r
+ lcdPrint(LCD_PORT,2,"Launch!");\r
\r
- //elapsed = millis() - opmillis;\r
- //lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));\r
- //lcdPrint(uart1,1,"%.3lf V",(float)analogRead(1)/70/4);\r
+ delay(500);\r
+\r
+ motorSet(LIFT_PUSHER,127);\r
+ delay(500);\r
+ motorSet(LIFT_PUSHER,-127);\r
+ delay(800);\r
+ motorSet(LIFT_PUSHER,0);\r
\r
- //lcdPrint(uart1,2,"%4.0lf/%4.0lf",trpm,rpm);\r
- delay(LCD_RATE);\r
+ lcdPrint(LCD_PORT,2," ");\r
+ }\r
+\r
+ delay(10);\r
}\r
}\r
\r
+#else\r
+\r
+/*****************************************************************************\r
+ * AUTONOMOUS - NORMAL *\r
+ *****************************************************************************/\r
+\r
void autonomous(){\r
- static unsigned long elapsed = 0;\r
- opmillis = millis();\r
- taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+ static unsigned long start,elapsed = 0;\r
+ static unsigned long inc = 0;\r
+\r
+ EXTRA_TRPM;\r
+ start = millis();\r
+ intakeLiftTop.initial = readSensor(&intakeLiftTop);\r
+\r
+ taskInit(taskCan,&No);\r
+\r
+ motorSet(LIFT_1,127);\r
+ motorSet(LIFT_2,127);\r
+ //motorSet(INTAKE_1,127);\r
+ //motorSet(INTAKE_2,127);\r
+\r
while(1){\r
- elapsed = millis() - opmillis;\r
- lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));\r
+ elapsed = millis() - start;\r
\r
- if(cannReady){\r
- zMotorSet("Misc",127,0);\r
- delay(500);\r
- zMotorSet("Misc",-127,0);\r
- delay(500);\r
- zMotorSet("Misc",0,0);\r
+ if(++inc == 50){\r
+ inc = 0;\r
+ lcdPrint(LCD_PORT,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));\r
+ lcdPrint(LCD_PORT,2,"%.0lf RPM",rpm);\r
+ }\r
+\r
+ readSensor(&intakeLiftTop);\r
+ if(underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT)){\r
+\r
+\r
+ delay(200);\r
+ motorSet(LIFT_1,0);\r
+ motorSet(LIFT_2,0);\r
+ //motorSet(INTAKE_1,0);\r
+ //motorSet(INTAKE_2,0);\r
+\r
+ if(rpm >= trpm){\r
+ motorSet(LIFT_PUSHER,127);\r
+ delay(500);\r
+ motorSet(LIFT_PUSHER,-127);\r
+ delay(500);\r
+ motorSet(LIFT_PUSHER,0);\r
+\r
+ motorSet(LIFT_1,127);\r
+ motorSet(LIFT_2,127);\r
+ //motorSet(INTAKE_1,127);\r
+ //motorSet(INTAKE_2,127);\r
+ }\r
}\r
+\r
delay(10);\r
}\r
}\r
+\r
+#endif // AUTO_SKILLS\r
--- /dev/null
+#include <main.h>
+
+/**
+ * Cause the Cortex to reset, which results in the Cortex restarting the
+ * operator control code.
+ *
+ * This reset is accomplished through setting two bits, SYSRESETREQ and
+ * VECTRESET, in the Application Interrupt and Reset Control Register (AIRCR),
+ * which is at a known location in memory. SYSRESETREQ will actually request
+ * the system reset, while VECTRESET is 'reserved for debugging'. This second
+ * bit may not need to be set; it's only set here because others have found it
+ * necessary.
+ */
+
+#define AIRCR_ADDR 0xE000ED0C
+#define VECTKEY 0x05FA
+#define SYSRESETREQ (1<<2)
+#define VECTRESET (1<<0)
+
+void softwareReset(void){
+
+ /*
+ * Read the current value of the AIRCR, since some flags currently set in
+ * the register need to remain the same in order for the reset to work.
+ */
+
+ uint32_t AIRCR = *((uint32_t *)AIRCR_ADDR);
+
+ /*
+ * Here we save the lower 16 bits of the register, write a special key to
+ * the higher 16 bits that'll allow the write to occur, and then set the
+ * reset flags.
+ */
+
+ AIRCR = (AIRCR & 0xFFFF) | (VECTKEY << 16) | SYSRESETREQ | VECTRESET;
+
+ // Update the AIRCR.
+
+ *((uint32_t *)0xE000ED0C) = AIRCR;
+
+ /*
+ * This instruction causes the program to wait until the previous memory
+ * write is complete, ensuring it is taken into effect and the reset
+ * request is made.
+ */
+
+ asm("DSB");
+
+ // Wait until the system reset completes.
+
+ while(1);
+}
--- /dev/null
+#include <main.h>
+
+/**
+ * Nowhere else to put this...?
+ */
+
+unsigned char MOTOR_USE_MAP[10] = {
+ 0,0,0,0,0,0,0,0,0,0
+};
+
+int getIMEPort(unsigned int port){
+ switch(port){
+ case CANNON_LEFT : return CANNON_LEFT_IME;
+ case CANNON_RIGHT : return CANNON_RIGHT_IME;
+ case LIFT_PUSHER :
+ case INTAKE_1 :
+ case INTAKE_2 : return -1;
+ case DRIVE_RIGHT : return DRIVE_RIGHT_IME;
+ case DRIVE_LEFT : return DRIVE_LEFT_IME;
+ case LIFT_1 : return LIFT_1_IME;
+ case LIFT_2 : return LIFT_2_IME;
+ case LIFT_ROTATER : return LIFT_ROTATER_IME;
+ }
+ return -1;
+}
+
+int getIME(unsigned int port){
+ int ret;
+ imeGet(port,&ret);
+ return ret;
+}
+
+int getIMEVelocity(unsigned int port){
+ int ret;
+ imeGetVelocity(port,&ret);
+ return ret;
+}
+
+/**
+ * Actual sensor stuffs
+ */
+
+Sensor initSensor(uint32_t port,unsigned char type){
+ Sensor s;
+ s.type = type;
+ switch(s.type){
+ case DIGITAL:
+ pinMode(port,INPUT);
+ s.port = port;
+ //s.initial = digitalRead(port);
+ break;
+ case ANALOG:
+ pinMode(port + 13,INPUT_ANALOG);
+ s.port = port;
+ //s.initial = analogRead(port);
+ break;
+ case GYRO:
+ s.port = 0;
+ s.data.gyro = gyroInit(port,0);
+ //s.initial = gyroGet(s.data.gyro);
+ break;
+ case ULTRASONIC:
+ s.port = 0;
+ s.data.sonic = ultrasonicInit((uint16_t)port,port>>16);
+ //s.initial = ultrasonicGet(s.data.sonic);
+ break;
+ }
+ s.value = 0;
+ return s;
+}
+
+int readSensor(Sensor *s){
+ switch(s->type){
+ case DIGITAL:
+ return (s->value = digitalRead(s->port));
+ break;
+ case ANALOG:
+ return (s->value = analogRead(s->port));
+ break;
+ case GYRO:
+ return (s->value = gyroGet(s->data.gyro));
+ break;
+ case ULTRASONIC:
+ return (s->value = ultrasonicGet(s->data.sonic));
+ break;
+ }
+ return (s->value = -1);
+}
+++ /dev/null
-#include <zephyr.h>
-#include <main.h>
-
-#include <string.h>
-
-#ifdef LCD_PORT
-#endif // LCD_PORT
-
-#ifdef GYRO_PORT
-
-static Gyro gyro;
-static bool gyroEnabled = false;
-
-void zGyroInit(void){
- if((gyro=gyroInit(2,0))){
- gyroEnabled = true;
- }
-}
-
-int zGyroGet(void){
- return gyroGet(gyro);
-}
-
-void zGyroReset(void){
- gyroReset(gyro);
-}
-
-#endif // GYRO_PORT
-
-/*
- * A map of what's plugged into the motor ports.
- * Expected declarations:
- *
- * DRIVEL - Left drive port
- * DRIVER - Right drive port
- *
-*/
-
-#define MOTOR_PORT_COUNT 10
-
-#ifdef IME_ENABLE
-#define MOTOR_IME_COUNT 7
-#endif // IME_ENABLE
-
-const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = {
- "Left cannon",
- "Right cannon",
- "Misc",
- "Intake",
- "Intake",
- "Right drive",
- "Left drive",
- "Lift 1",
- "Lift 2",
- "Rotater"
-};
-
-static unsigned int mInUse[10]={
- 0,0,0,0,0,0,0,0,0,0
-};
-
-#ifdef IME_ENABLE
-
-const char *MOTOR_IME_MAP[MOTOR_IME_COUNT] = {
- "Right drive",
- "Left drive",
- "Rotater",
- "Lift 1",
- "Lift 2",
- "Left cannon",
- "Right cannon"
-};
-
-static unsigned int imeCount = 0;
-
-void zIMEInit(void){
- imeCount = imeInitializeAll();
-}
-
-#endif // IME_ENABLE
-
-void zMotorSet(const char *motor,int speed,unsigned int id){
- for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
- if(!strcmp(MOTOR_PORT_MAP[i],motor) && mInUse[i] == id){
- if(speed > 127)speed = 127;
- if(speed < -127)speed = -127;
- motorSet(i+1,speed);
- //return;
- }
- }
-}
-
-char zMotorGet(const char *motor){
- for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
- if(!strcmp(MOTOR_PORT_MAP[i],motor)){
- return motorGet(i+1);
- }
- }
- return 0;
-}
-
-void zMotorTake(const char *motor,unsigned int id){
- for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
- if(!strcmp(MOTOR_PORT_MAP[i],motor)){
- mInUse[i] = id;
- }
- }
-}
-void zMotorReturn(const char *motor){
- for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
- if(!strcmp(MOTOR_PORT_MAP[i],motor)){
- mInUse[i] = 0;
- }
- }
-}
-
-#ifdef IME_ENABLE
-
-int zMotorIMEGet(const char *motor){
- int IMEValue = 0;
- for(unsigned char i=0;i<imeCount;i++){
- if(!strcmp(MOTOR_IME_MAP[i],motor)){
- imeGet(i,&IMEValue);
- break;
- }
- }
- return IMEValue;
-}
-
-int zMotorIMEGetVelocity(const char *motor){
- int IMEValue = 0;
- for(unsigned char i=0;i<imeCount;i++){
- if(!strcmp(MOTOR_IME_MAP[i],motor)){
- imeGetVelocity(i,&IMEValue);
- break;
- }
- }
- return IMEValue;
-}
-
-bool zMotorIMEReset(const char *motor){
- for(unsigned char i=0;i<imeCount;i++){
- if(!strcmp(MOTOR_IME_MAP[i],motor)){
- return imeReset(i);
- }
- }
- return false;
-}
-
-#endif // IME_ENABLE
-
-void zDriveUpdate(void){
-
-#ifdef DRIVE_NORMAL
- char y = joystickGetAnalog(DRIVE_JOY,DRIVE_NORMAL);
- char x = joystickGetAnalog(DRIVE_JOY,4);
-
- APPLY_THRESH(x,DRIVE_THRESHOLD);
- APPLY_THRESH(y,DRIVE_THRESHOLD);
-
- zMotorSet("Left drive" ,y+x,0);
- zMotorSet("Right drive",y-x,0);
-
-#else
-
- char l,r;
-
- l = joystickGetAnalog(DRIVE_JOY,DRIVE_TANK_LEFT);
- r = joystickGetAnalog(DRIVE_JOY,DRIVE_TANK_RIGHT);
-
- APPLY_THRESH(l,DRIVE_THRESHOLD);
- APPLY_THRESH(r,DRIVE_THRESHOLD);
-
- zMotorSet("Left drive",l,0);
- zMotorSet("Right drive",r,0);
-
-#endif // DRIVE_NORMAL
-
-}
-
-char zGetDigitalMotorSpeed(unsigned char joy,unsigned char btn_group,unsigned char btn_pos,unsigned char btn_neg,char speed){
- return joystickGetDigital(joy,btn_group,btn_pos) ? speed :
- joystickGetDigital(joy,btn_group,btn_neg) ? -speed : 0;
-}