SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
SRC += commands_cs.c commands_cobboard.c commands.c
SRC += i2c_protocol.c sensor.c actuator.c cs.c
-SRC += state.c ax12_user.c spickle.c
+SRC += state.c ax12_user.c spickle.c shovel.c
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
#include "sensor.h"
#include "../common/i2c_commands.h"
-#include "actuator.h"
#include "main.h"
+#include "actuator.h"
+
+#define COBROLLER_SPEED 800
+
+#define SERVO_DOOR_PWM ((void *)&gen.servo2)
+#define SERVO_DOOR_OPEN 250
+#define SERVO_DOOR_CLOSED 470
+
+void actuator_init(void);
void servo_carry_open(void)
{
+ /* TODO */
}
void servo_carry_close(void)
{
+ /* TODO */
}
void servo_door_open(void)
{
+ pwm_ng_set(SERVO_DOOR_PWM, SERVO_DOOR_OPEN);
}
void servo_door_close(void)
{
+ pwm_ng_set(SERVO_DOOR_PWM, SERVO_DOOR_CLOSED);
+}
+
+void left_cobroller_on(void)
+{
+ cobboard.left_cobroller_speed = COBROLLER_SPEED;
+}
+
+void right_cobroller_on(void)
+{
+ cobboard.right_cobroller_speed = COBROLLER_SPEED;
+}
+
+void left_cobroller_off(void)
+{
+ cobboard.left_cobroller_speed = 0;
+}
+
+void right_cobroller_off(void)
+{
+ cobboard.right_cobroller_speed = 0;
}
void actuator_init(void)
#define _ACTUATOR_H_
void actuator_init(void);
+
void servo_carry_open(void);
void servo_carry_close(void);
void servo_door_open(void);
void servo_door_close(void);
+void left_cobroller_on(void);
+void right_cobroller_on(void);
+void left_cobroller_off(void);
+void right_cobroller_off(void);
+
#endif
extern parse_pgm_inst_t cmd_state_debug;
extern parse_pgm_inst_t cmd_state_machine;
extern parse_pgm_inst_t cmd_servo_door;
+extern parse_pgm_inst_t cmd_cobroller;
extern parse_pgm_inst_t cmd_servo_carry;
extern parse_pgm_inst_t cmd_spickle;
extern parse_pgm_inst_t cmd_spickle_params;
extern parse_pgm_inst_t cmd_spickle_params_show;
+extern parse_pgm_inst_t cmd_shovel;
extern parse_pgm_inst_t cmd_test;
(parse_pgm_inst_t *)&cmd_state_debug,
(parse_pgm_inst_t *)&cmd_state_machine,
(parse_pgm_inst_t *)&cmd_servo_door,
+ (parse_pgm_inst_t *)&cmd_cobroller,
(parse_pgm_inst_t *)&cmd_servo_carry,
(parse_pgm_inst_t *)&cmd_spickle,
(parse_pgm_inst_t *)&cmd_spickle_params,
(parse_pgm_inst_t *)&cmd_spickle_params_show,
+ (parse_pgm_inst_t *)&cmd_shovel,
(parse_pgm_inst_t *)&cmd_test,
NULL,
#include "i2c_protocol.h"
#include "actuator.h"
#include "spickle.h"
+#include "shovel.h"
extern uint16_t state_debug;
__attribute__((unused)) void *data)
{
struct cmd_state1_result *res = parsed_result;
- struct i2c_cmd_cobboard_set_mode command;
- if (!strcmp_P(res->arg1, PSTR("init"))) {
+ if (!strcmp_P(res->arg1, PSTR("init")))
state_init();
- return;
- }
+ else if (!strcmp_P(res->arg1, PSTR("eject")))
+ state_set_mode(I2C_COBBOARD_MODE_EJECT);
- if (!strcmp_P(res->arg1, PSTR("manual")))
- command.mode = I2C_COBBOARD_MODE_MANUAL;
- else if (!strcmp_P(res->arg1, PSTR("harvest")))
- command.mode = I2C_COBBOARD_MODE_HARVEST;
- state_set_mode(&command);
+ /* other commands */
}
prog_char str_state1_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
-prog_char str_state1_arg1[] = "init#manual";
+prog_char str_state1_arg1[] = "init#eject";
parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
prog_char help_state1[] = "set cobboard mode";
__attribute__((unused)) void *data)
{
struct cmd_state2_result *res = parsed_result;
- struct i2c_cmd_cobboard_set_mode command;
- uint8_t side;
+ uint8_t side, mode = state_get_mode();
- if (!strcmp_P(res->arg2, PSTR("left")))
+ if (!strcmp_P(res->arg2, PSTR("left"))) {
side = I2C_LEFT_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("right")))
+ mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
+ }
+ else {
side = I2C_RIGHT_SIDE;
+ mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
+ }
- if (!strcmp_P(res->arg1, PSTR("yyy"))) {
+ if (!strcmp_P(res->arg1, PSTR("pack"))) {
+ /* nothing to do */
}
- else if (!strcmp_P(res->arg1, PSTR("xxx"))) {
+ else if (!strcmp_P(res->arg1, PSTR("deploy"))) {
+ if (side == I2C_LEFT_SIDE)
+ mode |= I2C_COBBOARD_MODE_L_DEPLOY;
+ else
+ mode |= I2C_COBBOARD_MODE_R_DEPLOY;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("harvest"))) {
+ if (side == I2C_LEFT_SIDE) {
+ mode |= I2C_COBBOARD_MODE_L_DEPLOY;
+ mode |= I2C_COBBOARD_MODE_L_HARVEST;
+ }
+ else {
+ mode |= I2C_COBBOARD_MODE_R_DEPLOY;
+ mode |= I2C_COBBOARD_MODE_R_HARVEST;
+ }
}
-
- state_set_mode(&command);
+ state_set_mode(mode);
}
prog_char str_state2_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
-prog_char str_state2_arg1[] = "xxx";
+prog_char str_state2_arg1[] = "harvest#deploy#pack";
parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
prog_char str_state2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
__attribute__((unused)) void *data)
{
struct cmd_state3_result *res = parsed_result;
- struct i2c_cmd_cobboard_set_mode command;
if (!strcmp_P(res->arg1, PSTR("xxx"))) {
/* xxx = res->arg2 */
}
else if (!strcmp_P(res->arg1, PSTR("yyy"))) {
}
- state_set_mode(&command);
+ state_set_mode(0);
}
prog_char str_state3_arg0[] = "cobboard";
},
};
+/**********************************************************/
+/* cobroller */
+
+/* this structure is filled when cmd_cobroller is parsed successfully */
+struct cmd_cobroller_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_cobroller is parsed successfully */
+static void cmd_cobroller_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_cobroller_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("left"))) {
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ left_cobroller_on();
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ left_cobroller_off();
+ }
+ else if (!strcmp_P(res->arg1, PSTR("right"))) {
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ right_cobroller_on();
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ right_cobroller_off();
+ }
+}
+
+prog_char str_cobroller_arg0[] = "cobroller";
+parse_pgm_token_string_t cmd_cobroller_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg0, str_cobroller_arg0);
+prog_char str_cobroller_arg1[] = "left#right";
+parse_pgm_token_string_t cmd_cobroller_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg1, str_cobroller_arg1);
+prog_char str_cobroller_arg2[] = "on#off";
+parse_pgm_token_string_t cmd_cobroller_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg2, str_cobroller_arg2);
+
+prog_char help_cobroller[] = "Servo door function";
+parse_pgm_inst_t cmd_cobroller = {
+ .f = cmd_cobroller_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_cobroller,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_cobroller_arg0,
+ (prog_void *)&cmd_cobroller_arg1,
+ (prog_void *)&cmd_cobroller_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* shovel */
+
+/* this structure is filled when cmd_shovel is parsed successfully */
+struct cmd_shovel_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_shovel is parsed successfully */
+static void cmd_shovel_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_shovel_result *res = parsed_result;
+ if (!strcmp_P(res->arg1, PSTR("down")))
+ shovel_down();
+ else if (!strcmp_P(res->arg1, PSTR("up")))
+ shovel_up();
+ else if (!strcmp_P(res->arg1, PSTR("mid")))
+ shovel_mid();
+}
+
+prog_char str_shovel_arg0[] = "shovel";
+parse_pgm_token_string_t cmd_shovel_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_shovel_result, arg0, str_shovel_arg0);
+prog_char str_shovel_arg1[] = "down#up#mid";
+parse_pgm_token_string_t cmd_shovel_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_shovel_result, arg1, str_shovel_arg1);
+
+prog_char help_shovel[] = "Servo shovel function";
+parse_pgm_inst_t cmd_shovel = {
+ .f = cmd_shovel_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_shovel,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_shovel_arg0,
+ (prog_void *)&cmd_shovel_arg1,
+ NULL,
+ },
+};
+
/**********************************************************/
/* Servo_Carry */
};
/**********************************************************/
-/* Spickles tests */
+/* Spickle tests */
/* this structure is filled when cmd_spickle is parsed successfully */
struct cmd_spickle_result {
fixed_string_t arg0;
fixed_string_t arg1;
+ fixed_string_t arg2;
};
/* function called when cmd_spickle is parsed successfully */
__attribute__((unused)) void *data)
{
struct cmd_spickle_result * res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("up"))) {
- spickle_up();
- }
- else if (!strcmp_P(res->arg1, PSTR("down"))) {
- spickle_down();
- }
- else if (!strcmp_P(res->arg1, PSTR("stop"))) {
- spickle_stop();
+ uint8_t side;
+
+ if (!strcmp_P(res->arg1, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else
+ side = I2C_RIGHT_SIDE;
+
+ if (!strcmp_P(res->arg2, PSTR("deploy"))) {
+ spickle_deploy(side);
}
- else if (!strcmp_P(res->arg1, PSTR("auto"))) {
- spickle_auto();
+ else if (!strcmp_P(res->arg2, PSTR("pack"))) {
+ spickle_pack(side);
}
-
printf_P(PSTR("done\r\n"));
}
prog_char str_spickle_arg0[] = "spickle";
-parse_pgm_token_string_t cmd_spickle_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg0, str_spickle_arg0);
-prog_char str_spickle_arg1[] = "auto#up#down#stop";
-parse_pgm_token_string_t cmd_spickle_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1);
-
-prog_char help_spickle[] = "spickle auto mode: spickle auto delay_up delay_down";
+parse_pgm_token_string_t cmd_spickle_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg0, str_spickle_arg0);
+prog_char str_spickle_arg1[] = "left#right";
+parse_pgm_token_string_t cmd_spickle_arg1 =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1);
+prog_char str_spickle_arg2[] = "deploy#pack";
+parse_pgm_token_string_t cmd_spickle_arg2 =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg2, str_spickle_arg2);
+
+prog_char help_spickle[] = "move spickle";
parse_pgm_inst_t cmd_spickle = {
.f = cmd_spickle_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
.tokens = { /* token list, NULL terminated */
(prog_void *)&cmd_spickle_arg0,
(prog_void *)&cmd_spickle_arg1,
+ (prog_void *)&cmd_spickle_arg2,
NULL,
},
};
struct cmd_spickle_params_result {
fixed_string_t arg0;
fixed_string_t arg1;
- int32_t arg2;
+ fixed_string_t arg2;
int32_t arg3;
+ int32_t arg4;
};
/* function called when cmd_spickle_params is parsed successfully */
__attribute__((unused)) void *data)
{
struct cmd_spickle_params_result * res = parsed_result;
-
-
- if (!strcmp_P(res->arg1, PSTR("delay"))) {
- spickle_set_delays(res->arg2, res->arg3);
- }
- else if (!strcmp_P(res->arg1, PSTR("coef"))) {
- spickle_set_coefs(res->arg2, res->arg3);
- }
- else if (!strcmp_P(res->arg1, PSTR("pos"))) {
- spickle_set_pos(res->arg2, res->arg3);
+ uint8_t side;
+
+ if (!strcmp_P(res->arg1, PSTR("show"))) {
+ spickle_dump_params();
+ return;
}
- /* else show */
- spickle_dump_params();
+ if (!strcmp_P(res->arg1, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else
+ side = I2C_RIGHT_SIDE;
+
+ if (!strcmp_P(res->arg2, PSTR("pos")))
+ spickle_set_pos(side, res->arg3, res->arg4);
+ else if (!strcmp_P(res->arg2, PSTR("delay")))
+ spickle_set_delay(side, res->arg3, res->arg4);
}
prog_char str_spickle_params_arg0[] = "spickle_params";
-parse_pgm_token_string_t cmd_spickle_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg0, str_spickle_params_arg0);
-prog_char str_spickle_params_arg1[] = "delay#pos#coef";
-parse_pgm_token_string_t cmd_spickle_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1);
-parse_pgm_token_num_t cmd_spickle_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg2, INT32);
-parse_pgm_token_num_t cmd_spickle_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32);
-
-prog_char help_spickle_params[] = "Set spickle_params values";
+parse_pgm_token_string_t cmd_spickle_params_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg0, str_spickle_params_arg0);
+prog_char str_spickle_params_arg1[] = "left#right";
+parse_pgm_token_string_t cmd_spickle_params_arg1 =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1);
+prog_char str_spickle_params_arg2[] = "pos#delay";
+parse_pgm_token_string_t cmd_spickle_params_arg2 =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg2, str_spickle_params_arg2);
+parse_pgm_token_num_t cmd_spickle_params_arg3 =
+ TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32);
+parse_pgm_token_num_t cmd_spickle_params_arg4 =
+ TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg4, INT32);
+
+prog_char help_spickle_params[] = "Set spickle pos values";
parse_pgm_inst_t cmd_spickle_params = {
.f = cmd_spickle_params_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
(prog_void *)&cmd_spickle_params_arg1,
(prog_void *)&cmd_spickle_params_arg2,
(prog_void *)&cmd_spickle_params_arg3,
+ (prog_void *)&cmd_spickle_params_arg4,
NULL,
},
};
prog_char str_spickle_params_arg1_show[] = "show";
-parse_pgm_token_string_t cmd_spickle_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1_show);
+parse_pgm_token_string_t cmd_spickle_params_arg1_show =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1_show);
prog_char help_spickle_params_show[] = "show spickle params";
parse_pgm_inst_t cmd_spickle_params_show = {
},
};
+/**********************************************************/
+/* Set Spickle Params */
+
+/* this structure is filled when cmd_spickle_params2 is parsed successfully */
+struct cmd_spickle_params2_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int32_t arg2;
+ int32_t arg3;
+};
+
+/* function called when cmd_spickle_params2 is parsed successfully */
+static void cmd_spickle_params2_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_spickle_params2_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("coef"))) {
+ spickle_set_coefs(res->arg2, res->arg3);
+ }
+
+ /* else show */
+ spickle_dump_params();
+}
+
+prog_char str_spickle_params2_arg0[] = "spickle_params2";
+parse_pgm_token_string_t cmd_spickle_params2_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg0, str_spickle_params2_arg0);
+prog_char str_spickle_params2_arg1[] = "coef";
+parse_pgm_token_string_t cmd_spickle_params2_arg1 =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg1, str_spickle_params2_arg1);
+parse_pgm_token_num_t cmd_spickle_params2_arg2 =
+ TOKEN_NUM_INITIALIZER(struct cmd_spickle_params2_result, arg2, INT32);
+parse_pgm_token_num_t cmd_spickle_params2_arg3 =
+ TOKEN_NUM_INITIALIZER(struct cmd_spickle_params2_result, arg3, INT32);
+
+prog_char help_spickle_params2[] = "Set spickle_params2 values";
+parse_pgm_inst_t cmd_spickle_params2 = {
+ .f = cmd_spickle_params2_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_spickle_params2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_spickle_params2_arg0,
+ (prog_void *)&cmd_spickle_params2_arg1,
+ (prog_void *)&cmd_spickle_params2_arg2,
+ (prog_void *)&cmd_spickle_params2_arg3,
+ NULL,
+ },
+};
+
+prog_char str_spickle_params2_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_spickle_params2_arg1_show =
+ TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg1, str_spickle_params2_arg1_show);
+
+prog_char help_spickle_params2_show[] = "show spickle params";
+parse_pgm_inst_t cmd_spickle_params2_show = {
+ .f = cmd_spickle_params2_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_spickle_params2_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_spickle_params2_arg0,
+ (prog_void *)&cmd_spickle_params2_arg1_show,
+ NULL,
+ },
+};
+
/**********************************************************/
/* Test */
}
prog_char str_test_arg0[] = "test";
-parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
+parse_pgm_token_string_t cmd_test_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
prog_char help_test[] = "Test function";
parse_pgm_inst_t cmd_test = {
ans.mode = state_get_mode();
ans.status = 0x55;
+ ans.left_cobroller_speed = cobboard.left_cobroller_speed;
+ ans.right_cobroller_speed = cobboard.right_cobroller_speed;
+
i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
sizeof(ans), I2C_CTRL_GENERIC);
}
static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
{
- state_set_mode(cmd);
+ state_set_mode(cmd->mode);
return 0;
}
#include "state.h"
#include "actuator.h"
#include "spickle.h"
+#include "shovel.h"
#include "cs.h"
#include "i2c_protocol.h"
# error not supported
#endif
- eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD);
+ //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD);
/* check eeprom to avoid to run the bad program */
if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
EEPROM_MAGIC_COBBOARD) {
/* actuators */
actuator_init();
- /* spickle */
+ /* spickle, shovel */
spickle_init();
+ shovel_init();
/* state_init(); */
uint8_t our_color;
volatile uint8_t cob_count;
volatile uint8_t status;
+
+ /* synchronized to mainboard */
+ int16_t left_cobroller_speed;
+ int16_t right_cobroller_speed;
};
extern struct genboard gen;
--- /dev/null
+/*
+ * Copyright Droids Corporation (2010)
+ *
+ * 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
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * 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: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <clock_time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+#include <rdline.h>
+
+#include "main.h"
+
+/* init spickle position at beginning */
+static void shovel_autopos(void)
+{
+ pwm_ng_set(SHOVEL_PWM, -500);
+ wait_ms(1000);
+ pwm_ng_set(LEFT_SPICKLE_PWM, 0);
+ encoders_spi_set_value(SHOVEL_ENCODER, 0);
+}
+
+void shovel_init(void)
+{
+ shovel_autopos();
+ cobboard.shovel.on = 1;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2010)
+ *
+ * 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
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * 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: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $
+ *
+ */
+
+#ifndef _SHOVEL_H_
+#define _SHOVEL_H_
+
+#define SHOVEL_DOWN 100
+#define SHOVEL_MID 4000
+#define SHOVEL_UP 10000
+
+void shovel_init(void);
+
+static inline void shovel_down(void)
+{
+ cs_set_consign(&cobboard.shovel.cs, SHOVEL_DOWN);
+}
+
+static inline void shovel_mid(void)
+{
+ cs_set_consign(&cobboard.shovel.cs, SHOVEL_MID);
+}
+
+static inline void shovel_up(void)
+{
+ cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP);
+}
+
+#endif /* _SHOVEL_H_ */
#include "sensor.h"
#include "../common/i2c_commands.h"
-#include "actuator.h"
#include "main.h"
+#include "actuator.h"
-
-#define OFF 0
-#define WAIT_SENSOR 1
-#define SENSOR_OK 2
-#define WAIT_DOWN 3
-
-static volatile uint8_t spickle_state = OFF;
-static volatile uint32_t spickle_pos_up = 35000;
-static volatile uint32_t spickle_pos_down = 0;
-static volatile uint32_t spickle_delay_up = 500;
-static volatile uint32_t spickle_delay_down = 2000;
-static volatile uint32_t delay = 0;
-static volatile int32_t spickle_k1 = 500, spickle_k2 = 20;
-static volatile int32_t spickle_cmd = 0;
+struct spickle_params {
+ /* current limit (common to left and right) */
+ int32_t k1;
+ int32_t k2;
+
+ /* cs blocks */
+ struct cs_block * const csb[2];
+
+ /* params */
+ int16_t delay_deployed[2];
+ int16_t delay_packed[2];
+ int32_t pos_deployed[2];
+ int32_t pos_packed[2];
+};
+
+static struct spickle_params spickle = {
+ .k1 = 500,
+ .k2 = 20,
+ .csb = {
+ &cobboard.left_spickle,
+ &cobboard.right_spickle,
+ },
+ .delay_deployed = {
+ 500, /* left */
+ 500, /* right */
+ },
+ .delay_packed = {
+ 500, /* left */
+ 500, /* right */
+ },
+ .pos_deployed = {
+ 35000, /* left */
+ 35000, /* right */
+ },
+ .pos_packed = {
+ 0, /* left */
+ 0, /* right */
+ },
+};
/* init spickle position at beginning */
static void spickle_autopos(void)
{
pwm_ng_set(LEFT_SPICKLE_PWM, -500);
- wait_ms(3000);
+ //pwm_ng_set(RIGHT_SPICKLE_PWM, -500);
+ wait_ms(1000);
pwm_ng_set(LEFT_SPICKLE_PWM, 0);
+ pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
+ encoders_spi_set_value(RIGHT_SPICKLE_ENCODER, 0);
}
-/* set CS command for spickle */
+/* Set CS command for spickle. Called by CS manager. */
void spickle_set(void *mot, int32_t cmd)
{
static int32_t oldpos_left, oldpos_right;
speed = pos - oldpos;
if (speed > 0 && cmd < 0)
- maxcmd = spickle_k1;
+ maxcmd = spickle.k1;
else if (speed < 0 && cmd > 0)
- maxcmd = spickle_k1;
+ maxcmd = spickle.k1;
else {
speed = ABS(speed);
- maxcmd = spickle_k1 + spickle_k2 * speed;
+ maxcmd = spickle.k1 + spickle.k2 * speed;
}
if (cmd > maxcmd)
cmd = maxcmd;
void spickle_set_coefs(uint32_t k1, uint32_t k2)
{
- spickle_k1 = k1;
- spickle_k2 = k2;
+ spickle.k1 = k1;
+ spickle.k2 = k2;
}
-void spickle_set_delays(uint32_t delay_up, uint32_t delay_down)
+void spickle_set_pos(uint8_t side, uint32_t pos_deployed, uint32_t pos_packed)
{
- spickle_delay_up = delay_up;
- spickle_delay_down = delay_down;
+ spickle.pos_deployed[side] = pos_deployed;
+ spickle.pos_packed[side] = pos_packed;
}
-void spickle_set_pos(uint32_t pos_up, uint32_t pos_down)
+void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed)
{
- spickle_pos_up = pos_up;
- spickle_pos_down = pos_down;
+ spickle.delay_deployed[side] = delay_deployed;
+ spickle.delay_packed[side] = delay_packed;
}
void spickle_dump_params(void)
{
- printf_P(PSTR("coef %ld %ld\r\n"), spickle_k1, spickle_k2);
- printf_P(PSTR("pos %ld %ld\r\n"), spickle_pos_up, spickle_pos_down);
- printf_P(PSTR("delay %ld %ld\r\n"), spickle_delay_up, spickle_delay_down);
+ printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2);
+ printf_P(PSTR("left pos %ld %ld\r\n"),
+ spickle.pos_deployed[I2C_LEFT_SIDE],
+ spickle.pos_packed[I2C_LEFT_SIDE]);
+ printf_P(PSTR("left delay %ld %ld\r\n"),
+ spickle.delay_deployed[I2C_LEFT_SIDE],
+ spickle.delay_packed[I2C_LEFT_SIDE]);
+ printf_P(PSTR("right pos %ld %ld\r\n"),
+ spickle.pos_deployed[I2C_RIGHT_SIDE],
+ spickle.pos_packed[I2C_RIGHT_SIDE]);
+ printf_P(PSTR("right delay %ld %ld\r\n"),
+ spickle.delay_deployed[I2C_RIGHT_SIDE],
+ spickle.delay_packed[I2C_RIGHT_SIDE]);
}
-void spickle_up(void)
+void spickle_deploy(uint8_t side)
{
- spickle_state = 0;
- cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
+ cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
}
-void spickle_down(void)
+void spickle_pack(uint8_t side)
{
- spickle_state = 0;
- cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down);
+ cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
}
-void spickle_stop(void)
+uint16_t spickle_get_deploy_delay(uint8_t side)
{
- spickle_state = 0;
+ return spickle.delay_deployed[side];
}
-void spickle_auto(void)
+uint16_t spickle_get_pack_delay(uint8_t side)
{
- spickle_state = WAIT_SENSOR;
- cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
-}
-
-/* for spickle auto mode */
-static void spickle_cb(__attribute__((unused)) void *dummy)
-{
- static uint8_t prev = 0;
- uint8_t val;
-
- val = sensor_get(S_LCOB);
-
- switch (spickle_state) {
- case OFF:
- break;
- case WAIT_SENSOR:
- if (val && !prev) {
- delay = spickle_delay_up;
- spickle_state = SENSOR_OK;
- }
- break;
- case SENSOR_OK:
- if (delay-- == 0) {
- cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down);
- spickle_state = WAIT_DOWN;
- delay = spickle_delay_down;
- }
- break;
- case WAIT_DOWN:
- if (delay-- == 0) {
- cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up);
- spickle_state = WAIT_SENSOR;
- }
- break;
- default:
- break;
- }
- prev = val;
+ return spickle.delay_packed[side];
}
void spickle_init(void)
{
spickle_autopos();
-
- scheduler_add_periodical_event_priority(spickle_cb, NULL,
- 1000L / SCHEDULER_UNIT,
- SPICKLE_PRIO);
+ cobboard.left_spickle.on = 1;
+ //cobboard.right_spickle.on = 1;
}
void spickle_set(void *dummy, int32_t cmd);
void spickle_set_coefs(uint32_t k1, uint32_t k2);
-void spickle_set_delays(uint32_t delay_up, uint32_t delay_down);
-void spickle_set_pos(uint32_t pos_up, uint32_t pos_down);
+void spickle_set_pos(uint8_t side, uint32_t pos_deploy, uint32_t pos_pack);
+void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed);
+
void spickle_dump_params(void);
-void spickle_left_manage(void);
-void spickle_up(void);
-void spickle_down(void);
-void spickle_stop(void);
-void spickle_auto(void);
+
+void spickle_deploy(uint8_t side);
+void spickle_pack(uint8_t side);
+
+uint16_t spickle_get_deploy_delay(uint8_t side);
+uint16_t spickle_get_pack_delay(uint8_t side);
+
void spickle_init(void);
#endif
#include "cmdline.h"
#include "sensor.h"
#include "actuator.h"
+#include "spickle.h"
+#include "shovel.h"
#include "state.h"
#define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args)
#define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args)
#define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args)
-/* shorter aliases for this file */
-#define INIT I2C_COBBOARD_MODE_INIT
-#define MANUAL I2C_COBBOARD_MODE_MANUAL
-#define HARVEST I2C_COBBOARD_MODE_HARVEST
-#define EXIT I2C_COBBOARD_MODE_EXIT
-
-static struct i2c_cmd_cobboard_set_mode mainboard_command;
static struct vt100 local_vt100;
-static volatile uint8_t prev_state;
-static volatile uint8_t changed = 0;
+static volatile uint8_t state_mode;
+static uint8_t cob_count;
+
+/* short aliases */
+#define L_DEPLOY(mode) (!!((mode) & I2C_COBBOARD_MODE_L_DEPLOY))
+#define R_DEPLOY(mode) (!!((mode) & I2C_COBBOARD_MODE_R_DEPLOY))
+#define DEPLOY(side, mode) ((side) == I2C_LEFT_SIDE ? L_DEPLOY(mode) : R_DEPLOY(mode))
+#define L_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_L_HARVEST))
+#define R_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_R_HARVEST))
+#define HARVEST(side, mode) ((side) == I2C_LEFT_SIDE ? L_HARVEST(mode) : R_HARVEST(mode))
+#define EJECT(mode) (!!((mode) & I2C_COBBOARD_MODE_EJECT))
uint8_t state_debug = 0;
-void state_dump_sensors(void)
+#if 0
+static void state_dump_sensors(void)
{
STMCH_DEBUG("TODO\n");
}
+#endif
-void state_debug_wait_key_pressed(void)
+static void state_debug_wait_key_pressed(void)
{
if (!state_debug)
return;
while(!cmdline_keypressed());
}
+/* return true if cob is present */
+static uint8_t state_cob_present(uint8_t side)
+{
+ if (side == I2C_LEFT_SIDE)
+ return sensor_get(S_LCOB);
+ else
+ /* XXX */
+ // return sensor_get(S_LCOB);
+ return 0;
+}
+
+/* return the detected color of the cob (only valid if present) */
+static uint8_t state_cob_color(uint8_t side)
+{
+ if (side == I2C_LEFT_SIDE)
+ return I2C_COB_WHITE;
+ else
+ /* XXX */
+ // return sensor_get(S_LCOB);
+ return 0;
+}
+
/* set a new state, return 0 on success */
-int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+int8_t state_set_mode(uint8_t mode)
{
- changed = 1;
- prev_state = mainboard_command.mode;
- memcpy(&mainboard_command, cmd, sizeof(mainboard_command));
- STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode);
+ state_mode = mode;
+ /* XXX synchrounous pack here */
+ STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d "
+ "r_deploy=%d r_harvest=%d eject=%d",
+ __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode),
+ R_DEPLOY(mode), R_HARVEST(mode), EJECT(mode));
return 0;
}
/* check that state is the one in parameter and that state did not
* changed */
-uint8_t state_check(uint8_t mode)
+static uint8_t state_want_exit(void)
{
int16_t c;
- if (mode != mainboard_command.mode)
- return 0;
-
- if (changed)
- return 0;
/* force quit when CTRL-C is typed */
c = cmdline_getchar();
if (c == -1)
- return 1;
- if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) {
- mainboard_command.mode = EXIT;
return 0;
- }
- return 1;
+ if (vt100_parser(&local_vt100, c) == KEY_CTRL_C)
+ return 1;
+ return 0;
}
uint8_t state_get_mode(void)
{
- return mainboard_command.mode;
+ return state_mode;
}
-/* manual mode, arm position is sent from mainboard */
-static void state_do_manual(void)
+/* harvest cobs from area */
+static void state_do_harvest(uint8_t side)
{
- if (!state_check(MANUAL))
- return;
- STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
- while (state_check(MANUAL));
-}
+ uint16_t delay;
-/* init mode */
-static void state_do_init(void)
-{
- if (!state_check(INIT))
+ /* if there is no cob, return */
+ if (state_cob_present(side))
+ return;
+
+ /* if it is black, nothing to do */
+ if (state_cob_color(side) == I2C_COB_BLACK)
return;
- state_init();
- STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
- while (state_check(INIT));
+
+ /* eat the cob */
+ spickle_pack(side);
+ state_debug_wait_key_pressed();
+ delay = spickle_get_pack_delay(side);
+ time_wait_ms(delay);
+
+ /* redeploy the spickle */
+ spickle_deploy(side);
+ state_debug_wait_key_pressed();
+
+ cob_count ++;
+
+ /* store it */
+ shovel_up();
+ wait_ms(200);
+ state_debug_wait_key_pressed();
+ shovel_down();
+ state_debug_wait_key_pressed();
}
-/* harvest columns elts from area */
-static void state_do_harvest(void)
+/* eject cobs */
+static void state_do_eject(void)
{
- if (!state_check(HARVEST))
- return;
- STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode());
- while (state_check(HARVEST));
+ cob_count = 0;
+ shovel_mid();
+ time_wait_ms(2000);
+ shovel_down();
}
+
/* main state machine */
void state_machine(void)
{
- while (state_get_mode() != EXIT) {
- changed = 0;
- state_do_init();
- state_do_manual();
- state_do_harvest();
+ while (state_want_exit() == 0) {
+
+ /* pack spickles */
+ if (!L_DEPLOY(state_mode))
+ spickle_pack(I2C_LEFT_SIDE);
+ if (!R_DEPLOY(state_mode))
+ spickle_pack(I2C_RIGHT_SIDE);
+
+ /* harvest */
+ if (L_DEPLOY(state_mode) && L_HARVEST(state_mode))
+ state_do_harvest(I2C_LEFT_SIDE);
+ if (R_DEPLOY(state_mode) && R_HARVEST(state_mode))
+ state_do_harvest(I2C_RIGHT_SIDE);
+
+ /* eject */
+ if (EJECT(state_mode))
+ state_do_eject();
}
}
void state_init(void)
{
vt100_init(&local_vt100);
- mainboard_command.mode = HARVEST;
- cobboard.cob_count = 0;
+ shovel_down();
+ spickle_pack(I2C_LEFT_SIDE);
+ spickle_pack(I2C_RIGHT_SIDE);
+ state_mode = 0;
+ cob_count = 0;
}
#define _STATE_H_
/* set a new state, return 0 on success */
-int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd);
+int8_t state_set_mode(uint8_t mode);
/* get current state */
uint8_t state_get_mode(void);
#define I2C_COLOR_RED 0
#define I2C_COLOR_GREEN 1
+#define I2C_COB_BLACK 0
+#define I2C_COB_WHITE 1
+
struct i2c_cmd_hdr {
uint8_t cmd;
};
struct i2c_cmd_cobboard_set_mode {
struct i2c_cmd_hdr hdr;
-#define I2C_COBBOARD_MODE_INIT 0x00
-#define I2C_COBBOARD_MODE_MANUAL 0x01
-#define I2C_COBBOARD_MODE_HARVEST 0x02
-#define I2C_COBBOARD_MODE_EXIT 0xFF
- uint8_t mode;
- union {
- struct {
- } manual;
- struct {
- } harvest;
- };
+#define I2C_COBBOARD_MODE_L_DEPLOY 0x01 /* deploy the spickle */
+#define I2C_COBBOARD_MODE_L_HARVEST 0x02 /* auto harvest withe cobs */
+#define I2C_COBBOARD_MODE_R_DEPLOY 0x04 /* deploy the spickle */
+#define I2C_COBBOARD_MODE_R_HARVEST 0x08 /* auto harvest withe cobs */
+#define I2C_COBBOARD_MODE_EJECT 0x10 /* eject cobs */
+ uint8_t mode;
};
/****/
uint8_t status;
uint8_t cob_count;
+
+ int16_t left_cobroller_speed;
+ int16_t right_cobroller_speed;
};
#define I2C_REQ_BALLBOARD_STATUS 0x82
git.droids-corp.org / aversive.git / commitdiff