From daad5eaa0296ff30624da5fbfaacdb88792b5fea Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Sat, 5 Nov 2016 15:13:21 -0400 Subject: [PATCH] good - 11/5 --- Makefile | 23 ++--- include/control.h | 71 ++++++++++++++++ include/motor.h | 33 ++++++++ main.c => main.cpp | 0 setup.mk | 4 +- src/motor.cpp | 3 + vexuser.c | 134 ----------------------------- vexuser.cpp | 207 +++++++++++++++++++++++++++++++++++++++++++++ 8 files changed, 328 insertions(+), 147 deletions(-) create mode 100644 include/control.h create mode 100644 include/motor.h rename main.c => main.cpp (100%) 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.cpp similarity index 100% rename from main.c rename to main.cpp 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); + } +}*/ -- 2.39.5