Commit from zer0 (2009-03-01 15:43 CET)
================
Add some commands (sensors, cs).
Set the gains + coefs approximatively
aversive_projects microb2009/mainboard/strat_base.c 1.2
aversive_projects microb2009/mainboard/sensor.h 1.3
aversive_projects microb2009/mainboard/sensor.c 1.3
aversive_projects microb2009/mainboard/main.h 1.3
aversive_projects microb2009/mainboard/main.c 1.3
aversive_projects microb2009/mainboard/cs.h 1.2
aversive_projects microb2009/mainboard/cs.c 1.2
aversive_projects microb2009/mainboard/commands_mainboard.c 1.3
aversive_projects microb2009/mainboard/commands_gen.c 1.3
aversive_projects microb2009/mainboard/commands_cs.c 1.3
aversive_projects microb2009/mainboard/commands.c 1.3
===================================================
aversive_projects/microb2009/mainboard/strat_base.c (1.1 -> 1.2)
===================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: strat_base.c,v 1.1 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: strat_base.c,v 1.2 2009-03-01 14:43:16 zer0 Exp $
*
*/
@@ -143,11 +143,11 @@
strat_preinit();
/* if start sw not plugged */
- if (sensor_start_switch()) {
+ if (sensor_get(S_START_SWITCH)) {
printf_P(PSTR("No start switch, press a key or plug it\r\n"));
/* while start sw not plugged */
- while (sensor_start_switch()) {
+ while (sensor_get(S_START_SWITCH)) {
if (cmdline_keypressed()) {
for (i=3; i>0; i--) {
printf_P(PSTR("%d\r\n"), i);
@@ -159,10 +159,10 @@
}
/* if start sw plugged */
- if (!sensor_start_switch()) {
+ if (!sensor_get(S_START_SWITCH)) {
printf_P(PSTR("Ready, unplug start switch to start\r\n"));
/* while start sw plugged */
- while (!sensor_start_switch());
+ while (!sensor_get(S_START_SWITCH));
}
strat_init();
===============================================
aversive_projects/microb2009/mainboard/sensor.h (1.2 -> 1.3)
===============================================
@@ -16,14 +16,43 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: sensor.h,v 1.2 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: sensor.h,v 1.3 2009-03-01 14:43:16 zer0 Exp $
*
*/
-#define SENSOR_ADC_COUNT 6
+/* synchronize with sensor.c */
+#define ADC_CSENSE1 0
+#define ADC_CSENSE2 1
+#define ADC_CSENSE3 2
+#define ADC_CSENSE4 3
+#define ADC_BATTERY1 4
+#define ADC_BATTERY2 5
+#define ADC_MAX 6
+
+/* synchronize with sensor.c */
+#define S_START_SWITCH 0
+#define S_CAP2 1
+#define S_CAP3 2
+#define S_CAP4 3
+#define S_CAP5 4
+#define S_CAP6 5
+#define S_CAP7 6
+#define S_CAP8 7
+#define S_RESERVED1 8
+#define S_RESERVED2 9
+#define S_RESERVED3 10
+#define S_RESERVED4 11
+#define S_RESERVED5 12
+#define S_RESERVED6 13
+#define S_RESERVED7 14
+#define S_RESERVED8 15
+#define SENSOR_MAX 16
void sensor_init(void);
-int16_t sensor_read_adc(uint8_t i);
-/* XXX */
-#define sensor_start_switch() 0
+/* get filtered values for adc */
+int16_t sensor_get_adc(uint8_t i);
+
+/* get filtered values of boolean sensors */
+uint16_t sensor_get_all(void);
+uint8_t sensor_get(uint8_t i);
===============================================
aversive_projects/microb2009/mainboard/sensor.c (1.2 -> 1.3)
===============================================
@@ -16,7 +16,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: sensor.c,v 1.2 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: sensor.c,v 1.3 2009-03-01 14:43:16 zer0 Exp $
*
*/
@@ -45,6 +45,8 @@
#include "main.h"
#include "sensor.h"
+/************ ADC */
+
struct adc_infos {
uint16_t config;
int16_t value;
@@ -76,41 +78,31 @@
#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
-/* define which ADC to poll, update SENSOR_ADC_COUNT in sensor.h */
-static struct adc_infos adc_infos[SENSOR_ADC_COUNT] = {
-#define ADC_CSENSE1 0
- { .config = ADC_CONF(0), .filter = rii_medium },
-#define ADC_CSENSE2 1
- { .config = ADC_CONF(1), .filter = rii_medium },
-#define ADC_CSENSE3 2
- { .config = ADC_CONF(2), .filter = rii_medium },
-#define ADC_CSENSE4 3
- { .config = ADC_CONF(3), .filter = rii_medium },
-#define ADC_BATTERY 4
- { .config = ADC_CONF(8), .filter = rii_strong },
-#define ADC_CAP6 5
- { .config = ADC_CONF(12) },
+/* define which ADC to poll, see in sensor.h */
+static struct adc_infos adc_infos[ADC_MAX] = {
+ [ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
+ [ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
+ [ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
+ [ADC_CSENSE4] = { .config = ADC_CONF(3), .filter = rii_medium },
+ [ADC_BATTERY1] = { .config = ADC_CONF(8), .filter = rii_strong },
+ [ADC_BATTERY2] = { .config = ADC_CONF(9), .filter = rii_strong },
+
+ /* add adc on "cap" pins if needed */
+/* [ADC_CAP1] = { .config = ADC_CONF(10) }, */
+/* [ADC_CAP2] = { .config = ADC_CONF(11) }, */
+/* [ADC_CAP3] = { .config = ADC_CONF(12) }, */
+/* [ADC_CAP4] = { .config = ADC_CONF(13) }, */
};
static void adc_event(int16_t result);
-/* called every 10 ms */
+/* called every 10 ms, see init below */
static void do_adc(void *dummy)
{
/* launch first conversion */
adc_launch(adc_infos[0].config);
}
-void sensor_init(void)
-{
- adc_init();
- adc_register_event(adc_event);
- /* CS EVENT */
- scheduler_add_periodical_event_priority(do_adc, NULL,
- 10000L / SCHEDULER_UNIT,
- ADC_PRIO);
-}
-
static void adc_event(int16_t result)
{
static uint8_t i = 0;
@@ -123,12 +115,13 @@
adc_infos[i].value = result;
i ++;
- i %= SENSOR_ADC_COUNT;
- if (i != 0)
+ if (i >= ADC_MAX)
+ i = 0;
+ else
adc_launch(adc_infos[i].config);
}
-int16_t sensor_read_adc(uint8_t i)
+int16_t sensor_get_adc(uint8_t i)
{
int16_t tmp;
uint8_t flags;
@@ -138,3 +131,123 @@
IRQ_UNLOCK(flags);
return tmp;
}
+
+/************ boolean sensors */
+
+
+struct sensor_filter {
+ uint8_t filter;
+ uint8_t prev;
+ uint8_t thres_off;
+ uint8_t thres_on;
+ uint8_t cpt;
+};
+
+static struct sensor_filter sensor_filter[SENSOR_MAX] = {
+ [S_START_SWITCH] = { 1, 0, 0, 1, 0 }, /* 0 */
+ [S_CAP2] = { 1, 0, 0, 1, 0 }, /* 1 */
+ [S_CAP3] = { 1, 0, 0, 1, 0 }, /* 2 */
+ [S_CAP4] = { 1, 0, 0, 1, 0 }, /* 3 */
+ [S_CAP5] = { 1, 0, 0, 1, 0 }, /* 4 */
+ [S_CAP6] = { 1, 0, 0, 1, 0 }, /* 5 */
+ [S_CAP7] = { 1, 0, 0, 1, 0 }, /* 6 */
+ [S_CAP8] = { 1, 0, 0, 1, 0 }, /* 7 */
+ [S_RESERVED1] = { 10, 0, 3, 7, 0 }, /* 8 */
+ [S_RESERVED2] = { 10, 0, 3, 7, 0 }, /* 9 */
+ [S_RESERVED3] = { 1, 0, 0, 1, 0 }, /* 10 */
+ [S_RESERVED4] = { 1, 0, 0, 1, 0 }, /* 11 */
+ [S_RESERVED5] = { 1, 0, 0, 1, 0 }, /* 12 */
+ [S_RESERVED6] = { 1, 0, 0, 1, 0 }, /* 13 */
+ [S_RESERVED7] = { 1, 0, 0, 1, 0 }, /* 14 */
+ [S_RESERVED8] = { 1, 0, 0, 1, 0 }, /* 15 */
+};
+
+/* value of filtered sensors */
+static uint16_t sensor_filtered = 0;
+
+/* sensor mapping :
+ * 0-3: PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13)
+ * 4-5: PORTL 0->1 (cap5 -> cap6)
+ * 6-7: PORTE 3->4 (cap7 -> cap8)
+ * 8-15: reserved
+ */
+
+uint16_t sensor_get_all(void)
+{
+ uint16_t tmp;
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ tmp = sensor_filtered;
+ IRQ_UNLOCK(flags);
+ return tmp;
+}
+
+uint8_t sensor_get(uint8_t i)
+{
+ uint16_t tmp = sensor_get_all();
+ return (tmp & _BV(i));
+}
+
+/* get the physical value of pins */
+static uint16_t sensor_read(void)
+{
+ uint16_t tmp = 0;
+ tmp |= (uint16_t)((PINK & (_BV(2)|_BV(3)|_BV(4)|_BV(5))) >> 2) << 0;
+ tmp |= (uint16_t)((PINL & (_BV(0)|_BV(1))) >> 0) << 4;
+ tmp |= (uint16_t)((PINE & (_BV(3)|_BV(4))) >> 3) << 6;
+ /* add reserved sensors here */
+ return tmp;
+}
+
+/* called every 10 ms, see init below */
+static void do_boolean_sensors(void *dummy)
+{
+ uint8_t i;
+ uint8_t flags;
+ uint16_t sensor = sensor_read();
+ uint16_t tmp = 0;
+
+ for (i=0; i<SENSOR_MAX; i++) {
+ if ((1 << i) & sensor) {
+ if (sensor_filter[i].cpt < sensor_filter[i].filter)
+ sensor_filter[i].cpt++;
+ if (sensor_filter[i].cpt >= sensor_filter[i].thres_on)
+ sensor_filter[i].prev = 1;
+ }
+ else {
+ if (sensor_filter[i].cpt > 0)
+ sensor_filter[i].cpt--;
+ if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
+ sensor_filter[i].prev = 0;
+ }
+
+ if (sensor_filter[i].prev) {
+ tmp |= (1UL << i);
+ }
+ }
+ IRQ_LOCK(flags);
+ sensor_filtered = tmp;
+ IRQ_UNLOCK(flags);
+}
+
+
+
+/************ global sensor init */
+
+/* called every 10 ms, see init below */
+static void do_sensors(void *dummy)
+{
+ do_adc(NULL);
+ do_boolean_sensors(NULL);
+}
+
+void sensor_init(void)
+{
+ adc_init();
+ adc_register_event(adc_event);
+ /* CS EVENT */
+ scheduler_add_periodical_event_priority(do_sensors, NULL,
+ 10000L / SCHEDULER_UNIT,
+ ADC_PRIO);
+}
+
=============================================
aversive_projects/microb2009/mainboard/main.h (1.2 -> 1.3)
=============================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: main.h,v 1.2 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: main.h,v 1.3 2009-03-01 14:43:16 zer0 Exp $
*
*/
@@ -29,11 +29,11 @@
#define BRAKE_ON() do { PORTJ |= 0xF0; } while(0)
#define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0)
-/* it is 90 don't forget it :) */
+/* only 90 seconds, don't forget it :) */
#define MATCH_TIME 90
/* decrease track to decrease angle */
-#define EXT_TRACK_CM 30.0
+#define EXT_TRACK_CM 30.125
#define VIRTUAL_TRACK_CM EXT_TRACK_CM
/* it is a 2048 imps -> 8192 because we see 1/4 period
@@ -46,18 +46,12 @@
#define IMP_COEF 10.
#define DIST_IMP_CM (((IMP_ENCODERS*4)/WHEEL_PERIM_CM)*IMP_COEF)
-/* XXX */
#define LEFT_ENCODER ((void *)1)
#define RIGHT_ENCODER ((void *)2)
-/* XXX */
#define LEFT_PWM ((void *)&gen.pwm3_1B)
#define RIGHT_PWM ((void *)&gen.pwm4_1C)
-/* XXX */
-#define MAINBOARD_MOT_PWM (&gen.pwm4_1C)
-#define MAINBOARD_ENC ((void *)3)
-
/** ERROR NUMS */
#define E_USER_STRAT 194
#define E_USER_I2C_PROTO 195
@@ -120,8 +114,6 @@
/* control systems */
struct cs_block angle;
struct cs_block distance;
- struct cs_block example1;
- struct cs_block example2;
/* x,y positionning */
struct robot_system rs;
=============================================
aversive_projects/microb2009/mainboard/main.c (1.2 -> 1.3)
=============================================
@@ -16,7 +16,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: main.c,v 1.2 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: main.c,v 1.3 2009-03-01 14:43:16 zer0 Exp $
*
*/
@@ -65,14 +65,6 @@
* BOOTRST : 0 (reset on bootloader)
*/
-/*
- * TODO:
- * circuit VN: il faut le tester et comprendre comment ca marche
- * max232: y a t il eu des modifs ?
- * diode zener sur le Uv : 5.6K et 1K
- * condos sur les LMD
- */
-
struct genboard gen;
struct mainboard mainboard;
@@ -80,12 +72,16 @@
void bootloader(void)
{
+ /* XXX update bootloader to enable the brake */
+ /* XXX don't wait at boot */
+ /* XXX exit when waiting for addr */
#define BOOTLOADER_ADDR 0x3f000
if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
printf_P(PSTR("Bootloader is not present\r\n"));
return;
}
cli();
+ BRAKE_ON();
/* ... very specific :( */
TIMSK0 = 0;
TIMSK1 = 0;
@@ -148,7 +144,7 @@
memset(&gen, 0, sizeof(gen));
memset(&mainboard, 0, sizeof(mainboard));
mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
- DO_POS | DO_POWER; // DO_BD
+ DO_POS; // | DO_POWER; // DO_BD
/* UART */
uart_init();
@@ -227,6 +223,7 @@
sei();
+ printf_P(PSTR("\r\n"));
printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
cmdline_interact();
===========================================
aversive_projects/microb2009/mainboard/cs.h (1.1 -> 1.2)
===========================================
@@ -16,8 +16,10 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: cs.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: cs.h,v 1.2 2009-03-01 14:43:16 zer0 Exp $
*
*/
void microb_cs_init(void);
+void dump_cs(const char *name, struct cs *cs);
+void dump_pid(const char *name, struct pid_filter *pid);
===========================================
aversive_projects/microb2009/mainboard/cs.c (1.1 -> 1.2)
===========================================
@@ -16,7 +16,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: cs.c,v 1.1 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: cs.c,v 1.2 2009-03-01 14:43:16 zer0 Exp $
*
*/
@@ -78,10 +78,6 @@
cs_manage(&mainboard.angle.cs);
if (mainboard.distance.on)
cs_manage(&mainboard.distance.cs);
- if (mainboard.example1.on)
- cs_manage(&mainboard.example1.cs);
- if (mainboard.example2.on)
- cs_manage(&mainboard.example2.cs);
}
if ((cpt & 1) && (mainboard.flags & DO_POS)) {
position_manage(&mainboard.pos); /* about 1.5ms (worst case) */
@@ -118,6 +114,25 @@
cpt++;
}
+void dump_cs(const char *name, struct cs *cs)
+{
+ printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld"
+ "in=% .5ld out=% .5ld\r\n"),
+ name, cs_get_consign(cs), cs_get_filtered_consign(cs),
+ cs_get_error(cs), cs_get_filtered_feedback(cs),
+ cs_get_out(cs));
+}
+
+void dump_pid(const char *name, struct pid_filter *pid)
+{
+ printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ name,
+ pid_get_value_in(pid) * pid_get_gain_P(pid),
+ pid_get_value_I(pid) * pid_get_gain_I(pid),
+ pid_get_value_D(pid) * pid_get_gain_D(pid),
+ pid_get_value_out(pid));
+}
+
void microb_cs_init(void)
{
/* ROBOT_SYSTEM */
@@ -126,9 +141,9 @@
rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM);
/* increase gain to decrease dist, increase left and it will turn more
left */
rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
- LEFT_ENCODER, IMP_COEF);
+ LEFT_ENCODER, IMP_COEF * 1.0435);
rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
- RIGHT_ENCODER, IMP_COEF);
+ RIGHT_ENCODER, IMP_COEF * 1.0025);
/* rs will use external encoders */
rs_set_flags(&mainboard.rs, RS_USE_EXT);
@@ -143,16 +158,16 @@
trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs,
&mainboard.angle.cs);
trajectory_set_robot_params(&mainboard.traj, &mainboard.rs,
&mainboard.pos);
- trajectory_set_speed(&mainboard.traj, 3000, 3000); /* d, a */
+ trajectory_set_speed(&mainboard.traj, 400, 400); /* d, a */
/* distance window, angle window, angle start */
trajectory_set_windows(&mainboard.traj, 35.0, 5.0, 30.0);
/* ---- CS angle */
/* PID */
pid_init(&mainboard.angle.pid);
- pid_set_gains(&mainboard.angle.pid, 20, 1, 800);
- pid_set_maximums(&mainboard.angle.pid, 0, 30000, 0);
- pid_set_out_shift(&mainboard.angle.pid, 6);
+ pid_set_gains(&mainboard.angle.pid, 500, 10, 7000);
+ pid_set_maximums(&mainboard.angle.pid, 0, 20000, 0);
+ pid_set_out_shift(&mainboard.angle.pid, 10);
pid_set_derivate_filter(&mainboard.angle.pid, 4);
/* QUADRAMP */
@@ -171,9 +186,9 @@
/* ---- CS distance */
/* PID */
pid_init(&mainboard.distance.pid);
- pid_set_gains(&mainboard.distance.pid, 20, 1, 300);
- pid_set_maximums(&mainboard.distance.pid, 0, 30000, 0);
- pid_set_out_shift(&mainboard.distance.pid, 6);
+ pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
+ pid_set_maximums(&mainboard.distance.pid, 0, 2000, 0);
+ pid_set_out_shift(&mainboard.distance.pid, 10);
pid_set_derivate_filter(&mainboard.distance.pid, 6);
/* QUADRAMP */
@@ -189,26 +204,9 @@
cs_set_process_out(&mainboard.distance.cs, rs_get_distance,
&mainboard.rs);
cs_set_consign(&mainboard.distance.cs, 0);
- /* ---- other CS */
- /* PID */
- pid_init(&mainboard.example1.pid);
- pid_set_gains(&mainboard.example1.pid, 20, 0, 0);
- pid_set_maximums(&mainboard.example1.pid, 0, 30000, 4095);
- pid_set_out_shift(&mainboard.example1.pid, 6);
- pid_set_derivate_filter(&mainboard.example1.pid, 6);
+ mainboard.angle.on = 1;
+ mainboard.distance.on = 1;
- /* QUADRAMP */
- quadramp_init(&mainboard.example1.qr);
- quadramp_set_1st_order_vars(&mainboard.example1.qr, 400, 400); /* set
speed */
- quadramp_set_2nd_order_vars(&mainboard.example1.qr, 5, 5); /* set accel
*/
-
- /* CS */
- cs_init(&mainboard.example1.cs);
- cs_set_consign_filter(&mainboard.example1.cs, quadramp_do_filter,
&mainboard.example1.qr);
- cs_set_correct_filter(&mainboard.example1.cs, pid_do_filter,
&mainboard.example1.pid);
- cs_set_process_in(&mainboard.example1.cs, pwm_ng_set,
MAINBOARD_MOT_PWM);
- cs_set_process_out(&mainboard.example1.cs, encoders_spi_get_value,
MAINBOARD_ENC);
- cs_set_consign(&mainboard.example1.cs, 0);
scheduler_add_periodical_event_priority(do_cs, NULL,
5000L / SCHEDULER_UNIT,
===========================================================
aversive_projects/microb2009/mainboard/commands_mainboard.c (1.2 -> 1.3)
===========================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: commands_mainboard.c,v 1.2 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: commands_mainboard.c,v 1.3 2009-03-01 14:43:16 zer0 Exp $
*
* Olivier MATZ <z...@droids-corp.org>
*/
@@ -69,6 +69,32 @@
struct cmd_event_result * res = parsed_result;
+ if (!strcmp_P(res->arg1, PSTR("all"))) {
+ bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
+ DO_BD | DO_TIMER | DO_POWER;
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ mainboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ mainboard.flags &= bit;
+ else { /* show */
+ printf_P(PSTR("encoders is %s\r\n"),
+ (DO_ENCODERS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("cs is %s\r\n"),
+ (DO_CS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("rs is %s\r\n"),
+ (DO_RS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("pos is %s\r\n"),
+ (DO_POS & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("bd is %s\r\n"),
+ (DO_BD & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("timer is %s\r\n"),
+ (DO_TIMER & mainboard.flags) ? "on":"off");
+ printf_P(PSTR("power is %s\r\n"),
+ (DO_POWER & mainboard.flags) ? "on":"off");
+ }
+ return;
+ }
+
if (!strcmp_P(res->arg1, PSTR("encoders")))
bit = DO_ENCODERS;
else if (!strcmp_P(res->arg1, PSTR("cs"))) {
@@ -103,7 +129,7 @@
prog_char str_event_arg0[] = "event";
parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_event_result, arg0, str_event_arg0);
-prog_char str_event_arg1[] = "encoders#cs#rs#pos#bd#timer#power";
+prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_event_result, arg1, str_event_arg1);
prog_char str_event_arg2[] = "on#off#show";
parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct
cmd_event_result, arg2, str_event_arg2);
@@ -793,104 +819,52 @@
},
};
+
/**********************************************************/
-/* Sensor */
+/* Rs tests */
-/* this structure is filled when cmd_sensor is parsed successfully */
-struct cmd_sensor_result {
+/* this structure is filled when cmd_rs is parsed successfully */
+struct cmd_rs_result {
fixed_string_t arg0;
fixed_string_t arg1;
};
-/* function called when cmd_sensor is parsed successfully */
-static void cmd_sensor_parsed(void *parsed_result, void *data)
+/* function called when cmd_rs is parsed successfully */
+static void cmd_rs_parsed(void *parsed_result, void *data)
{
-#ifdef notyet
- struct cmd_sensor_result *res = parsed_result;
- uint8_t i;
-
- if (!strcmp_P(res->arg1, PSTR("show"))) {
- printf_P(PSTR("Sensors state:\r\n"
- " %8.8lx\r\n"
- " IR disp=%d\r\n"), mainboard.sensor,
IR_DISP_SENSOR());
- }
- else if (!strcmp_P(res->arg1, PSTR("all_loop"))) { /* show_loop */
- while(uart0_recv_nowait() == -1) {
- for (i=0 ; i<32; i++) {
- if (mainboard.sensor & (1UL<<i))
- printf_P(PSTR("|%.2d"), i);
- else
- printf_P(PSTR(" "));
- }
- if (IR_DISP_SENSOR()) {
- printf_P(PSTR("IR|"));
- }
- else {
- printf_P(PSTR(" |"));
- }
- printf_P(PSTR("\r\n"));
- wait_ms(100);
- }
- }
- else if (!strcmp_P(res->arg1, PSTR("ir_loop"))) { /* show_loop */
- while(uart0_recv_nowait() == -1) {
- if (IR1())
- printf_P(PSTR("LEFT|"));
- else
- printf_P(PSTR(" |"));
- if (IR2())
- printf_P(PSTR("MID-L|"));
- else
- printf_P(PSTR(" |"));
- if (IR3())
- printf_P(PSTR("MID|"));
- else
- printf_P(PSTR(" |"));
- if (IR4())
- printf_P(PSTR("MID-R|"));
- else
- printf_P(PSTR(" |"));
- if (IR5())
- printf_P(PSTR("RIGHT| |"));
- else
- printf_P(PSTR(" | |"));
-
- if (sensor_obstacle())
- printf_P(PSTR("OBSTACLE|"));
- else
- printf_P(PSTR(" |"));
-
- if (strat_obstacle())
- printf_P(PSTR("OBSTACLE IN AREA (%d,%d)|"),
- mainboard.opponent_x,
mainboard.opponent_y);
- else
- printf_P(PSTR(" |"));
- printf_P(PSTR("\r\n"));
- wait_ms(100);
- }
- }
-#endif
+ // struct cmd_rs_result *res = parsed_result;
+ do {
+ printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
+ cs_get_consign(&mainboard.angle.cs),
+ cs_get_filtered_feedback(&mainboard.angle.cs),
+ cs_get_out(&mainboard.angle.cs));
+ printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
+ cs_get_consign(&mainboard.distance.cs),
+ cs_get_filtered_feedback(&mainboard.distance.cs),
+ cs_get_out(&mainboard.distance.cs));
+ printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
+ mainboard.pwm_r);
+ wait_ms(100);
+ } while(!cmdline_keypressed());
}
-prog_char str_sensor_arg0[] = "sensor";
-parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_sensor_result, arg0, str_sensor_arg0);
-prog_char str_sensor_arg1[] = "show#all_loop#ir_loop";
-parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_sensor_result, arg1, str_sensor_arg1);
-
-prog_char help_sensor[] = "Show sensors status";
-parse_pgm_inst_t cmd_sensor = {
- .f = cmd_sensor_parsed, /* function to call */
+prog_char str_rs_arg0[] = "rs";
+parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_rs_result, arg0, str_rs_arg0);
+prog_char str_rs_arg1[] = "show";
+parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_rs_result, arg1, str_rs_arg1);
+
+prog_char help_rs[] = "Show rs (robot system) values";
+parse_pgm_inst_t cmd_rs = {
+ .f = cmd_rs_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
- .help_str = help_sensor,
+ .help_str = help_rs,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_sensor_arg0,
- (prog_void *)&cmd_sensor_arg1,
+ (prog_void *)&cmd_rs_arg0,
+ (prog_void *)&cmd_rs_arg1,
NULL,
},
};
-
-
/**********************************************************/
/* Test */
=====================================================
aversive_projects/microb2009/mainboard/commands_gen.c (1.2 -> 1.3)
=====================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: commands_gen.c,v 1.2 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: commands_gen.c,v 1.3 2009-03-01 14:43:16 zer0 Exp $
*
* Olivier MATZ <z...@droids-corp.org>
*/
@@ -220,43 +220,6 @@
};
/**********************************************************/
-/* Brakes tests */
-
-/* this structure is filled when cmd_brake is parsed successfully */
-struct cmd_brake_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_brake is parsed successfully */
-static void cmd_brake_parsed(void *parsed_result, void *data)
-{
- struct cmd_brake_result *res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("on")))
- BRAKE_ON();
- else if (!strcmp_P(res->arg1, PSTR("off")))
- BRAKE_OFF();
-}
-
-prog_char str_brake_arg0[] = "brake";
-parse_pgm_token_string_t cmd_brake_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_brake_result, arg0, str_brake_arg0);
-prog_char str_brake_arg1[] = "on#off";
-parse_pgm_token_string_t cmd_brake_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_brake_result, arg1, str_brake_arg1);
-
-prog_char help_brake[] = "Enable/disable brake";
-parse_pgm_inst_t cmd_brake = {
- .f = cmd_brake_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_brake,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_brake_arg0,
- (prog_void *)&cmd_brake_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
/* Adcs tests */
/* this structure is filled when cmd_adc is parsed successfully */
@@ -276,8 +239,8 @@
do {
printf_P(PSTR("ADC values: "));
- for (i=0; i<SENSOR_ADC_COUNT; i++) {
- printf_P(PSTR("%.4d "), sensor_read_adc(i));
+ for (i=0; i<ADC_MAX; i++) {
+ printf_P(PSTR("%.4d "), sensor_get_adc(i));
}
printf_P(PSTR("\r\n"));
wait_ms(100);
@@ -303,6 +266,52 @@
/**********************************************************/
+/* Sensors tests */
+
+/* this structure is filled when cmd_sensor is parsed successfully */
+struct cmd_sensor_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_sensor is parsed successfully */
+static void cmd_sensor_parsed(void *parsed_result, void *data)
+{
+ struct cmd_sensor_result *res = parsed_result;
+ uint8_t i, loop = 0;
+
+ if (!strcmp_P(res->arg1, PSTR("loop_show")))
+ loop = 1;
+
+ do {
+ printf_P(PSTR("SENSOR values: "));
+ for (i=0; i<SENSOR_MAX; i++) {
+ printf_P(PSTR("%d "), sensor_get(i));
+ }
+ printf_P(PSTR("\r\n"));
+ wait_ms(100);
+ } while (loop && !cmdline_keypressed());
+}
+
+prog_char str_sensor_arg0[] = "sensor";
+parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_sensor_result, arg0, str_sensor_arg0);
+prog_char str_sensor_arg1[] = "show#loop_show";
+parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_sensor_result, arg1, str_sensor_arg1);
+
+prog_char help_sensor[] = "Show sensor values";
+parse_pgm_inst_t cmd_sensor = {
+ .f = cmd_sensor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sensor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_sensor_arg0,
+ (prog_void *)&cmd_sensor_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
/* Log */
/* this structure is filled when cmd_log is parsed successfully */
====================================================
aversive_projects/microb2009/mainboard/commands_cs.c (1.2 -> 1.3)
====================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: commands_cs.c,v 1.2 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: commands_cs.c,v 1.3 2009-03-01 14:43:16 zer0 Exp $
*
* Olivier MATZ <z...@droids-corp.org>
*/
@@ -47,6 +47,7 @@
#include <parse_num.h>
#include "main.h"
+#include "cs.h"
#include "cmdline.h"
struct csb_list {
@@ -56,13 +57,9 @@
prog_char csb_angle_str[] = "angle";
prog_char csb_distance_str[] = "distance";
-prog_char csb_example1_str[] = "example1";
-prog_char csb_example2_str[] = "example2";
struct csb_list csb_list[] = {
{ .name = csb_angle_str, .csb = &mainboard.angle },
{ .name = csb_distance_str, .csb = &mainboard.distance },
- { .name = csb_example1_str, .csb = &mainboard.example1 },
- { .name = csb_example2_str, .csb = &mainboard.example2 },
};
struct cmd_cs_result {
@@ -71,7 +68,7 @@
};
/* token to be used for all cs-related commands */
-prog_char str_csb_name[] = "angle#distance#example1#example2";
+prog_char str_csb_name[] = "angle#distance";
parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct
cmd_cs_result, csname, str_csb_name);
struct cs_block *cs_from_name(const char *name)
@@ -111,7 +108,7 @@
if (!show)
pid_set_gains(&csb->pid, res->p, res->i, res->d);
- printf_P(PSTR("%s %s %d %d %d\r\n"),
+ printf_P(PSTR("%s %s %d %d %d\r\n"),
res->cs.cmdname,
res->cs.csname,
pid_get_gain_P(&csb->pid),
@@ -529,13 +526,13 @@
struct cmd_cs_status_result *res = parsed_result;
struct cs_block *csb;
uint8_t loop = 0;
+ uint8_t print_pid = 0, print_cs = 0;
csb = cs_from_name(res->cs.csname);
if (csb == NULL) {
printf_P(PSTR("null csb\r\n"));
return;
}
-
if (strcmp_P(res->arg, PSTR("on")) == 0) {
csb->on = 1;
printf_P(PSTR("%s is on\r\n"), res->cs.csname);
@@ -546,25 +543,34 @@
printf_P(PSTR("%s is off\r\n"), res->cs.csname);
return;
}
- else if (strcmp_P(res->arg, PSTR("loop_show")) == 0)
+ else if (strcmp_P(res->arg, PSTR("show")) == 0) {
+ print_cs = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
+ loop = 1;
+ print_cs = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
+ print_pid = 1;
+ }
+ else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
+ print_pid = 1;
loop = 1;
+ }
printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
do {
- printf_P(PSTR("%s cons=%ld fil_cons=%ld err=%ld feed=%ld
out=%ld\r\n"),
- res->cs.csname,
- cs_get_consign(&csb->cs),
- cs_get_filtered_consign(&csb->cs),
- cs_get_error(&csb->cs),
- cs_get_filtered_feedback(&csb->cs),
- cs_get_out(&csb->cs));
+ if (print_cs)
+ dump_cs(res->cs.csname, &csb->cs);
+ if (print_pid)
+ dump_pid(res->cs.csname, &csb->pid);
wait_ms(100);
} while(loop && !cmdline_keypressed());
}
prog_char str_cs_status_arg0[] = "cs_status";
parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
-prog_char str_cs_status_arg[] = "show#loop_show#on#off";
+prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct
cmd_cs_status_result, arg, str_cs_status_arg);
prog_char help_cs_status[] = "Show cs status";
=================================================
aversive_projects/microb2009/mainboard/commands.c (1.2 -> 1.3)
=================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: commands.c,v 1.2 2009-02-27 22:23:37 zer0 Exp $
+ * Revision : $Id: commands.c,v 1.3 2009-03-01 14:43:16 zer0 Exp $
*
* Olivier MATZ <z...@droids-corp.org>
*/
@@ -29,8 +29,8 @@
extern parse_pgm_inst_t cmd_bootloader;
extern parse_pgm_inst_t cmd_encoders;
extern parse_pgm_inst_t cmd_pwm;
-extern parse_pgm_inst_t cmd_brake;
extern parse_pgm_inst_t cmd_adc;
+extern parse_pgm_inst_t cmd_sensor;
extern parse_pgm_inst_t cmd_log;
extern parse_pgm_inst_t cmd_log_show;
extern parse_pgm_inst_t cmd_log_type;
@@ -67,7 +67,7 @@
extern parse_pgm_inst_t cmd_start;
extern parse_pgm_inst_t cmd_interact;
extern parse_pgm_inst_t cmd_color;
-extern parse_pgm_inst_t cmd_sensor;
+extern parse_pgm_inst_t cmd_rs;
extern parse_pgm_inst_t cmd_test;
/* commands_traj.c */
@@ -95,8 +95,8 @@
(parse_pgm_inst_t *)&cmd_bootloader,
(parse_pgm_inst_t *)&cmd_encoders,
(parse_pgm_inst_t *)&cmd_pwm,
- (parse_pgm_inst_t *)&cmd_brake,
(parse_pgm_inst_t *)&cmd_adc,
+ (parse_pgm_inst_t *)&cmd_sensor,
(parse_pgm_inst_t *)&cmd_log,
(parse_pgm_inst_t *)&cmd_log_show,
(parse_pgm_inst_t *)&cmd_log_type,
@@ -132,7 +132,7 @@
(parse_pgm_inst_t *)&cmd_start,
(parse_pgm_inst_t *)&cmd_interact,
(parse_pgm_inst_t *)&cmd_color,
- (parse_pgm_inst_t *)&cmd_sensor,
+ (parse_pgm_inst_t *)&cmd_rs,
(parse_pgm_inst_t *)&cmd_test,
/* commands_traj.c */
Commit from zer0 on branch b_zer0 (2009-03-01 15:44 CET)
=================================
Fix bug introduced by previous commit...
aversive modules/devices/control_system/filters/pid/pid.c 1.5.4.9
=========================================================
aversive/modules/devices/control_system/filters/pid/pid.c (1.5.4.8 -> 1.5.4.9)
=========================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: pid.c,v 1.5.4.8 2009-02-27 21:32:59 zer0 Exp $
+ * Revision : $Id: pid.c,v 1.5.4.9 2009-03-01 14:44:08 zer0 Exp $
*
*/
@@ -197,7 +197,7 @@
*/
prev_index = p->index + 1;
- if (prev_index > p->derivate_nb_samples)
+ if (prev_index >= p->derivate_nb_samples)
prev_index = 0;
/* saturate input... it influences integral an derivate */
Commit from zer0 (2009-03-01 15:44 CET)
================
s/extension/generic/
aversive_projects microb2009/common/i2c_commands.h 1.3
==================================================
aversive_projects/microb2009/common/i2c_commands.h (1.2 -> 1.3)
==================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: i2c_commands.h,v 1.2 2009-02-20 21:10:01 zer0 Exp $
+ * Revision : $Id: i2c_commands.h,v 1.3 2009-03-01 14:44:36 zer0 Exp $
*
*/
@@ -49,7 +49,7 @@
#define I2C_GENERIC_COLOR_RED 0
#define I2C_GENERIC_COLOR_GREEN 1
-struct i2c_cmd_mechboard_color {
+struct i2c_cmd_generic_color {
struct i2c_cmd_hdr hdr;
uint8_t color;
};
_______________________________________________
Avr-list mailing list
Avr-list@droids-corp.org
CVSWEB : http://cvsweb.droids-corp.org/cgi-bin/viewcvs.cgi/aversive
WIKI : http://wiki.droids-corp.org/index.php/Aversive
DOXYGEN : http://zer0.droids-corp.org/doxygen_aversive/html/
BUGZILLA : http://bugzilla.droids-corp.org
COMMIT LOGS : http://zer0.droids-corp.org/aversive_commitlog
