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