From 0c4732c1e255ea56aa766b772dfdb6be49620830 Mon Sep 17 00:00:00 2001 From: zer0 Date: Mon, 5 Apr 2010 14:07:17 +0200 Subject: [PATCH] cobboard --- projects/microb2010/cobboard/actuator.c | 16 ++++++-- projects/microb2010/cobboard/cs.c | 51 +++++++++++++++++++++---- projects/microb2010/cobboard/main.c | 28 +++++++------- projects/microb2010/cobboard/main.h | 9 +++-- projects/microb2010/cobboard/sensor.c | 4 +- projects/microb2010/cobboard/sensor.h | 4 +- projects/microb2010/cobboard/spickle.c | 6 +-- projects/microb2010/cobboard/state.c | 25 +++++++++++- 8 files changed, 107 insertions(+), 36 deletions(-) diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index cebcd2a..79f91cd 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -48,19 +48,27 @@ #define COBROLLER_SPEED 800 //#define COBROLLER_SPEED 400 -#define SERVO_DOOR_OPEN 250 -#define SERVO_DOOR_CLOSED 470 +#define SERVO_DOOR_OPEN 260 +#define SERVO_DOOR_CLOSED 490 + +#define SERVO_CARRY_L_OPEN 280 +#define SERVO_CARRY_L_CLOSED 510 + +#define SERVO_CARRY_R_OPEN 470 +#define SERVO_CARRY_R_CLOSED 250 void actuator_init(void); void servo_carry_open(void) { - /* TODO */ + pwm_ng_set(SERVO_CARRY_L_PWM, SERVO_CARRY_L_OPEN); + pwm_ng_set(SERVO_CARRY_R_PWM, SERVO_CARRY_R_OPEN); } void servo_carry_close(void) { - /* TODO */ + pwm_ng_set(SERVO_CARRY_L_PWM, SERVO_CARRY_L_CLOSED); + pwm_ng_set(SERVO_CARRY_R_PWM, SERVO_CARRY_R_CLOSED); } void servo_door_open(void) diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c index 17d8e54..f9c0b7d 100644 --- a/projects/microb2010/cobboard/cs.c +++ b/projects/microb2010/cobboard/cs.c @@ -47,13 +47,20 @@ #include "actuator.h" #include "spickle.h" +#define DEBUG_CPLD + /* called every 5 ms */ -static void do_cs(__attribute__((unused)) void *dummy) +static void do_cs(__attribute__((unused)) void *dummy) { /* read encoders */ if (cobboard.flags & DO_ENCODERS) { encoders_spi_manage(NULL); } +#ifdef DEBUG_CPLD + cobboard.left_spickle.prev = cobboard.left_spickle.cs.filtered_feedback_value; + cobboard.right_spickle.prev = cobboard.right_spickle.cs.filtered_feedback_value; + cobboard.shovel.prev = cobboard.shovel.cs.filtered_feedback_value; +#endif /* control system */ if (cobboard.flags & DO_CS) { if (cobboard.left_spickle.on) @@ -63,6 +70,31 @@ static void do_cs(__attribute__((unused)) void *dummy) if (cobboard.shovel.on) cs_manage(&cobboard.shovel.cs); } +#ifdef DEBUG_CPLD + { + extern int16_t g_encoders_spi_previous[4]; + int32_t ls, rs, sh; + + ls = (cobboard.left_spickle.prev - cobboard.left_spickle.cs.filtered_feedback_value); + rs = (cobboard.right_spickle.prev - cobboard.right_spickle.cs.filtered_feedback_value); + sh = (cobboard.shovel.prev - cobboard.shovel.cs.filtered_feedback_value); + if (ls < -2000 || ls > 2000 || + rs < -2000 || rs > 2000 || + sh < -2000 || sh > 2000) { + printf_P(PSTR("left_spickle %ld "), ls); + printf_P(PSTR("right_spickle %ld "), rs); + printf_P(PSTR("shovel %ld "), sh); + printf_P(PSTR("/ %d %d %d %d\r\n"), + g_encoders_spi_previous[0], + g_encoders_spi_previous[1], + g_encoders_spi_previous[2], + g_encoders_spi_previous[3]); + BRAKE_ON(); + while (1); + } + } +#endif + if (cobboard.flags & DO_BD) { bd_manage_from_cs(&cobboard.left_spickle.bd, &cobboard.left_spickle.cs); bd_manage_from_cs(&cobboard.right_spickle.bd, &cobboard.right_spickle.cs); @@ -77,7 +109,7 @@ static void do_cs(__attribute__((unused)) void *dummy) void dump_cs_debug(const char *name, struct cs *cs) { DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld " - "in=% .5ld out=% .5ld", + "in=% .5ld out=% .5ld", name, cs_get_consign(cs), cs_get_filtered_consign(cs), cs_get_error(cs), cs_get_filtered_feedback(cs), cs_get_out(cs)); @@ -86,7 +118,7 @@ void dump_cs_debug(const char *name, struct cs *cs) 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"), + "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)); @@ -147,14 +179,19 @@ void microb_cs_init(void) /* ---- CS shovel */ /* PID */ pid_init(&cobboard.shovel.pid); - pid_set_gains(&cobboard.shovel.pid, 1000, 10, 200); + pid_set_gains(&cobboard.shovel.pid, 1000, 10, 1400); pid_set_maximums(&cobboard.shovel.pid, 0, 10000, 3200); /* max is 18 V */ pid_set_out_shift(&cobboard.shovel.pid, 10); pid_set_derivate_filter(&cobboard.shovel.pid, 4); + /* quadramp */ + quadramp_init(&cobboard.shovel.qr); + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 20); /* set accel */ + /* CS */ cs_init(&cobboard.shovel.cs); - //cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr); + cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr); cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid); cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM); cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER); @@ -170,7 +207,7 @@ void microb_cs_init(void) cobboard.right_spickle.on = 0; cobboard.shovel.on = 0; - scheduler_add_periodical_event_priority(do_cs, NULL, - CS_PERIOD / SCHEDULER_UNIT, + scheduler_add_periodical_event_priority(do_cs, NULL, + CS_PERIOD / SCHEDULER_UNIT, CS_PRIO); } diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index 39318d9..62103c8 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation * Olivier Matz - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -65,7 +65,7 @@ /* 0 means "programmed" * ---- with 16 Mhz quartz * CKSEL 3-0 : 0111 - * SUT 1-0 : 10 + * SUT 1-0 : 10 * CKDIV8 : 1 * ---- bootloader * BOOTZ 1-0 : 01 (4K bootloader) @@ -109,7 +109,7 @@ void bootloader(void) __asm__ __volatile__ ("ldi r31,0xf8\n"); __asm__ __volatile__ ("ldi r30,0x00\n"); __asm__ __volatile__ ("eijmp\n"); - + /* never returns */ } @@ -122,7 +122,7 @@ void do_led_blink(__attribute__((unused)) void *dummy) LED1_ON(); else LED1_OFF(); - + a = !a; #endif } @@ -183,6 +183,8 @@ int main(void) error_register_notice(mylog); error_register_debug(mylog); + wait_ms(3000); + /* SPI + ENCODERS */ encoders_spi_init(); /* this will also init spi hardware */ @@ -196,11 +198,11 @@ int main(void) timer0_register_OV_intr(main_timer_interrupt); /* PWM */ - PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, + PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, TIMER1_PRESCALER_DIV_1); - PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, + PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, TIMER4_PRESCALER_DIV_1); - + PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED | PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4); PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED, @@ -213,11 +215,11 @@ int main(void) /* servos */ - PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10, + PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10, TIMER1_PRESCALER_DIV_256); PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL, NULL, 0); - PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10, + PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10, TIMER1_PRESCALER_DIV_256); PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL, NULL, 0); @@ -225,12 +227,12 @@ int main(void) NULL, 0); PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL, NULL, 0); - + /* SCHEDULER */ scheduler_init(); - scheduler_add_periodical_event_priority(do_led_blink, NULL, - 100000L / SCHEDULER_UNIT, + scheduler_add_periodical_event_priority(do_led_blink, NULL, + 100000L / SCHEDULER_UNIT, LED_PRIO); /* all cs management */ microb_cs_init(); diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index a0dbac8..0d07a9c 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -51,6 +51,8 @@ #define SHOVEL_ENCODER ((void *)2) #define SERVO_DOOR_PWM ((void *)&gen.servo2) +#define SERVO_CARRY_L_PWM ((void *)&gen.servo1) +#define SERVO_CARRY_R_PWM ((void *)&gen.servo3) #define LEFT_SPICKLE_PWM ((void *)&gen.pwm1_4A) #define RIGHT_SPICKLE_PWM ((void *)&gen.pwm2_4B) @@ -92,7 +94,7 @@ struct genboard { struct pwm_ng servo2; struct pwm_ng servo3; struct pwm_ng servo4; - + /* ax12 interface */ AX12 ax12; @@ -104,6 +106,7 @@ struct genboard { struct cs_block { uint8_t on; + int32_t prev; struct cs cs; struct pid_filter pid; struct quadramp_filter qr; diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index eec58b1..b820d98 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -143,8 +143,8 @@ struct sensor_filter { */ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_CAP1] = { 10, 0, 3, 7, 0, 0 }, /* 0 */ - [S_FRONT] = { 5, 0, 4, 1, 0, 0 }, /* 1 */ - [S_CAP3] = { 10, 0, 3, 7, 0, 0 }, /* 2 */ + [S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 0 }, /* 1 */ + [S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ [S_LCOB] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h index 6f47742..4e634fc 100644 --- a/projects/microb2010/cobboard/sensor.h +++ b/projects/microb2010/cobboard/sensor.h @@ -29,8 +29,8 @@ /* synchronize with sensor.c */ #define S_CAP1 0 -#define S_FRONT 1 -#define S_CAP3 2 +#define S_COB_INSIDE_L 1 +#define S_COB_INSIDE_R 2 #define S_CAP4 3 #define S_LCOB 4 #define S_LEFT 5 diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index 3686eb7..62732c2 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -102,7 +102,7 @@ void spickle_set(void *mot, int32_t cmd) { static int32_t oldpos_left, oldpos_right; int32_t oldpos, pos, maxcmd, speed; - + if (mot == LEFT_SPICKLE_PWM) { pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER); oldpos = oldpos_left; diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 2903a4a..3308405 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -114,6 +114,13 @@ static uint8_t state_cob_color(uint8_t side) return 0; } +/* return true if the cob is correctly inside */ +static uint8_t state_cob_inside(void) +{ + return sensor_get(S_COB_INSIDE_L) && + sensor_get(S_COB_INSIDE_R); +} + /* set a new state, return 0 on success */ int8_t state_set_mode(uint8_t mode) { @@ -163,23 +170,34 @@ static void state_do_harvest(uint8_t side) /* eat the cob */ spickle_pack(side); - state_debug_wait_key_pressed(); /* xxx */ time_wait_ms(250); left_cobroller_on(); delay = spickle_get_pack_delay(side); - time_wait_ms(delay); + + WAIT_COND_OR_TIMEOUT(state_cob_inside(), delay); /* redeploy the spickle */ spickle_deploy(side); state_debug_wait_key_pressed(); + /* let the cob go */ + servo_carry_open(); + wait_ms(300); /* XXX */ + state_debug_wait_key_pressed(); + cob_count ++; /* store it */ shovel_up(); wait_ms(200); state_debug_wait_key_pressed(); + + /* close the carry servos */ + servo_carry_close(); + wait_ms(300); /* XXX */ + state_debug_wait_key_pressed(); + shovel_down(); left_cobroller_off(); state_debug_wait_key_pressed(); @@ -191,10 +209,12 @@ static void state_do_eject(void) { cob_count = 0; shovel_mid(); + servo_carry_open(); servo_door_open(); time_wait_ms(2000); shovel_down(); servo_door_close(); + servo_carry_close(); } /* main state machine */ @@ -246,6 +266,7 @@ void state_init(void) vt100_init(&local_vt100); shovel_down(); servo_door_close(); + servo_carry_close(); spickle_pack(I2C_LEFT_SIDE); spickle_pack(I2C_RIGHT_SIDE); state_mode = 0; -- 2.20.1