\r
# Compiler options here.\r
ifeq ($(USE_OPT),)\r
- USE_OPT = -std=gnu99 -O2 -ggdb -fomit-frame-pointer -falign-functions=16 -fsingle-precision-constant\r
+ USE_OPT = -O2 -ggdb -fomit-frame-pointer -falign-functions=16 -fsingle-precision-constant \r
endif\r
\r
# C specific options here (added to USE_OPT).\r
ifeq ($(USE_COPT),)\r
- USE_COPT = \r
+ USE_COPT = -std=gnu99\r
endif\r
\r
# C++ specific options here (added to USE_OPT).\r
ifeq ($(USE_CPPOPT),)\r
- USE_CPPOPT = -fno-rtti\r
+ USE_CPPOPT = -fno-rtti \r
endif\r
\r
# Enable this if you want the linker to remove unused code and data\r
\r
# Enable this if you want to see the full log while compiling.\r
ifeq ($(USE_VERBOSE_COMPILE),)\r
- USE_VERBOSE_COMPILE = no\r
+ USE_VERBOSE_COMPILE = yes\r
endif\r
\r
#\r
$(CHIBIOS)/os/various/syscalls.c \\r
$(CHIBIOS)/os/various/chprintf.c \\r
$(VEXFWSRC) \\r
- $(VEXOPTSRC) \\r
- $(VEXUSERSRC) \\r
- main.c\r
+ $(VEXOPTSRC)\r
+# $(VEXUSERSRC) \\r
+# main.c\r
\r
# C++ sources that can be compiled in ARM or THUMB mode depending on the global\r
# setting.\r
-CPPSRC =\r
+CPPSRC = $(VEXUSERSRC) \\r
+ main.cpp\r
\r
# C sources to be compiled in ARM mode regardless of the global setting.\r
# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler\r
# Enable loading with g++ only if you need C++ runtime support.\r
# NOTE: You can use C++ even without C++ support if you are careful. C++\r
# runtime support makes code size explode.\r
-LD = $(TRGT)gcc\r
-#LD = $(TRGT)g++\r
+#D = $(TRGT)gcc\r
+LD = $(TRGT)g++\r
CP = $(TRGT)objcopy\r
AS = $(TRGT)gcc -x assembler-with-cpp\r
OD = $(TRGT)objdump\r
CWARN = -Wall -Wextra -Wstrict-prototypes\r
\r
# Define C++ warning options here\r
-CPPWARN = -Wall -Wextra\r
+CPPWARN = -Wall -Wextra -Wno-write-strings\r
\r
#\r
# Compiler settings\r
--- /dev/null
+#ifndef CONTROL_H_
+#define CONTROL_H_
+
+#include <stdint.h>
+
+#include "ch.h"
+#include "hal.h"
+#include "vex.h"
+
+typedef enum {
+ BUTTON_UP,
+ BUTTON_RELEASE,
+ BUTTON_DOWN
+} button_t;
+
+struct joydata_t {
+ int Ch1 :8;
+ int Ch2 :8;
+ int Ch3 :8;
+ int Ch4 :8;
+
+ struct accel_t {
+ int y :8;
+ int x :8;
+ int z :8;
+ } __attribute__ ((packed)) accel;
+
+ char Btn5D :1;
+ char Btn5U :1;
+ char Btn6D :1;
+ char Btn6U :1;
+
+ char Reserved :4;
+
+ char Btn8D :1;
+ char Btn8L :1;
+ char Btn8U :1;
+ char Btn8R :1;
+ char Btn7D :1;
+ char Btn7L :1;
+ char Btn7U :1;
+ char Btn7R :1;
+
+ char Reserved2[2];
+
+} __attribute__ ((packed));
+
+struct Controller {
+private:
+ int index;
+ struct joydata_t* data;
+
+public:
+ Controller(int idx = 1)
+ : index(idx), data(nullptr) {}
+ ~Controller(void) {}
+
+ void update(void) {
+ data = reinterpret_cast<struct joydata_t*>(vexSpiGetJoystickDataPtr(index));
+ data->Ch1 = vexControllerGet(Ch1);//( data->Ch1 == 0xFF ) ? 127 : data->Ch1 - 127;
+ data->Ch2 = vexControllerGet(Ch2);//-(( data->Ch2 == 0xFF ) ? 127 : data->Ch2 - 127);
+ data->Ch3 = vexControllerGet(Ch3);//-(( data->Ch3 == 0xFF ) ? 127 : data->Ch3 - 127);
+ data->Ch4 = vexControllerGet(Ch4);//( data->Ch4 == 0xFF ) ? 127 : data->Ch4 - 127;
+ }
+
+ inline const struct joydata_t* operator->(void) const
+ { return data; }
+
+} __attribute__ ((packed));
+
+#endif // CONTROL_H_
--- /dev/null
+#ifndef MOTOR_H_
+#define MOTOR_H_
+
+#include "ch.h"
+#include "hal.h"
+#include "vex.h"
+
+typedef enum {
+ /* 1 */ mClawThingy = kVexMotor_1,
+ /* 2 */ mLiftHighLeft,
+ /* 3 */ mLiftLowLeft,
+ /* 4 */ mLiftHighRight,
+ /* 5 */ mLiftLowRight,
+ /* 6 */ mDriveFrontLeft,
+ /* 7 */ mDriveBackLeft,
+ /* 8 */ mDriveBackRight,
+ /* 9 */ mDriveFrontRight,
+ /* 10 */ mPickupThingy,
+} motor_port_t;
+
+constexpr const tVexImeChannels iLiftLowLeft = kImeChannel_1;
+constexpr const tVexImeChannels iLiftHighLeft = kImeChannel_2;
+constexpr const tVexImeChannels iLiftLowRight = kImeChannel_3;
+constexpr const tVexImeChannels iLiftHighRight = kImeChannel_4;
+
+#define DIGI_CFG_DIGI_OUT(p) { kVexDigital_##p, kVexSensorDigitalOutput, kVexConfigOutput, 0 }
+#define DIGI_CFG_DIGI_IN(p) { kVexDigital_##p, kVexSensorDigitalInput, kVexConfigInput, 0 }
+
+#define MOTOR_CFG_MOT(p, t, r) (tVexMotor)p, kVexMotor##t, kVexMotor##r
+#define MOTOR_CFG_NOIME kVexSensorNone, 0
+#define MOTOR_CFG_IME(c) kVexSensorIME, c
+
+#endif // MOTOR_H_
+++ /dev/null
-/*-----------------------------------------------------------------------------*/\r
-/* */\r
-/* Copyright (c) James Pearman */\r
-/* 2013 */\r
-/* All Rights Reserved */\r
-/* */\r
-/*-----------------------------------------------------------------------------*/\r
-/* */\r
-/* Module: vexmain.c */\r
-/* Author: James Pearman */\r
-/* Created: 7 May 2013 */\r
-/* */\r
-/* Revisions: */\r
-/* V1.00 04 July 2013 - Initial release */\r
-/* */\r
-/*-----------------------------------------------------------------------------*/\r
-/* */\r
-/* The author is supplying this software for use with the VEX cortex */\r
-/* control system. This file can be freely distributed and teams are */\r
-/* authorized to freely use this program , however, it is requested that */\r
-/* improvements or additions be shared with the Vex community via the vex */\r
-/* forum. Please acknowledge the work of the authors when appropriate. */\r
-/* Thanks. */\r
-/* */\r
-/* Licensed under the Apache License, Version 2.0 (the "License"); */\r
-/* you may not use this file except in compliance with the License. */\r
-/* You may obtain a copy of the License at */\r
-/* */\r
-/* http://www.apache.org/licenses/LICENSE-2.0 */\r
-/* */\r
-/* Unless required by applicable law or agreed to in writing, software */\r
-/* distributed under the License is distributed on an "AS IS" BASIS, */\r
-/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */\r
-/* See the License for the specific language governing permissions and */\r
-/* limitations under the License. */\r
-/* */\r
-/* The author can be contacted on the vex forums as jpearman */\r
-/* or electronic mail using jbpearman_at_mac_dot_com */\r
-/* Mentor for team 8888 RoboLancers, Pasadena CA. */\r
-/* */\r
-/*-----------------------------------------------------------------------------*/\r
-\r
-\r
-#include <string.h>\r
-\r
-#include "ch.h"\r
-#include "hal.h"\r
-#include "chprintf.h"\r
-#include "vex.h"\r
-\r
-/*-----------------------------------------------------------------------------*/\r
-/* Command line related. */\r
-/*-----------------------------------------------------------------------------*/\r
-\r
-#define SHELL_WA_SIZE THD_WA_SIZE(512)\r
-\r
-// Shell command\r
-static const ShellCommand commands[] = {\r
- {"adc", vexAdcDebug },\r
- {"spi", vexSpiDebug },\r
- {"motor", vexMotorDebug},\r
- {"lcd", vexLcdDebug},\r
- {"enc", vexEncoderDebug},\r
- {"son", vexSonarDebug},\r
- {"ime", vexIMEDebug},\r
- {"test", vexTestDebug},\r
- {NULL, NULL}\r
-};\r
-\r
-// configuration for the shell\r
-static const ShellConfig shell_cfg1 = {\r
- (vexStream *)SD_CONSOLE,\r
- commands\r
-};\r
-\r
-/*-----------------------------------------------------------------------------*/\r
-// Application entry point. */\r
-/*-----------------------------------------------------------------------------*/\r
-\r
-int main(void)\r
-{\r
- Thread *shelltp = NULL;\r
- short timeout = 0;\r
-\r
- // System initializations.\r
- // - HAL initialization, this also initializes the configured device drivers\r
- // and performs the board-specific initializations.\r
- // - Kernel initialization, the main() function becomes a thread and the\r
- // RTOS is active.\r
- halInit();\r
- chSysInit();\r
-\r
- // Init the serial port associated with the console\r
- vexConsoleInit();\r
-\r
- // init VEX\r
- vexCortexInit();\r
-\r
- // wait for good spi comms\r
- while( vexSpiGetOnlineStatus() == 0 )\r
- {\r
- // wait for a while\r
- chThdSleepMilliseconds(100);\r
- // dump after 5 seconds\r
- if(timeout++ == 50)\r
- break;\r
- }\r
-\r
- // Shell manager initialization.\r
- shellInit();\r
-\r
- // spin in loop monitoring the shell\r
- while (TRUE)\r
- {\r
- if (!shelltp)\r
- shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);\r
- else\r
- if (chThdTerminated(shelltp))\r
- {\r
- chThdRelease(shelltp); /* Recovers memory of the previous shell. */\r
- shelltp = NULL; /* Triggers spawning of a new shell. */\r
- }\r
-\r
- chThdSleepMilliseconds(50);\r
- }\r
-}\r
--- /dev/null
+/*-----------------------------------------------------------------------------*/\r
+/* */\r
+/* Copyright (c) James Pearman */\r
+/* 2013 */\r
+/* All Rights Reserved */\r
+/* */\r
+/*-----------------------------------------------------------------------------*/\r
+/* */\r
+/* Module: vexmain.c */\r
+/* Author: James Pearman */\r
+/* Created: 7 May 2013 */\r
+/* */\r
+/* Revisions: */\r
+/* V1.00 04 July 2013 - Initial release */\r
+/* */\r
+/*-----------------------------------------------------------------------------*/\r
+/* */\r
+/* The author is supplying this software for use with the VEX cortex */\r
+/* control system. This file can be freely distributed and teams are */\r
+/* authorized to freely use this program , however, it is requested that */\r
+/* improvements or additions be shared with the Vex community via the vex */\r
+/* forum. Please acknowledge the work of the authors when appropriate. */\r
+/* Thanks. */\r
+/* */\r
+/* Licensed under the Apache License, Version 2.0 (the "License"); */\r
+/* you may not use this file except in compliance with the License. */\r
+/* You may obtain a copy of the License at */\r
+/* */\r
+/* http://www.apache.org/licenses/LICENSE-2.0 */\r
+/* */\r
+/* Unless required by applicable law or agreed to in writing, software */\r
+/* distributed under the License is distributed on an "AS IS" BASIS, */\r
+/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */\r
+/* See the License for the specific language governing permissions and */\r
+/* limitations under the License. */\r
+/* */\r
+/* The author can be contacted on the vex forums as jpearman */\r
+/* or electronic mail using jbpearman_at_mac_dot_com */\r
+/* Mentor for team 8888 RoboLancers, Pasadena CA. */\r
+/* */\r
+/*-----------------------------------------------------------------------------*/\r
+\r
+\r
+#include <string.h>\r
+\r
+#include "ch.h"\r
+#include "hal.h"\r
+#include "chprintf.h"\r
+#include "vex.h"\r
+\r
+/*-----------------------------------------------------------------------------*/\r
+/* Command line related. */\r
+/*-----------------------------------------------------------------------------*/\r
+\r
+#define SHELL_WA_SIZE THD_WA_SIZE(512)\r
+\r
+// Shell command\r
+static const ShellCommand commands[] = {\r
+ {"adc", vexAdcDebug },\r
+ {"spi", vexSpiDebug },\r
+ {"motor", vexMotorDebug},\r
+ {"lcd", vexLcdDebug},\r
+ {"enc", vexEncoderDebug},\r
+ {"son", vexSonarDebug},\r
+ {"ime", vexIMEDebug},\r
+ {"test", vexTestDebug},\r
+ {NULL, NULL}\r
+};\r
+\r
+// configuration for the shell\r
+static const ShellConfig shell_cfg1 = {\r
+ (vexStream *)SD_CONSOLE,\r
+ commands\r
+};\r
+\r
+/*-----------------------------------------------------------------------------*/\r
+// Application entry point. */\r
+/*-----------------------------------------------------------------------------*/\r
+\r
+int main(void)\r
+{\r
+ Thread *shelltp = NULL;\r
+ short timeout = 0;\r
+\r
+ // System initializations.\r
+ // - HAL initialization, this also initializes the configured device drivers\r
+ // and performs the board-specific initializations.\r
+ // - Kernel initialization, the main() function becomes a thread and the\r
+ // RTOS is active.\r
+ halInit();\r
+ chSysInit();\r
+\r
+ // Init the serial port associated with the console\r
+ vexConsoleInit();\r
+\r
+ // init VEX\r
+ vexCortexInit();\r
+\r
+ // wait for good spi comms\r
+ while( vexSpiGetOnlineStatus() == 0 )\r
+ {\r
+ // wait for a while\r
+ chThdSleepMilliseconds(100);\r
+ // dump after 5 seconds\r
+ if(timeout++ == 50)\r
+ break;\r
+ }\r
+\r
+ // Shell manager initialization.\r
+ shellInit();\r
+\r
+ // spin in loop monitoring the shell\r
+ while (TRUE)\r
+ {\r
+ if (!shelltp)\r
+ shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);\r
+ else\r
+ if (chThdTerminated(shelltp))\r
+ {\r
+ chThdRelease(shelltp); /* Recovers memory of the previous shell. */\r
+ shelltp = NULL; /* Triggers spawning of a new shell. */\r
+ }\r
+\r
+ chThdSleepMilliseconds(50);\r
+ }\r
+}\r
#CONVEX_OPT = yes\r
\r
# User C code files\r
-VEXUSERSRC = vexuser.c\r
+VEXUSERSRC = vexuser.cpp src/motor.cpp \r
\r
# Uncomment and add/modify user include files\r
-#VEXUSERINC = myfile.h\r
+VEXUSERINC = include/\r
\r
--- /dev/null
+#include <motor.h>
+
+
+++ /dev/null
-#include <stdlib.h>\r
-#include <math.h>\r
-\r
-#define DIGI_CFG_DIGI_OUT(p) { kVexDigital_##p, kVexSensorDigitalOutput, kVexConfigOutput, 0 }\r
-#define DIGI_CFG_DIGI_IN(p) { kVexDigital_##p, kVexSensorDigitalInput, kVexConfigInput, 0 }\r
-\r
-#define MOTOR_CFG_MOT(p, t, r) p, kVexMotor##t, kVexMotor##r\r
-#define MOTOR_CFG_NOIME kVexSensorNone, 0\r
-#define MOTOR_CFG_IME(c) kVexSensorIME, kImeChannel_##c\r
-\r
-#include "ch.h" // needs for all ChibiOS programs\r
-#include "hal.h" // hardware abstraction layer header\r
-#include "vex.h" // vex library header\r
-\r
-// motor ports\r
-#define mDriveFrontLeft kVexMotor_6\r
-#define mDriveFrontRight kVexMotor_9\r
-#define mDriveBackLeft kVexMotor_7\r
-#define mDriveBackRight kVexMotor_8\r
-#define mLiftLowRight kVexMotor_5\r
-#define mLiftHighRight kVexMotor_4\r
-#define mLiftLowLeft kVexMotor_3\r
-#define mLiftHighLeft kVexMotor_2\r
-#define mPickupThingy kVexMotor_10\r
-#define mClawThingy kVexMotor_1\r
-\r
-// Digi IO configuration\r
-static vexDigiCfg dConfig[] = {\r
-};\r
-\r
-static vexMotorCfg mConfig[] = {\r
- { MOTOR_CFG_MOT(mClawThingy, 393T, Normal), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mDriveFrontLeft, 393T, Normal), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mDriveFrontRight, 393T, Reversed), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mDriveBackLeft, 393T, Reversed), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mDriveBackRight, 393T, Normal), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mLiftLowRight, 393T, Normal), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mLiftHighRight, 393T, Normal), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mLiftLowLeft, 393T, Normal), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mLiftHighLeft, 393T, Normal), MOTOR_CFG_NOIME },\r
- { MOTOR_CFG_MOT(mPickupThingy, 393T, Normal), MOTOR_CFG_NOIME }\r
-};\r
-\r
-void vexUserSetup(void)\r
-{\r
- vexDigitalConfigure(dConfig, DIG_CONFIG_SIZE(dConfig));\r
- vexMotorConfigure(mConfig, MOT_CONFIG_SIZE(mConfig));\r
-}\r
-\r
-void vexUserInit(void)\r
-{}\r
-\r
-msg_t vexAutonomous(void* arg)\r
-{\r
- (void)arg;\r
-\r
- vexTaskRegister("auton");\r
-\r
- vexMotorSet(mClawThingy, -127);\r
- vexMotorSet(mPickupThingy, -64);\r
- vexSleep(300);\r
- vexMotorSet(mClawThingy, 0);\r
- vexMotorSet(mPickupThingy, 0);\r
-\r
- vexMotorSet(mDriveFrontLeft, 127);\r
- vexMotorSet(mDriveFrontRight, 127);\r
- vexMotorSet(mDriveBackLeft, 127);\r
- vexMotorSet(mDriveBackRight, 127);\r
- vexSleep(3000);\r
- vexMotorSet(mDriveFrontLeft, -30);\r
- vexMotorSet(mDriveFrontRight, -30);\r
- vexMotorSet(mDriveBackLeft, -30);\r
- vexMotorSet(mDriveBackRight, -30);\r
- vexSleep(1000);\r
- vexMotorSet(mDriveFrontLeft, 0);\r
- vexMotorSet(mDriveFrontRight, 0);\r
- vexMotorSet(mDriveBackLeft, 0);\r
- vexMotorSet(mDriveBackRight, 0);\r
-\r
- while(1)\r
- vexSleep(25);\r
-\r
- return (msg_t)0;\r
-}\r
-\r
-typedef struct {\r
- int x, y;\r
-} __attribute__ ((packed)) joy_t;\r
-\r
-int doubleButton(int btnp, int btnn, int speed);\r
-\r
-msg_t vexOperator(void* arg)\r
-{\r
- static joy_t joyLeft, joyRight;\r
-\r
- (void)arg;\r
-\r
- vexTaskRegister("operator");\r
-\r
- while(!chThdShouldTerminate()) {\r
-\r
- // control update\r
- joyLeft = (joy_t) {vexControllerGet(Ch4), vexControllerGet(Ch3)};\r
- joyRight = (joy_t) {vexControllerGet(Ch1), vexControllerGet(Ch2)};\r
-\r
- // drive motors\r
- vexMotorSet(mDriveFrontLeft, joyLeft.y + joyLeft.x);\r
- vexMotorSet(mDriveFrontRight, joyLeft.y - joyLeft.x);\r
- vexMotorSet(mDriveBackLeft, joyLeft.y + joyLeft.x);\r
- vexMotorSet(mDriveBackRight, joyLeft.y - joyLeft.x);\r
-\r
- // lift motors\r
- vexMotorSet(mLiftLowRight, joyRight.y);\r
- vexMotorSet(mLiftHighRight, joyRight.y);\r
- vexMotorSet(mLiftLowLeft, joyRight.y);\r
- vexMotorSet(mLiftHighLeft, joyRight.y);\r
-\r
- // lift thingy\r
- vexMotorSet(mPickupThingy, doubleButton(Btn5U, Btn5D, 64));\r
-\r
- // claw thingy\r
- vexMotorSet(mClawThingy, doubleButton(Btn6U, Btn6D, 127));\r
-\r
- vexSleep(25);\r
- }\r
-\r
- return (msg_t)0;\r
-}\r
-\r
-\r
-int doubleButton(int btnp, int btnn, int speed)\r
-{\r
- return (vexControllerGet(btnp) ? speed : (vexControllerGet(btnn) ? -speed : 0));\r
-}\r
--- /dev/null
+#include <unistd.h>\r
+#include <array>\r
+\r
+#include "ch.h" // needs for all ChibiOS programs\r
+#include "hal.h" // hardware abstraction layer header\r
+#include "vex.h" // vex library header\r
+\r
+#include <motor.h>\r
+#include <control.h>\r
+\r
+//static WORKING_AREA(waVexIME, 512);\r
+//static msg_t vexIME(void *);\r
+\r
+// Digi IO configuration\r
+static vexDigiCfg dConfig[] = {\r
+};\r
+\r
+static vexMotorCfg mConfig[] = {\r
+ { MOTOR_CFG_MOT(mClawThingy, 393T, Normal), MOTOR_CFG_NOIME },\r
+ { MOTOR_CFG_MOT(mDriveFrontLeft, 393T, Normal), MOTOR_CFG_NOIME },\r
+ { MOTOR_CFG_MOT(mDriveFrontRight, 393T, Reversed), MOTOR_CFG_NOIME },\r
+ { MOTOR_CFG_MOT(mDriveBackLeft, 393T, Reversed), MOTOR_CFG_NOIME },\r
+ { MOTOR_CFG_MOT(mDriveBackRight, 393T, Normal), MOTOR_CFG_NOIME },\r
+ { MOTOR_CFG_MOT(mLiftLowRight, 393T, Normal), MOTOR_CFG_IME(iLiftLowRight) },\r
+ { MOTOR_CFG_MOT(mLiftHighRight, 393T, Normal), MOTOR_CFG_IME(iLiftHighRight) },\r
+ { MOTOR_CFG_MOT(mLiftLowLeft, 393T, Normal), MOTOR_CFG_IME(iLiftLowLeft) },\r
+ { MOTOR_CFG_MOT(mLiftHighLeft, 393T, Normal), MOTOR_CFG_IME(iLiftHighLeft) },\r
+ { MOTOR_CFG_MOT(mPickupThingy, 393T, Normal), MOTOR_CFG_NOIME }\r
+};\r
+\r
+void vexUserSetup(void)\r
+{\r
+ vexDigitalConfigure(dConfig, DIG_CONFIG_SIZE(dConfig));\r
+ vexMotorConfigure(mConfig, MOT_CONFIG_SIZE(mConfig));\r
+}\r
+\r
+void vexUserInit(void)\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
+ uint32_t AIRCR = *((uint32_t *)AIRCR_ADDR);\r
+ AIRCR = (AIRCR & 0xFFFF) | (VECTKEY << 16) | SYSRESETREQ | VECTRESET;\r
+ *((volatile uint32_t *)0xE000ED0C) = AIRCR;\r
+ asm("DSB");\r
+ while(1);\r
+}\r
+\r
+msg_t vexAutonomous(void* arg)\r
+{\r
+ (void)arg;\r
+\r
+ vexTaskRegister("auton");\r
+\r
+ vexMotorSet(mClawThingy, -127);\r
+ vexMotorSet(mPickupThingy, -64);\r
+ vexSleep(300);\r
+ vexMotorSet(mClawThingy, 0);\r
+ vexMotorSet(mPickupThingy, 0);\r
+\r
+ vexMotorSet(mDriveFrontLeft, -127);\r
+ vexMotorSet(mDriveFrontRight, -127);\r
+ vexMotorSet(mDriveBackLeft, -127);\r
+ vexMotorSet(mDriveBackRight, -127);\r
+ vexSleep(3000);\r
+ vexMotorSet(mDriveFrontLeft, 30);\r
+ vexMotorSet(mDriveFrontRight, 30);\r
+ vexMotorSet(mDriveBackLeft, 30);\r
+ vexMotorSet(mDriveBackRight, 30);\r
+ vexSleep(1000);\r
+ vexMotorSet(mDriveFrontLeft, 0);\r
+ vexMotorSet(mDriveFrontRight, 0);\r
+ vexMotorSet(mDriveBackLeft, 0);\r
+ vexMotorSet(mDriveBackRight, 0);\r
+\r
+ while(1)\r
+ vexSleep(25);\r
+\r
+ return (msg_t)0;\r
+}\r
+\r
+int doubleButton(int btnp, int btnn, int speed);\r
+\r
+msg_t vexOperator(void* arg)\r
+{\r
+ Controller joyMain (1);\r
+\r
+ (void)arg;\r
+\r
+ vexTaskRegister("operator");\r
+\r
+ //chThdCreateStatic(waVexIME, sizeof(waVexIME), NORMALPRIO - 1, vexIME, nullptr);\r
+\r
+ while(!chThdShouldTerminate()) {\r
+\r
+ // control update\r
+ joyMain.update();\r
+\r
+ // drive motors\r
+ int dx = joyMain->Ch4, dy = -joyMain->Ch3;\r
+ vexMotorSet(mDriveFrontLeft, dy - dx);\r
+ vexMotorSet(mDriveFrontRight, dy + dx);\r
+ vexMotorSet(mDriveBackLeft, dy - dx);\r
+ vexMotorSet(mDriveBackRight, dy + dx);\r
+\r
+ // lift motors\r
+#ifndef NEW_LIFT\r
+ int ly = joyMain->Ch2;\r
+ vexMotorSet(mLiftLowRight, ly);\r
+ vexMotorSet(mLiftHighRight, ly);\r
+ vexMotorSet(mLiftLowLeft, ly);\r
+ vexMotorSet(mLiftHighLeft, ly);\r
+#else\r
+ if (joyMain->Btn8U)\r
+ motorCountInc();\r
+ else if (joyMain->Btn7D)\r
+ motorCountDec();\r
+#endif // NEW_LIFT\r
+\r
+ // lift thingy\r
+ vexMotorSet(mPickupThingy, doubleButton(joyMain->Btn5U, joyMain->Btn5D, 64));\r
+\r
+ // claw thingy\r
+ vexMotorSet(mClawThingy, doubleButton(joyMain->Btn6U, joyMain->Btn6D, 127));\r
+\r
+ if (joyMain->Btn8R)\r
+ softwareReset();\r
+\r
+ vexSleep(25);\r
+ }\r
+\r
+ return (msg_t)0;\r
+}\r
+\r
+\r
+int doubleButton(int btnp, int btnn, int speed)\r
+{\r
+ return (btnp ? speed : (btnn ? -speed : 0));\r
+}\r
+\r
+extern "C" {\r
+ void _exit(int code) {\r
+ (void)code;\r
+\r
+ vexLcdPrintf(0, 0, "PANIC: exit(%d)", code);\r
+ vexLcdPrintf(0, 1, "Halting...");\r
+\r
+ while(1);\r
+ }\r
+ void _kill(pid_t pid) {\r
+ (void)pid;\r
+ // no way to kill here\r
+ }\r
+ pid_t _getpid(void) {\r
+ // no pids here\r
+ return 0;\r
+ }\r
+}\r
+\r
+/*using CountTuple = std::tuple<tVexMotor, tVexImeChannels, int32_t>;\r
+\r
+static std::array<CountTuple, 4> MotorCounts = {\r
+ std::make_tuple(mLiftLowLeft, iLiftLowLeft, 0),\r
+ std::make_tuple(mLiftHighLeft, iLiftHighLeft, 0),\r
+ std::make_tuple(mLiftLowRight, iLiftLowRight, 0),\r
+ std::make_tuple(mLiftHighRight, iLiftHighRight, 0)\r
+};\r
+\r
+void\r
+motorCountInc(void)\r
+{\r
+ for (auto &c : MotorCounts)\r
+ std::get<2>(c) += 10;\r
+}\r
+\r
+void\r
+motorCountDec(void)\r
+{\r
+ for (auto &c : MotorCounts)\r
+ std::get<2>(c) -= 10;\r
+}\r
+\r
+static msg_t\r
+vexIME(void *arg)\r
+{\r
+ (void)arg;\r
+\r
+ vexTaskRegister("uime");\r
+\r
+ while (1) {\r
+ for (auto &c : MotorCounts) {\r
+ auto count = vexImeGetPtr(std::get<1>(c))->count;\r
+ auto comp = std::get<2>(c);\r
+\r
+ if (count > comp)\r
+ vexMotorSet(vexMotorGet(std::get<0>(c)) - 2);\r
+ else if(count < comp)\r
+ vexMotorSet(vexMotorGet(std::get<0>(c)) + 2);\r
+ }\r
+\r
+ vexSleep(100);\r
+ }\r
+}*/\r
projects / clyne / zephyr-os-alpha.git / commitdiff