good - 11/5

8 files changed, 328 insertions, 147 deletions

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 <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_
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
index 38b6a1b..38b6a1b 100644
--- a/main.c
+++ b/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 <motor.h>
+
+
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 <stdlib.h>

-#include <math.h>

-

-#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 <unistd.h>

+#include <array>

+

+#include "ch.h"  		// needs for all ChibiOS programs

+#include "hal.h" 		// hardware abstraction layer header

+#include "vex.h"		// vex library header

+

+#include <motor.h>

+#include <control.h>

+

+//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<tVexMotor, tVexImeChannels, int32_t>;

+

+static std::array<CountTuple, 4> 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);

+	}

+}*/