Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 574aa3475cf92173fd2d20d77a3443649e1d8d4a
|
From: Xavier L. <Ba...@us...> - 2010-04-24 14:06:09
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 574aa3475cf92173fd2d20d77a3443649e1d8d4a (commit)
via 23d313403576d39df483e0c4637867520ca9b0a4 (commit)
from c30984ce77fc957c1886e457bf46d9f498e622dd (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 574aa3475cf92173fd2d20d77a3443649e1d8d4a
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Apr 24 15:56:23 2010 +0200
Added trajectory computation and integration into monitor.
The trajectory computation is based on a screw that lets the user define the speed field of the robot with respect to the table.
commit 23d313403576d39df483e0c4637867520ca9b0a4
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Apr 24 15:26:56 2010 +0200
Modified the Makefile to display code size at end of compilation.
To use it, use : "make size", "make flash" or "make"
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile b/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
index 62b5e6c..3e1e571 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
@@ -82,6 +82,7 @@ CSRC += encoder.c
CSRC += motor.c
CSRC += cpu_load.c
CSRC += speed_control.c
+CSRC += trajectory.c
CSRC += main.c
# C++ sources that can be compiled in ARM or THUMB mode depending on the global
@@ -133,11 +134,12 @@ 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)gcc -Wl,--gc-sections
#LD = $(TRGT)g++
CP = $(TRGT)objcopy
AS = $(TRGT)gcc -x assembler-with-cpp
OD = $(TRGT)objdump
+SIZE = $(TRGT)size
HEX = $(CP) -O ihex
BIN = $(CP) -O binary
@@ -185,7 +187,7 @@ DLIBS =
#
# List all user C define here, like -D_DEBUG=1
-#UDEFS = -DCH_DBG_ENABLE_STACK_CHECK=1 -DCH_DBG_FILL_THREADS=1
+UDEFS = -DCH_DBG_FILL_THREADS=1
UDEFS =
# Define ASM defines here
@@ -198,7 +200,7 @@ UINCDIR =
ULIBDIR =
# List all user libraries here
-ULIBS =
+ULIBS = -lm
#
# End of user defines
@@ -213,3 +215,11 @@ endif
include $(CHIBIOS)/os/ports/GCC/ARM/rules.mk
include jtag/flash.mk
+
+# Display program size at end of compilation
+.PHONY:size
+size: all
+ $(SIZE) $(BUILDDIR)/$(PROJECT).elf
+
+# Prefer compilation with display of program size
+.DEFAULT_GOAL := size
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/jtag/flash.mk b/elec/boards/MotherBoard_KrobotJr2010/Firmware/jtag/flash.mk
index c29c732..a2a35bb 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/jtag/flash.mk
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/jtag/flash.mk
@@ -1,6 +1,6 @@
# rule to flash the firmware
-flash: $(ENSUREBUILDDIR) $(OBJS) $(OUTFILES)
+flash: size
@cp $(BUILDDIR)/$(PROJECT).bin jtag/fw.bin
@cd jtag; openocd -f openocd.cfg -f flash.cfg
@rm jtag/fw.bin
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
index 46724fa..efb0f38 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
@@ -15,6 +15,7 @@
#include "encoder.h"
#include "cpu_load.h"
#include "speed_control.h"
+#include "trajectory.h"
/*
* Global variables
@@ -23,12 +24,12 @@
/*
* Red LEDs blinker thread, times are in milliseconds.
*/
-static WORKING_AREA(waThread1, 256);
+static WORKING_AREA(waThread1, 1024);
static msg_t Thread1(void *arg) {
(void)arg;
while (TRUE) {
- sc_setRefSpeed(MOTOR1, 0);
+ /*sc_setRefSpeed(MOTOR1, 0);
sc_setRefSpeed(MOTOR2, -360);
sc_setRefSpeed(MOTOR3, 360);
chThdSleepMilliseconds(2000);
@@ -70,13 +71,17 @@ static msg_t Thread1(void *arg) {
sc_setRefSpeed(MOTOR1, 0);
sc_setRefSpeed(MOTOR2, 0);
- sc_setRefSpeed(MOTOR3, 0);
+ sc_setRefSpeed(MOTOR3, 0);*/
palClearPad(IOPORT3, GPIOC_LED);
chThdSleepMilliseconds(1000);
palSetPad(IOPORT3, GPIOC_LED);
chThdSleepMilliseconds(1000);
+
+ /*setScrew(0, 200, 0, 0, 60);
+ chThdSleepMilliseconds(6000);
+ turn(0);*/
}
return 0;
}
@@ -88,13 +93,13 @@ static void TimerHandler(eventid_t id) {
(void)id;
if (palReadPad(IOPORT1, GPIOA_BUTTON)) {
- cputs("Coucou !\r");
+ /*cputs("Coucou !\r");
if (TIM_GetCounter(TIM3) == 0)
cputs("c'est nul !\r");
else
cputs("ca marche (peut etre...)\r");
- fflush(stdout);
- }
+ fflush(stdout);*/
+ }
}
/*
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.c
index e3159f5..a3221cc 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.c
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.c
@@ -119,6 +119,43 @@ void setSpeedHandler(BaseChannel *chp, int argc, char* argv[]) {
(speed)%10);
}
+void moveHandler(BaseChannel *chp, int argc, char* argv[]) {
+
+ if (argc != 1) {
+ shellPrintLine(chp, "Usage : move mode.");
+ return;
+ }
+ switch (argv[0][0]) {
+ case '1':
+ setScrew(0, 0, 100, 0, 0);
+ chThdSleepMilliseconds(2000);
+ setScrew(0, 0, 0, 100, 0);
+ chThdSleepMilliseconds(2000);
+ setScrew(0, 0, -100, 0, 0);
+ chThdSleepMilliseconds(2000);
+ setScrew(0, 0, 0, -100, 0);
+ chThdSleepMilliseconds(2000);
+ setScrew(0, 0, 0, 0, 0);
+ break;
+ case '2':
+ turn(360);
+ chThdSleepMilliseconds(1000);
+ turn(-360);
+ chThdSleepMilliseconds(1000);
+ turn(0);
+ break;
+ case '3':
+ setScrew(0, 200, 0, 0, 60);
+ chThdSleepMilliseconds(6000);
+ turn(0);
+ break;
+ default:
+ shellPrintLine(chp, "Mode inconnu");
+ return;
+ }
+}
+
+
void getSpeedHandler(BaseChannel *chp, int argc, char* argv[]) {
int32_t speed;
@@ -150,6 +187,7 @@ void getSpeedHandler(BaseChannel *chp, int argc, char* argv[]) {
}
+
/*
* Shell configuration variables
*/
@@ -158,6 +196,7 @@ static const ShellCommand commands[] = {
{"load", loadHandler},
{"get", getHandler},
{"setSpeed", setSpeedHandler},
+ {"move", moveHandler},
{"getSpeed", getSpeedHandler},
{NULL, NULL}
};
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
index 7651066..7d3b13d 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
@@ -22,6 +22,7 @@
#include "motor.h"
#include "cpu_load.h"
#include "speed_control.h"
+#include "trajectory.h"
extern Thread *cdtp;
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/trajectory.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/trajectory.c
new file mode 100644
index 0000000..f43aa58
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/trajectory.c
@@ -0,0 +1,33 @@
+/*
+ * Trajectory generation
+ * Xavier Lagorce
+ */
+
+#include "trajectory.h"
+
+void setScrew(int16_t ptX, int16_t ptY, int16_t vX, int16_t vY, int16_t omega) {
+
+ int32_t w1, w2, w3, om, tmp;
+
+ // Real formulas, unusable because of floating point arithmetic...
+ /*om = (((float)omega)/180.0*M_PI);
+
+ w1 = (int32_t)((-vX - (ptY-SRADIUS)*om)/WRADIUS*180.0/M_PI);
+ w2 = (int32_t)((vX/2.0+(ptY+SRADIUS/2.0)*om/2.0-(vY-(ptX+SQRT3_2*SRADIUS)*om)*SQRT3_2)/WRADIUS*180.0/M_PI);
+ w3 = (int32_t)((vX/2.0+(ptY+SRADIUS/2.0)*om/2.0+(vY-(ptX-SQRT3_2*SRADIUS)*om)*SQRT3_2)/WRADIUS*180.0/M_PI);*/
+
+ om = (((int32_t)omega)*314);
+
+ w1 = (int32_t)((-(int32_t)vX*18000 - ((int32_t)ptY-SRADIUS)*om)/(314*WRADIUS));
+ tmp = ((int32_t)vX*18000+((int32_t)ptY+SRADIUS/2)*om)/2;
+ w2 = (int32_t)((tmp-((int32_t)vY*18000-((int32_t)ptX*10+SQRT3_2*SRADIUS/10)*om/10)*SQRT3_2/100)/(WRADIUS*314));
+ w3 = (int32_t)((tmp+((int32_t)vY*18000-((int32_t)ptX*10-SQRT3_2*SRADIUS/10)*om/10)*SQRT3_2/100)/(WRADIUS*314));
+
+ sc_setRefSpeed(MOTOR1, w1);
+ sc_setRefSpeed(MOTOR2, w2);
+ sc_setRefSpeed(MOTOR3, w3);
+}
+
+void turn(int16_t omega) {
+ setScrew(0, 0, 0, 0, omega);
+}
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/trajectory.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/trajectory.h
new file mode 100644
index 0000000..aa13e4d
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/trajectory.h
@@ -0,0 +1,23 @@
+/*
+ * Trajectory generation
+ * Xavier Lagorce
+ */
+
+#ifndef HEADER__TRAJECTORY
+#define HEADER__TRAJECTORY
+
+#include "ch.h"
+#include <math.h>
+#include "speed_control.h"
+
+// Mecanical characteristics
+#define SRADIUS 150 // Structural radius (mm)
+#define WRADIUS 25 // Wheel radius (mm)
+
+#define SQRT3_2 87//SQRT3_2 0.866025 // sqrt(3)/2 ... * 100
+
+void setScrew(int16_t ptX, int16_t ptY, int16_t vX, int16_t vY, int16_t omega);
+
+void turn(int16_t omega);
+
+#endif
hooks/post-receive
--
krobot
|
