From 5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 19 Dec 2009 19:29:44 +0100 Subject: [PATCH 1/1] microb 2010 --- projects/microb2010/bootloader/.config | 278 +++ projects/microb2010/bootloader/Makefile | 41 + projects/microb2010/bootloader/main.c | 361 +++ projects/microb2010/bootloader/uart_config.h | 120 + projects/microb2010/common/avr6.x | 269 ++ projects/microb2010/common/eeprom_mapping.h | 33 + projects/microb2010/common/i2c_commands.h | 295 +++ .../microb2010/common/i2c_commands.h.~1.9.~ | 291 +++ projects/microb2010/mainboard/.config | 252 ++ projects/microb2010/mainboard/.config.~1.5.~ | 251 ++ projects/microb2010/mainboard/Makefile | 30 + projects/microb2010/mainboard/Makefile.~1.6.~ | 29 + projects/microb2010/mainboard/actuator.c | 77 + projects/microb2010/mainboard/actuator.h | 25 + projects/microb2010/mainboard/adc_config.h | 0 projects/microb2010/mainboard/ax12_config.h | 7 + projects/microb2010/mainboard/ax12_user.c | 176 ++ projects/microb2010/mainboard/ax12_user.h | 36 + projects/microb2010/mainboard/beacon_test.png | Bin 0 -> 47409 bytes projects/microb2010/mainboard/cmdline.c | 172 ++ projects/microb2010/mainboard/cmdline.h | 44 + projects/microb2010/mainboard/commands.c | 219 ++ projects/microb2010/mainboard/commands_ax12.c | 375 +++ projects/microb2010/mainboard/commands_cs.c | 677 ++++++ projects/microb2010/mainboard/commands_gen.c | 586 +++++ .../microb2010/mainboard/commands_mainboard.c | 2154 +++++++++++++++++ projects/microb2010/mainboard/commands_traj.c | 1146 +++++++++ projects/microb2010/mainboard/cs.c | 239 ++ projects/microb2010/mainboard/cs.h | 26 + .../microb2010/mainboard/diagnostic_config.h | 44 + .../mainboard/encoders_spi_config.h | 33 + projects/microb2010/mainboard/error_config.h | 31 + projects/microb2010/mainboard/i2c_config.h | 30 + projects/microb2010/mainboard/i2c_protocol.c | 687 ++++++ projects/microb2010/mainboard/i2c_protocol.h | 92 + projects/microb2010/mainboard/main.c | 295 +++ projects/microb2010/mainboard/main.h | 227 ++ .../mainboard/obstacle_avoidance_config.h | 25 + projects/microb2010/mainboard/pid_config.h | 30 + projects/microb2010/mainboard/rdline_config.h | 0 .../microb2010/mainboard/scheduler_config.h | 47 + projects/microb2010/mainboard/sensor.c | 293 +++ projects/microb2010/mainboard/sensor.h | 62 + projects/microb2010/mainboard/spi_config.h | 36 + projects/microb2010/mainboard/strat.c | 807 ++++++ projects/microb2010/mainboard/strat.h | 302 +++ projects/microb2010/mainboard/strat_avoid.c | 535 ++++ projects/microb2010/mainboard/strat_avoid.h | 30 + projects/microb2010/mainboard/strat_base.c | 512 ++++ projects/microb2010/mainboard/strat_base.h | 90 + .../microb2010/mainboard/strat_building.c | 907 +++++++ .../microb2010/mainboard/strat_column_disp.c | 485 ++++ projects/microb2010/mainboard/strat_lintel.c | 247 ++ projects/microb2010/mainboard/strat_scan.c | 689 ++++++ .../mainboard/strat_static_columns.c | 416 ++++ projects/microb2010/mainboard/strat_utils.c | 413 ++++ projects/microb2010/mainboard/strat_utils.h | 76 + projects/microb2010/mainboard/time_config.h | 23 + projects/microb2010/mainboard/timer_config.h | 36 + projects/microb2010/mainboard/uart_config.h | 94 + projects/microb2010/mechboard/.config | 279 +++ projects/microb2010/mechboard/Makefile | 35 + projects/microb2010/mechboard/actuator.c | 163 ++ projects/microb2010/mechboard/actuator.h | 39 + projects/microb2010/mechboard/adc_config.h | 0 projects/microb2010/mechboard/arm_highlevel.c | 644 +++++ projects/microb2010/mechboard/arm_highlevel.h | 84 + projects/microb2010/mechboard/arm_xy.c | 809 +++++++ projects/microb2010/mechboard/arm_xy.h | 123 + projects/microb2010/mechboard/ax12_config.h | 7 + projects/microb2010/mechboard/ax12_user.c | 305 +++ projects/microb2010/mechboard/ax12_user.h | 76 + projects/microb2010/mechboard/cmdline.c | 149 ++ projects/microb2010/mechboard/cmdline.h | 40 + projects/microb2010/mechboard/commands.c | 150 ++ projects/microb2010/mechboard/commands_ax12.c | 427 ++++ projects/microb2010/mechboard/commands_cs.c | 670 +++++ projects/microb2010/mechboard/commands_gen.c | 579 +++++ .../microb2010/mechboard/commands_mechboard.c | 1033 ++++++++ projects/microb2010/mechboard/cs.c | 163 ++ projects/microb2010/mechboard/cs.h | 25 + .../microb2010/mechboard/diagnostic_config.h | 44 + .../mechboard/encoders_spi_config.h | 33 + projects/microb2010/mechboard/error_config.h | 31 + projects/microb2010/mechboard/i2c_config.h | 30 + projects/microb2010/mechboard/i2c_protocol.c | 211 ++ projects/microb2010/mechboard/i2c_protocol.h | 30 + projects/microb2010/mechboard/main.c | 269 ++ projects/microb2010/mechboard/main.h | 174 ++ projects/microb2010/mechboard/pid_config.h | 30 + projects/microb2010/mechboard/rdline_config.h | 0 .../microb2010/mechboard/scheduler_config.h | 47 + projects/microb2010/mechboard/sensor.c | 254 ++ projects/microb2010/mechboard/sensor.h | 56 + projects/microb2010/mechboard/spi_config.h | 36 + projects/microb2010/mechboard/state.c | 1407 +++++++++++ projects/microb2010/mechboard/state.h | 40 + projects/microb2010/mechboard/time_config.h | 23 + projects/microb2010/mechboard/timer_config.h | 36 + projects/microb2010/mechboard/uart_config.h | 67 + projects/microb2010/microb_cmd/microbcmd.py | 929 +++++++ projects/microb2010/sensorboard/.config | 279 +++ projects/microb2010/sensorboard/Makefile | 46 + projects/microb2010/sensorboard/actuator.c | 61 + projects/microb2010/sensorboard/actuator.h | 25 + projects/microb2010/sensorboard/adc_config.h | 0 projects/microb2010/sensorboard/ax12_config.h | 7 + projects/microb2010/sensorboard/ax12_user.c | 169 ++ projects/microb2010/sensorboard/ax12_user.h | 36 + projects/microb2010/sensorboard/beacon.c | 397 +++ projects/microb2010/sensorboard/beacon.h | 58 + projects/microb2010/sensorboard/cmdline.c | 149 ++ projects/microb2010/sensorboard/cmdline.h | 40 + projects/microb2010/sensorboard/commands.c | 128 + .../microb2010/sensorboard/commands_ax12.c | 368 +++ projects/microb2010/sensorboard/commands_cs.c | 670 +++++ .../microb2010/sensorboard/commands_gen.c | 573 +++++ .../microb2010/sensorboard/commands_scan.c | 401 +++ .../sensorboard/commands_sensorboard.c | 197 ++ projects/microb2010/sensorboard/cs.c | 145 ++ projects/microb2010/sensorboard/cs.h | 25 + .../sensorboard/diagnostic_config.h | 44 + .../sensorboard/encoders_spi_config.h | 33 + .../microb2010/sensorboard/error_config.h | 31 + .../microb2010/sensorboard/gen_scan_tab.c | 680 ++++++ projects/microb2010/sensorboard/i2c_config.h | 30 + .../microb2010/sensorboard/i2c_protocol.c | 256 ++ .../microb2010/sensorboard/i2c_protocol.h | 30 + .../microb2010/sensorboard/img_processing.c | 1619 +++++++++++++ .../microb2010/sensorboard/img_processing.h | 111 + projects/microb2010/sensorboard/main.c | 279 +++ projects/microb2010/sensorboard/main.h | 154 ++ projects/microb2010/sensorboard/pid_config.h | 30 + .../microb2010/sensorboard/rdline_config.h | 0 projects/microb2010/sensorboard/scanner.c | 898 +++++++ projects/microb2010/sensorboard/scanner.h | 158 ++ .../microb2010/sensorboard/scheduler_config.h | 47 + projects/microb2010/sensorboard/sensor.c | 258 ++ projects/microb2010/sensorboard/sensor.h | 56 + projects/microb2010/sensorboard/spi_config.h | 36 + projects/microb2010/sensorboard/time_config.h | 23 + .../microb2010/sensorboard/timer_config.h | 36 + projects/microb2010/sensorboard/uart_config.h | 102 + .../microb2010/tests/static_beacon/.config | 251 ++ .../tests/static_beacon/.config.old | 251 ++ .../microb2010/tests/static_beacon/Makefile | 24 + .../microb2010/tests/static_beacon/coin.c | 472 ++++ projects/microb2010/tests/static_beacon/hfuse | 1 + .../microb2010/tests/static_beacon/hfuse_new | 2 + projects/microb2010/tests/static_beacon/lfuse | 1 + .../microb2010/tests/static_beacon/lfuse_new | 2 + .../tests/static_beacon/static_beacon.bin | 2 + .../tests/static_beacon/static_beacon.c | 280 +++ .../microb2010/tests/test_board2008/.config | 280 +++ .../tests/test_board2008/.config.old | 280 +++ .../microb2010/tests/test_board2008/Makefile | 27 + .../microb2010/tests/test_board2008/action.c | 46 + .../microb2010/tests/test_board2008/action.h | 37 + .../tests/test_board2008/actuator.c | 366 +++ .../tests/test_board2008/actuator.h | 53 + .../tests/test_board2008/adc_config.h | 7 + .../microb2010/tests/test_board2008/cmdline.c | 167 ++ .../microb2010/tests/test_board2008/cmdline.h | 44 + .../tests/test_board2008/commands.c | 192 ++ .../tests/test_board2008/commands_ax12.c | 375 +++ .../tests/test_board2008/commands_cs.c | 679 ++++++ .../tests/test_board2008/commands_gen.c | 612 +++++ .../tests/test_board2008/commands_mainboard.c | 653 +++++ .../tests/test_board2008/commands_traj.c | 720 ++++++ projects/microb2010/tests/test_board2008/cs.c | 315 +++ projects/microb2010/tests/test_board2008/cs.h | 26 + .../tests/test_board2008/diagnostic_config.h | 44 + .../test_board2008/encoders_microb_config.h | 45 + .../test_board2008/encoders_spi_config.h | 33 + .../tests/test_board2008/error_config.h | 31 + .../microb2010/tests/test_board2008/main. | 0 .../microb2010/tests/test_board2008/main.bin | Bin 0 -> 68622 bytes .../microb2010/tests/test_board2008/main.c | 193 ++ .../microb2010/tests/test_board2008/main.h | 225 ++ .../tests/test_board2008/pid_config.h | 30 + .../tests/test_board2008/rdline_config.h | 0 .../tests/test_board2008/scheduler_config.h | 47 + .../microb2010/tests/test_board2008/sensor.c | 290 +++ .../microb2010/tests/test_board2008/sensor.h | 62 + .../tests/test_board2008/spi_config.h | 36 + .../tests/test_board2008/strat_base.c | 271 +++ .../tests/test_board2008/strat_base.h | 120 + .../tests/test_board2008/strat_utils.c | 404 ++++ .../tests/test_board2008/strat_utils.h | 76 + .../tests/test_board2008/time_config.h | 23 + .../tests/test_board2008/timer_config.h | 36 + .../tests/test_board2008/uart_config.h | 65 + 192 files changed, 42231 insertions(+) create mode 100644 projects/microb2010/bootloader/.config create mode 100755 projects/microb2010/bootloader/Makefile create mode 100755 projects/microb2010/bootloader/main.c create mode 100755 projects/microb2010/bootloader/uart_config.h create mode 100644 projects/microb2010/common/avr6.x create mode 100644 projects/microb2010/common/eeprom_mapping.h create mode 100644 projects/microb2010/common/i2c_commands.h create mode 100644 projects/microb2010/common/i2c_commands.h.~1.9.~ create mode 100644 projects/microb2010/mainboard/.config create mode 100644 projects/microb2010/mainboard/.config.~1.5.~ create mode 100755 projects/microb2010/mainboard/Makefile create mode 100755 projects/microb2010/mainboard/Makefile.~1.6.~ create mode 100644 projects/microb2010/mainboard/actuator.c create mode 100644 projects/microb2010/mainboard/actuator.h create mode 100644 projects/microb2010/mainboard/adc_config.h create mode 100755 projects/microb2010/mainboard/ax12_config.h create mode 100644 projects/microb2010/mainboard/ax12_user.c create mode 100644 projects/microb2010/mainboard/ax12_user.h create mode 100644 projects/microb2010/mainboard/beacon_test.png create mode 100644 projects/microb2010/mainboard/cmdline.c create mode 100644 projects/microb2010/mainboard/cmdline.h create mode 100644 projects/microb2010/mainboard/commands.c create mode 100644 projects/microb2010/mainboard/commands_ax12.c create mode 100644 projects/microb2010/mainboard/commands_cs.c create mode 100644 projects/microb2010/mainboard/commands_gen.c create mode 100644 projects/microb2010/mainboard/commands_mainboard.c create mode 100644 projects/microb2010/mainboard/commands_traj.c create mode 100644 projects/microb2010/mainboard/cs.c create mode 100644 projects/microb2010/mainboard/cs.h create mode 100644 projects/microb2010/mainboard/diagnostic_config.h create mode 100644 projects/microb2010/mainboard/encoders_spi_config.h create mode 100644 projects/microb2010/mainboard/error_config.h create mode 100644 projects/microb2010/mainboard/i2c_config.h create mode 100644 projects/microb2010/mainboard/i2c_protocol.c create mode 100644 projects/microb2010/mainboard/i2c_protocol.h create mode 100755 projects/microb2010/mainboard/main.c create mode 100755 projects/microb2010/mainboard/main.h create mode 100644 projects/microb2010/mainboard/obstacle_avoidance_config.h create mode 100755 projects/microb2010/mainboard/pid_config.h create mode 100644 projects/microb2010/mainboard/rdline_config.h create mode 100755 projects/microb2010/mainboard/scheduler_config.h create mode 100644 projects/microb2010/mainboard/sensor.c create mode 100644 projects/microb2010/mainboard/sensor.h create mode 100644 projects/microb2010/mainboard/spi_config.h create mode 100644 projects/microb2010/mainboard/strat.c create mode 100644 projects/microb2010/mainboard/strat.h create mode 100644 projects/microb2010/mainboard/strat_avoid.c create mode 100644 projects/microb2010/mainboard/strat_avoid.h create mode 100644 projects/microb2010/mainboard/strat_base.c create mode 100644 projects/microb2010/mainboard/strat_base.h create mode 100644 projects/microb2010/mainboard/strat_building.c create mode 100644 projects/microb2010/mainboard/strat_column_disp.c create mode 100644 projects/microb2010/mainboard/strat_lintel.c create mode 100644 projects/microb2010/mainboard/strat_scan.c create mode 100644 projects/microb2010/mainboard/strat_static_columns.c create mode 100644 projects/microb2010/mainboard/strat_utils.c create mode 100644 projects/microb2010/mainboard/strat_utils.h create mode 100755 projects/microb2010/mainboard/time_config.h create mode 100755 projects/microb2010/mainboard/timer_config.h create mode 100644 projects/microb2010/mainboard/uart_config.h create mode 100644 projects/microb2010/mechboard/.config create mode 100644 projects/microb2010/mechboard/Makefile create mode 100644 projects/microb2010/mechboard/actuator.c create mode 100644 projects/microb2010/mechboard/actuator.h create mode 100644 projects/microb2010/mechboard/adc_config.h create mode 100644 projects/microb2010/mechboard/arm_highlevel.c create mode 100644 projects/microb2010/mechboard/arm_highlevel.h create mode 100644 projects/microb2010/mechboard/arm_xy.c create mode 100644 projects/microb2010/mechboard/arm_xy.h create mode 100755 projects/microb2010/mechboard/ax12_config.h create mode 100644 projects/microb2010/mechboard/ax12_user.c create mode 100644 projects/microb2010/mechboard/ax12_user.h create mode 100644 projects/microb2010/mechboard/cmdline.c create mode 100644 projects/microb2010/mechboard/cmdline.h create mode 100644 projects/microb2010/mechboard/commands.c create mode 100644 projects/microb2010/mechboard/commands_ax12.c create mode 100644 projects/microb2010/mechboard/commands_cs.c create mode 100644 projects/microb2010/mechboard/commands_gen.c create mode 100644 projects/microb2010/mechboard/commands_mechboard.c create mode 100644 projects/microb2010/mechboard/cs.c create mode 100644 projects/microb2010/mechboard/cs.h create mode 100644 projects/microb2010/mechboard/diagnostic_config.h create mode 100644 projects/microb2010/mechboard/encoders_spi_config.h create mode 100644 projects/microb2010/mechboard/error_config.h create mode 100644 projects/microb2010/mechboard/i2c_config.h create mode 100644 projects/microb2010/mechboard/i2c_protocol.c create mode 100644 projects/microb2010/mechboard/i2c_protocol.h create mode 100755 projects/microb2010/mechboard/main.c create mode 100755 projects/microb2010/mechboard/main.h create mode 100755 projects/microb2010/mechboard/pid_config.h create mode 100644 projects/microb2010/mechboard/rdline_config.h create mode 100755 projects/microb2010/mechboard/scheduler_config.h create mode 100644 projects/microb2010/mechboard/sensor.c create mode 100644 projects/microb2010/mechboard/sensor.h create mode 100644 projects/microb2010/mechboard/spi_config.h create mode 100644 projects/microb2010/mechboard/state.c create mode 100644 projects/microb2010/mechboard/state.h create mode 100755 projects/microb2010/mechboard/time_config.h create mode 100755 projects/microb2010/mechboard/timer_config.h create mode 100644 projects/microb2010/mechboard/uart_config.h create mode 100755 projects/microb2010/microb_cmd/microbcmd.py create mode 100644 projects/microb2010/sensorboard/.config create mode 100644 projects/microb2010/sensorboard/Makefile create mode 100644 projects/microb2010/sensorboard/actuator.c create mode 100644 projects/microb2010/sensorboard/actuator.h create mode 100644 projects/microb2010/sensorboard/adc_config.h create mode 100755 projects/microb2010/sensorboard/ax12_config.h create mode 100644 projects/microb2010/sensorboard/ax12_user.c create mode 100644 projects/microb2010/sensorboard/ax12_user.h create mode 100755 projects/microb2010/sensorboard/beacon.c create mode 100755 projects/microb2010/sensorboard/beacon.h create mode 100644 projects/microb2010/sensorboard/cmdline.c create mode 100644 projects/microb2010/sensorboard/cmdline.h create mode 100644 projects/microb2010/sensorboard/commands.c create mode 100644 projects/microb2010/sensorboard/commands_ax12.c create mode 100644 projects/microb2010/sensorboard/commands_cs.c create mode 100644 projects/microb2010/sensorboard/commands_gen.c create mode 100644 projects/microb2010/sensorboard/commands_scan.c create mode 100644 projects/microb2010/sensorboard/commands_sensorboard.c create mode 100644 projects/microb2010/sensorboard/cs.c create mode 100644 projects/microb2010/sensorboard/cs.h create mode 100644 projects/microb2010/sensorboard/diagnostic_config.h create mode 100644 projects/microb2010/sensorboard/encoders_spi_config.h create mode 100644 projects/microb2010/sensorboard/error_config.h create mode 100644 projects/microb2010/sensorboard/gen_scan_tab.c create mode 100644 projects/microb2010/sensorboard/i2c_config.h create mode 100644 projects/microb2010/sensorboard/i2c_protocol.c create mode 100644 projects/microb2010/sensorboard/i2c_protocol.h create mode 100644 projects/microb2010/sensorboard/img_processing.c create mode 100644 projects/microb2010/sensorboard/img_processing.h create mode 100755 projects/microb2010/sensorboard/main.c create mode 100755 projects/microb2010/sensorboard/main.h create mode 100755 projects/microb2010/sensorboard/pid_config.h create mode 100644 projects/microb2010/sensorboard/rdline_config.h create mode 100644 projects/microb2010/sensorboard/scanner.c create mode 100644 projects/microb2010/sensorboard/scanner.h create mode 100755 projects/microb2010/sensorboard/scheduler_config.h create mode 100644 projects/microb2010/sensorboard/sensor.c create mode 100644 projects/microb2010/sensorboard/sensor.h create mode 100644 projects/microb2010/sensorboard/spi_config.h create mode 100755 projects/microb2010/sensorboard/time_config.h create mode 100755 projects/microb2010/sensorboard/timer_config.h create mode 100644 projects/microb2010/sensorboard/uart_config.h create mode 100644 projects/microb2010/tests/static_beacon/.config create mode 100644 projects/microb2010/tests/static_beacon/.config.old create mode 100755 projects/microb2010/tests/static_beacon/Makefile create mode 100644 projects/microb2010/tests/static_beacon/coin.c create mode 100644 projects/microb2010/tests/static_beacon/hfuse create mode 100644 projects/microb2010/tests/static_beacon/hfuse_new create mode 100644 projects/microb2010/tests/static_beacon/lfuse create mode 100644 projects/microb2010/tests/static_beacon/lfuse_new create mode 100755 projects/microb2010/tests/static_beacon/static_beacon.bin create mode 100755 projects/microb2010/tests/static_beacon/static_beacon.c create mode 100644 projects/microb2010/tests/test_board2008/.config create mode 100644 projects/microb2010/tests/test_board2008/.config.old create mode 100755 projects/microb2010/tests/test_board2008/Makefile create mode 100644 projects/microb2010/tests/test_board2008/action.c create mode 100644 projects/microb2010/tests/test_board2008/action.h create mode 100644 projects/microb2010/tests/test_board2008/actuator.c create mode 100644 projects/microb2010/tests/test_board2008/actuator.h create mode 100644 projects/microb2010/tests/test_board2008/adc_config.h create mode 100644 projects/microb2010/tests/test_board2008/cmdline.c create mode 100644 projects/microb2010/tests/test_board2008/cmdline.h create mode 100644 projects/microb2010/tests/test_board2008/commands.c create mode 100644 projects/microb2010/tests/test_board2008/commands_ax12.c create mode 100644 projects/microb2010/tests/test_board2008/commands_cs.c create mode 100644 projects/microb2010/tests/test_board2008/commands_gen.c create mode 100644 projects/microb2010/tests/test_board2008/commands_mainboard.c create mode 100644 projects/microb2010/tests/test_board2008/commands_traj.c create mode 100644 projects/microb2010/tests/test_board2008/cs.c create mode 100644 projects/microb2010/tests/test_board2008/cs.h create mode 100644 projects/microb2010/tests/test_board2008/diagnostic_config.h create mode 100644 projects/microb2010/tests/test_board2008/encoders_microb_config.h create mode 100644 projects/microb2010/tests/test_board2008/encoders_spi_config.h create mode 100644 projects/microb2010/tests/test_board2008/error_config.h create mode 100644 projects/microb2010/tests/test_board2008/main. create mode 100755 projects/microb2010/tests/test_board2008/main.bin create mode 100755 projects/microb2010/tests/test_board2008/main.c create mode 100755 projects/microb2010/tests/test_board2008/main.h create mode 100755 projects/microb2010/tests/test_board2008/pid_config.h create mode 100644 projects/microb2010/tests/test_board2008/rdline_config.h create mode 100755 projects/microb2010/tests/test_board2008/scheduler_config.h create mode 100644 projects/microb2010/tests/test_board2008/sensor.c create mode 100644 projects/microb2010/tests/test_board2008/sensor.h create mode 100644 projects/microb2010/tests/test_board2008/spi_config.h create mode 100644 projects/microb2010/tests/test_board2008/strat_base.c create mode 100644 projects/microb2010/tests/test_board2008/strat_base.h create mode 100644 projects/microb2010/tests/test_board2008/strat_utils.c create mode 100644 projects/microb2010/tests/test_board2008/strat_utils.h create mode 100755 projects/microb2010/tests/test_board2008/time_config.h create mode 100755 projects/microb2010/tests/test_board2008/timer_config.h create mode 100644 projects/microb2010/tests/test_board2008/uart_config.h diff --git a/projects/microb2010/bootloader/.config b/projects/microb2010/bootloader/.config new file mode 100644 index 0000000..b70ba89 --- /dev/null +++ b/projects/microb2010/bootloader/.config @@ -0,0 +1,278 @@ +# +# Automatically generated make config: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +# CONFIG_MCU_ATMEGA8 is not set +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +# CONFIG_MCU_ATMEGA128 is not set +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +CONFIG_MCU_ATMEGA2560=y +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +# CONFIG_MATH_LIB is not set +# CONFIG_FDEVOPEN_COMPAT is not set +CONFIG_NO_PRINTF=y +# CONFIG_MINIMAL_PRINTF is not set +# CONFIG_STANDARD_PRINTF is not set +CONFIG_FORMAT_IHEX=y +# CONFIG_FORMAT_SREC is not set +# CONFIG_FORMAT_BINARY is not set + +# +# Base modules +# + +# +# Enable math library in generation options to see all modules +# +# CONFIG_MODULE_CIRBUF is not set +# CONFIG_MODULE_CIRBUF_LARGE is not set +# CONFIG_MODULE_FIXED_POINT is not set +# CONFIG_MODULE_VECT2 is not set +# CONFIG_MODULE_GEOMETRY is not set +# CONFIG_MODULE_GEOMETRY_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER is not set +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +CONFIG_MODULE_SCHEDULER_TIMER0=y +# CONFIG_MODULE_SCHEDULER_MANUAL is not set +# CONFIG_MODULE_TIME is not set +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# + +# +# uart needs circular buffer, mf2 client may need scheduler +# +# CONFIG_MODULE_UART is not set +# CONFIG_MODULE_UART_9BITS is not set +# CONFIG_MODULE_UART_CREATE_CONFIG is not set +# CONFIG_MODULE_SPI is not set +# CONFIG_MODULE_SPI_CREATE_CONFIG is not set +# CONFIG_MODULE_I2C is not set +# CONFIG_MODULE_I2C_MASTER is not set +# CONFIG_MODULE_I2C_MULTIMASTER is not set +# CONFIG_MODULE_I2C_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +# CONFIG_MODULE_TIMER is not set +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +# CONFIG_MODULE_PWM_NG is not set +# CONFIG_MODULE_ADC is not set +# CONFIG_MODULE_ADC_CREATE_CONFIG is not set + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +# CONFIG_MODULE_VT100 is not set +# CONFIG_MODULE_RDLINE is not set +# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set +# CONFIG_MODULE_RDLINE_KILL_BUF is not set +# CONFIG_MODULE_RDLINE_HISTORY is not set +# CONFIG_MODULE_PARSE is not set +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +# CONFIG_MODULE_AX12 is not set +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +# CONFIG_MODULE_ENCODERS_MICROB is not set +# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_SPI is not set +# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set + +# +# Robot specific modules +# +# CONFIG_MODULE_ROBOT_SYSTEM is not set +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +# CONFIG_MODULE_POSITION_MANAGER is not set +# CONFIG_MODULE_TRAJECTORY_MANAGER is not set +# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set + +# +# Control system modules +# +# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set + +# +# Filters +# +# CONFIG_MODULE_PID is not set +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +# CONFIG_MODULE_QUADRAMP is not set +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# + +# +# Some radio devices require SPI to be activated +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# + +# +# Crypto modules depend on utils module +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# + +# +# Encoding modules depend on utils module +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# + +# +# Debug modules depend on utils module +# +# CONFIG_MODULE_DIAGNOSTIC is not set +# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set +# CONFIG_MODULE_ERROR is not set +# CONFIG_MODULE_ERROR_CREATE_CONFIG is not set + +# +# Programmer options +# +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/bootloader/Makefile b/projects/microb2010/bootloader/Makefile new file mode 100755 index 0000000..04c078e --- /dev/null +++ b/projects/microb2010/bootloader/Makefile @@ -0,0 +1,41 @@ +TARGET = main + +# repertoire des modules +AVERSIVE_DIR = ../../.. +# VALUE, absolute or relative path : example ../.. # + +CFLAGS += -Werror + +# atm128 +# address is 0xf000 (in words) +# LDFLAGS += -Wl,--section-start=.text=1e000 +# UART_NUM = 0 + +# atm2560 +# address is 0x1f800 (in words) +LDFLAGS += -Wl,--section-start=.text=3f000 +UART_NUM = 1 + +CFLAGS += -DUART_NUM=$(UART_NUM) + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + +######################################## + +-include .aversive_conf +include $(AVERSIVE_DIR)/mk/aversive_project.mk + +program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep + echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + diff --git a/projects/microb2010/bootloader/main.c b/projects/microb2010/bootloader/main.c new file mode 100755 index 0000000..8595d72 --- /dev/null +++ b/projects/microb2010/bootloader/main.c @@ -0,0 +1,361 @@ +/* + * 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 + * (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: main.c,v 1.4 2009-05-27 20:04:06 zer0 Exp $ + * + */ + +/* + * A simple bootloader example. + */ + +#include +#include +#include + +#include +#include +#include +#include + +#define BRAKE_DDR() do { DDRJ |= 0xF0; } while(0) +#define BRAKE_ON() do { PORTJ |= 0xF0; } while(0) +#define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0) + +#define LED1_ON() sbi(PORTJ, 2) +#define LED1_OFF() cbi(PORTJ, 2) + +#define LED2_ON() sbi(PORTJ, 3) +#define LED2_OFF() cbi(PORTJ, 3) + + +#define NOECHO + +#ifdef NOECHO +#define echo(c) do {} while(0) +#else +#define echo(c) uart_send(c) +#endif + +#if UART_NUM == 0 + +#define UCSRxA UCSR0A +#define UCSRxB UCSR0B +#define UCSRxC UCSR0C +#define RXCx RXC0 +#define UDRx UDR0 +#define UDREx UDRE0 +#define U2Xx U2X0 +#define RXENx RXEN0 +#define TXENx TXEN0 +#define UCSZx0 UCSZ00 +#define UCSZx1 UCSZ01 +#define UBRRx UBRR0 + +#elif UART_NUM == 1 + +#define UCSRxA UCSR1A +#define UCSRxB UCSR1B +#define UCSRxC UCSR1C +#define RXCx RXC1 +#define UDRx UDR1 +#define UDREx UDRE1 +#define U2Xx U2X1 +#define RXENx RXEN1 +#define TXENx TXEN1 +#define UCSZx0 UCSZ10 +#define UCSZx1 UCSZ11 +#define UBRRx UBRR1 + +#elif UART_NUM == 2 + +#define UCSRxA UCSR2A +#define UCSRxB UCSR2B +#define UCSRxC UCSR2C +#define RXCx RXC2 +#define UDRx UDR2 +#define UDREx UDRE2 +#define U2Xx U2X2 +#define RXENx RXEN2 +#define TXENx TXEN2 +#define UCSZx0 UCSZ20 +#define UCSZx1 UCSZ21 +#define UBRRx UBRR2 + +#elif UART_NUM == 3 + +#define UCSRxA UCSR3A +#define UCSRxB UCSR3B +#define UCSRxC UCSR3C +#define RXCx RXC3 +#define UDRx UDR3 +#define UDREx UDRE3 +#define U2Xx U2X3 +#define RXENx RXEN3 +#define TXENx TXEN3 +#define UCSZx0 UCSZ30 +#define UCSZx1 UCSZ31 +#define UBRRx UBRR3 + +#endif + + +static char uart_recv(void) +{ + while ( !(UCSRxA & (1<= '0' && c <= '9') { + tmp <<= 4; + tmp += (c - '0'); + } + else if (c >= 'a' && c <= 'f') { + tmp <<= 4; + tmp += (c - 'a' + 10); + } + else if (c >= 'A' && c <= 'F') { + tmp <<= 4; + tmp += (c - 'A' + 10); + } + else + return -1; + } + return 0; +} + +/* launch application */ +static void launch_app(void) +{ + uart_puts("Boot..."); + MCUCR = (1 << IVCE); + MCUCR = (0 << IVSEL); + reset(); +} + +static void disp_digit(uint8_t x) +{ + if (x < 10) + x += '0'; + else + x += 'a' - 10 ; + uart_send(x); +} + +static void disp_hex8(uint8_t x) +{ + disp_digit(x>>4); + disp_digit(x&0xf); +} + +static void disp_hex16(uint16_t x) +{ + disp_hex8(x>>8); + disp_hex8(x); +} + +static void crc_app(void) +{ + uint32_t start_addr, addr, size; + uint8_t c; + uint16_t crc = 0xffff; + uint16_t sum = 0; + + uart_puts("addr?\r\n"); + if (bootloader_query_hex(&start_addr)) + goto fail; + if (start_addr > FLASHEND) + goto fail; + uart_puts("size?\r\n"); + if (bootloader_query_hex(&size)) + goto fail; + if (start_addr + size > FLASHEND) + goto fail; + for (addr=start_addr; addr= 256 && addr < 512) + continue; +#endif + c = pgm_read_byte_far(addr); + crc = _crc_ccitt_update(crc, c); + sum += c; + } + disp_hex16(crc); + disp_hex16(sum); + return; + fail: + uart_puts("KO"); +} + +static void read32(void) +{ + uint32_t start_addr, val = 0; + uint8_t c, i; + + uart_puts("addr?\r\n"); + if (bootloader_query_hex(&start_addr)) + goto fail; + if (start_addr > FLASHEND) + goto fail; + for (i=0; i<4; i++) { + c = pgm_read_byte_far(start_addr+i); + val <<= 8; + val |= c; + } + disp_hex16(val); + return; + fail: + uart_puts("KO"); +} + +static void prog_page(void) +{ + int c; + uint32_t addr; + uint16_t i; + uint16_t crc = 0xffff; + uint16_t sum = 0; + uint8_t buf[SPM_PAGESIZE]; + +#define SPM_PAGEMASK ((uint32_t)SPM_PAGESIZE-1) + uart_puts("addr?\r\n"); + if (bootloader_query_hex(&addr)) + goto fail; + if (addr > FLASHEND) + goto fail; + /* start_addr must be page aligned */ + if (addr & SPM_PAGEMASK) + goto fail; + + uart_puts("ok\r\n"); + + PORTJ = 0xF0; + + /* data is received like the .bin format (which is already in + * little endian) */ + for (i=0; i "); + + /* timeout */ + while ( !(UCSRxA & (1<1000000) /* wait about 1 sec */ + launch_app(); + } + + while (1) { + uart_puts("\r\ncmd> "); + c = uart_recv(); + if (c == 'x') + launch_app(); + else if (c == 'c') + crc_app(); + else if (c == 'p') + prog_page(); + else if (c == 'd') + read32(); + else + uart_puts("p:prog_page c:crc x:exec d:dump32"); + } + + return 0; +} diff --git a/projects/microb2010/bootloader/uart_config.h b/projects/microb2010/bootloader/uart_config.h new file mode 100755 index 0000000..506806b --- /dev/null +++ b/projects/microb2010/bootloader/uart_config.h @@ -0,0 +1,120 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: uart_config.h,v 1.3 2009-05-27 20:04:06 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +#if UART_NUM == 0 + +/* compile uart0 fonctions, undefine it to pass compilation */ +#define UART0_COMPILE + +/* enable uart0 if == 1, disable if == 0 */ +#define UART0_ENABLED 1 + +/* enable uart0 interrupts if == 1, disable if == 0 */ +#define UART0_INTERRUPT_ENABLED 1 + +#define UART0_BAUDRATE 57600 + +#define UART0_USE_DOUBLE_SPEED 0 + +#define UART0_RX_FIFO_SIZE 64 +#define UART0_TX_FIFO_SIZE 16 + +#define UART0_NBITS 8 +#define UART0_PARITY UART_PARTITY_NONE +#define UART0_STOP_BIT UART_STOP_BITS_1 + +#elif UART_NUM == 1 + +/* compile uart1 fonctions, undefine it to pass compilation */ +#define UART1_COMPILE + +/* enable uart1 if == 1, disable if == 0 */ +#define UART1_ENABLED 1 + +/* enable uart1 interrupts if == 1, disable if == 0 */ +#define UART1_INTERRUPT_ENABLED 1 + +#define UART1_BAUDRATE 57600 + +#define UART1_USE_DOUBLE_SPEED 0 + +#define UART1_RX_FIFO_SIZE 32 +#define UART1_TX_FIFO_SIZE 4 + +#define UART1_NBITS 8 +#define UART1_PARITY UART_PARTITY_NONE +#define UART1_STOP_BIT UART_STOP_BITS_1 + +#elif UART_NUM == 2 + +/* compile uart2 fonctions, undefine it to pass compilation */ +#define UART2_COMPILE + +/* enable uart2 if == 1, disable if == 0 */ +#define UART2_ENABLED 1 + +/* enable uart2 interrupts if == 1, disable if == 0 */ +#define UART2_INTERRUPT_ENABLED 1 + +#define UART2_BAUDRATE 57600 + +#define UART2_USE_DOUBLE_SPEED 0 + +#define UART2_RX_FIFO_SIZE 32 +#define UART2_TX_FIFO_SIZE 4 + +#define UART2_NBITS 8 +#define UART2_PARITY UART_PARTITY_NONE +#define UART2_STOP_BIT UART_STOP_BITS_1 + +#elif UART_NUM == 3 + +/* compile uart3 fonctions, undefine it to pass compilation */ +#define UART3_COMPILE + +/* enable uart3 if == 1, disable if == 0 */ +#define UART3_ENABLED 1 + +/* enable uart3 interrupts if == 1, disable if == 0 */ +#define UART3_INTERRUPT_ENABLED 1 + +#define UART3_BAUDRATE 57600 + +#define UART3_USE_DOUBLE_SPEED 0 + +#define UART3_RX_FIFO_SIZE 32 +#define UART3_TX_FIFO_SIZE 4 + +#define UART3_NBITS 8 +#define UART3_PARITY UART_PARTITY_NONE +#define UART3_STOP_BIT UART_STOP_BITS_1 + +#endif /* uart num */ + +#endif + diff --git a/projects/microb2010/common/avr6.x b/projects/microb2010/common/avr6.x new file mode 100644 index 0000000..a758e96 --- /dev/null +++ b/projects/microb2010/common/avr6.x @@ -0,0 +1,269 @@ +/* Default linker script, for normal executables */ +OUTPUT_FORMAT("elf32-avr","elf32-avr","elf32-avr") +OUTPUT_ARCH(avr:6) +MEMORY +{ + text (rx) : ORIGIN = 0, LENGTH = 1024K + data (rw!x) : ORIGIN = 0x800200, LENGTH = 0xfe00 + eeprom (rw!x) : ORIGIN = 0x810000, LENGTH = 64K + fuse (rw!x) : ORIGIN = 0x820000, LENGTH = 1K + lock (rw!x) : ORIGIN = 0x830000, LENGTH = 1K + signature (rw!x) : ORIGIN = 0x840000, LENGTH = 1K +} +SECTIONS +{ + /* Read-only sections, merged into text segment: */ + .hash : { *(.hash) } + .dynsym : { *(.dynsym) } + .dynstr : { *(.dynstr) } + .gnu.version : { *(.gnu.version) } + .gnu.version_d : { *(.gnu.version_d) } + .gnu.version_r : { *(.gnu.version_r) } + .rel.init : { *(.rel.init) } + .rela.init : { *(.rela.init) } + .rel.text : + { + *(.rel.text) + *(.rel.text.*) + *(.rel.gnu.linkonce.t*) + } + .rela.text : + { + *(.rela.text) + *(.rela.text.*) + *(.rela.gnu.linkonce.t*) + } + .rel.fini : { *(.rel.fini) } + .rela.fini : { *(.rela.fini) } + .rel.rodata : + { + *(.rel.rodata) + *(.rel.rodata.*) + *(.rel.gnu.linkonce.r*) + } + .rela.rodata : + { + *(.rela.rodata) + *(.rela.rodata.*) + *(.rela.gnu.linkonce.r*) + } + .rel.data : + { + *(.rel.data) + *(.rel.data.*) + *(.rel.gnu.linkonce.d*) + } + .rela.data : + { + *(.rela.data) + *(.rela.data.*) + *(.rela.gnu.linkonce.d*) + } + .rel.ctors : { *(.rel.ctors) } + .rela.ctors : { *(.rela.ctors) } + .rel.dtors : { *(.rel.dtors) } + .rela.dtors : { *(.rela.dtors) } + .rel.got : { *(.rel.got) } + .rela.got : { *(.rela.got) } + .rel.bss : { *(.rel.bss) } + .rela.bss : { *(.rela.bss) } + .rel.plt : { *(.rel.plt) } + .rela.plt : { *(.rela.plt) } + /* Internal text space or external memory. */ + .text : + { + *(.vectors) + KEEP(*(.vectors)) + . = 256 + ALIGN(256); /* placeholder for misc microb infos */ + /* For data that needs to reside in the lower 64k of progmem. */ + *(.progmem.gcc*) + *(.progmem*) + . = ALIGN(2048); + __trampolines_start = . ; + /* The jump trampolines for the 16-bit limited relocs will reside here. */ + *(.trampolines) + *(.trampolines*) + __trampolines_end = . ; + /* For future tablejump instruction arrays for 3 byte pc devices. + We don't relax jump/call instructions within these sections. */ + *(.jumptables) + *(.jumptables*) + /* For code that needs to reside in the lower 128k progmem. */ + *(.lowtext) + *(.lowtext*) + __ctors_start = . ; + *(.ctors) + __ctors_end = . ; + __dtors_start = . ; + *(.dtors) + __dtors_end = . ; + KEEP(SORT(*)(.ctors)) + KEEP(SORT(*)(.dtors)) + /* From this point on, we don't bother about wether the insns are + below or above the 16 bits boundary. */ + *(.init0) /* Start here after reset. */ + KEEP (*(.init0)) + *(.init1) + KEEP (*(.init1)) + *(.init2) /* Clear __zero_reg__, set up stack pointer. */ + KEEP (*(.init2)) + *(.init3) + KEEP (*(.init3)) + *(.init4) /* Initialize data and BSS. */ + KEEP (*(.init4)) + *(.init5) + KEEP (*(.init5)) + *(.init6) /* C++ constructors. */ + KEEP (*(.init6)) + *(.init7) + KEEP (*(.init7)) + *(.init8) + KEEP (*(.init8)) + *(.init9) /* Call main(). */ + KEEP (*(.init9)) + *(.text.*) /* trucs de gcc ? */ + . = ALIGN(2048); + uart*(.text) + pwm*(.text) + parse*(.text) + rdline*(.text) + vt100*(.text) + scheduler*(.text) + control_system*(.text) + pid*(.text) + f08*(.text) + f16*(.text) + f32*(.text) + f64*(.text) + vect2*(.text) + quadramp*(.text) + blocking*(.text) + obstacle*(.text) + trajectory*(.text) + position*(.text) + adc*(.text) + robot*(.text) + error*(.text) + encoders*(.text) + time*(.text) + cirbuf*(.text) + i2c*(.text) + spi*(.text) + ax12*(.text) + . = ALIGN(2048); + /* some libc stuff */ + str*(.text) + mem*(.text) + printf*(.text) + vfprintf*(.text) + sprintf*(.text) + snprintf*(.text) + malloc*(.text) + free*(.text) + fdevopen*(.text) + fputc*(.text) + . = ALIGN(2048); + *(.text) + . = ALIGN(2); + *(.fini9) /* _exit() starts here. */ + KEEP (*(.fini9)) + *(.fini8) + KEEP (*(.fini8)) + *(.fini7) + KEEP (*(.fini7)) + *(.fini6) /* C++ destructors. */ + KEEP (*(.fini6)) + *(.fini5) + KEEP (*(.fini5)) + *(.fini4) + KEEP (*(.fini4)) + *(.fini3) + KEEP (*(.fini3)) + *(.fini2) + KEEP (*(.fini2)) + *(.fini1) + KEEP (*(.fini1)) + *(.fini0) /* Infinite loop after program termination. */ + KEEP (*(.fini0)) + _etext = . ; + } > text + .data : AT (ADDR (.text) + SIZEOF (.text)) + { + PROVIDE (__data_start = .) ; + *(.data) + *(.data*) + *(.rodata) /* We need to include .rodata here if gcc is used */ + *(.rodata*) /* with -fdata-sections. */ + *(.gnu.linkonce.d*) + . = ALIGN(2); + _edata = . ; + PROVIDE (__data_end = .) ; + } > data + .bss SIZEOF(.data) + ADDR(.data) : + { + PROVIDE (__bss_start = .) ; + *(.bss) + *(.bss*) + *(COMMON) + PROVIDE (__bss_end = .) ; + } > data + __data_load_start = LOADADDR(.data); + __data_load_end = __data_load_start + SIZEOF(.data); + /* Global data not cleared after reset. */ + .noinit SIZEOF(.bss) + ADDR(.bss) : + { + PROVIDE (__noinit_start = .) ; + *(.noinit*) + PROVIDE (__noinit_end = .) ; + _end = . ; + PROVIDE (__heap_start = .) ; + } > data + .eeprom : + { + *(.eeprom*) + __eeprom_end = . ; + } > eeprom + .fuse : + { + KEEP(*(.fuse)) + KEEP(*(.lfuse)) + KEEP(*(.hfuse)) + KEEP(*(.efuse)) + } > fuse + .lock : + { + KEEP(*(.lock*)) + } > lock + .signature : + { + KEEP(*(.signature*)) + } > signature + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + /* DWARF debug sections. + Symbols in the DWARF debugging sections are relative to the beginning + of the section so we begin them at 0. */ + /* DWARF 1 */ + .debug 0 : { *(.debug) } + .line 0 : { *(.line) } + /* GNU DWARF 1 extensions */ + .debug_srcinfo 0 : { *(.debug_srcinfo) } + .debug_sfnames 0 : { *(.debug_sfnames) } + /* DWARF 1.1 and DWARF 2 */ + .debug_aranges 0 : { *(.debug_aranges) } + .debug_pubnames 0 : { *(.debug_pubnames) } + /* DWARF 2 */ + .debug_info 0 : { *(.debug_info) *(.gnu.linkonce.wi.*) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_line 0 : { *(.debug_line) } + .debug_frame 0 : { *(.debug_frame) } + .debug_str 0 : { *(.debug_str) } + .debug_loc 0 : { *(.debug_loc) } + .debug_macinfo 0 : { *(.debug_macinfo) } +} diff --git a/projects/microb2010/common/eeprom_mapping.h b/projects/microb2010/common/eeprom_mapping.h new file mode 100644 index 0000000..01bd6e3 --- /dev/null +++ b/projects/microb2010/common/eeprom_mapping.h @@ -0,0 +1,33 @@ +/* + * Copyright Droids Corporation (2007) + * + * 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: eeprom_mapping.h,v 1.2 2009-05-27 20:04:06 zer0 Exp $ + * + */ + +#ifndef _EEPROM_MAPPING_H_ +#define _EEPROM_MAPPING_H_ + +#define EEPROM_MAGIC_MAINBOARD 1 +#define EEPROM_MAGIC_MECHBOARD 2 +#define EEPROM_MAGIC_SENSORBOARD 3 + +#define EEPROM_MAGIC_ADDRESS ((uint8_t *)0x100) + +#define EEPROM_TIME_ADDRESS ((uint16_t *)0x110) + +#endif diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h new file mode 100644 index 0000000..fdb5d49 --- /dev/null +++ b/projects/microb2010/common/i2c_commands.h @@ -0,0 +1,295 @@ +/* + * Copyright Droids Corporation (2007) + * + * 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: i2c_commands.h,v 1.9 2009-05-27 20:04:06 zer0 Exp $ + * + */ + +#ifndef _I2C_COMMANDS_H_ +#define _I2C_COMMANDS_H_ + +#define I2C_MAINBOARD_ADDR 1 +#define I2C_MECHBOARD_ADDR 2 +#define I2C_SENSORBOARD_ADDR 3 + +#define I2C_LEFT_SIDE 0 +#define I2C_RIGHT_SIDE 1 +#define I2C_AUTO_SIDE 2 /* for prepare_pickup */ +#define I2C_CENTER_SIDE 3 /* for prepare_pickup */ + +#define I2C_COLOR_RED 0 +#define I2C_COLOR_GREEN 1 + +#define I2C_AUTOBUILD_DEFAULT_DIST 210 + +struct i2c_cmd_hdr { + uint8_t cmd; +}; + +/****/ +/* commands that do not need and answer */ +/****/ + +#define I2C_CMD_GENERIC_LED_CONTROL 0x00 + +struct i2c_cmd_led_control { + struct i2c_cmd_hdr hdr; + uint8_t led_num:7; + uint8_t state:1; +}; + +/****/ + +#define I2C_CMD_GENERIC_SET_COLOR 0x01 + +struct i2c_cmd_generic_color { + struct i2c_cmd_hdr hdr; + uint8_t color; +}; + +/****/ + +#define I2C_CMD_MECHBOARD_SET_MODE 0x02 + +struct i2c_cmd_mechboard_set_mode { + struct i2c_cmd_hdr hdr; +#define I2C_MECHBOARD_MODE_MANUAL 0x00 +#define I2C_MECHBOARD_MODE_HARVEST 0x01 +#define I2C_MECHBOARD_MODE_PREPARE_PICKUP 0x02 +#define I2C_MECHBOARD_MODE_PICKUP 0x03 +#define I2C_MECHBOARD_MODE_PREPARE_BUILD 0x04 +#define I2C_MECHBOARD_MODE_AUTOBUILD 0x05 +#define I2C_MECHBOARD_MODE_WAIT 0x06 +#define I2C_MECHBOARD_MODE_INIT 0x07 +#define I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL 0x08 +#define I2C_MECHBOARD_MODE_GET_LINTEL 0x09 +#define I2C_MECHBOARD_MODE_PUT_LINTEL 0x0A +#define I2C_MECHBOARD_MODE_PREPARE_EJECT 0x0B +#define I2C_MECHBOARD_MODE_EJECT 0x0C +#define I2C_MECHBOARD_MODE_CLEAR 0x0D +#define I2C_MECHBOARD_MODE_LAZY_HARVEST 0x0E +#define I2C_MECHBOARD_MODE_LOADED 0x0F +#define I2C_MECHBOARD_MODE_PREPARE_INSIDE 0x10 +#define I2C_MECHBOARD_MODE_STORE 0x11 +#define I2C_MECHBOARD_MODE_LAZY_PICKUP 0x12 +#define I2C_MECHBOARD_MODE_MANIVELLE 0x13 +#define I2C_MECHBOARD_MODE_PUSH_TEMPLE 0x14 +#define I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC 0x15 +#define I2C_MECHBOARD_MODE_EXIT 0xFF + uint8_t mode; + union { + struct { + + } manual; + + struct { + uint8_t side; + uint8_t next_mode; + } prep_pickup; + + struct { + uint8_t level_l; + uint8_t level_r; + } prep_build; + + struct { + uint8_t side; + } push_temple_disc; + + struct { + uint8_t level; + } push_temple; + + struct { + uint8_t level_left; + uint8_t level_right; + uint8_t count_left; + uint8_t count_right; + uint8_t distance_left; + uint8_t distance_right; + uint8_t do_lintel; + } autobuild; + + struct { + uint8_t level_l; + uint8_t level_r; + } prep_inside; + }; +}; + +/****/ + +/* only valid in manual mode */ +#define I2C_CMD_MECHBOARD_ARM_GOTO 0x03 + +struct i2c_cmd_mechboard_arm_goto { + struct i2c_cmd_hdr hdr; +#define I2C_MECHBOARD_ARM_LEFT 0 +#define I2C_MECHBOARD_ARM_RIGHT 1 +#define I2C_MECHBOARD_ARM_BOTH 2 + uint8_t which; + + uint8_t height; /* in cm */ + uint8_t distance; /* in cm */ +}; + +/****/ + +#define I2C_CMD_SENSORBOARD_SET_BEACON 0x04 + +struct i2c_cmd_sensorboard_start_beacon { + struct i2c_cmd_hdr hdr; + uint8_t enable; +}; + + +/****/ + +#define I2C_CMD_SENSORBOARD_SET_SCANNER 0x05 + +struct i2c_cmd_sensorboard_scanner { + struct i2c_cmd_hdr hdr; + +#define I2C_SENSORBOARD_SCANNER_STOP 0x00 +#define I2C_SENSORBOARD_SCANNER_PREPARE 0x01 +#define I2C_SENSORBOARD_SCANNER_START 0x02 + uint8_t mode; +}; + +/*****/ + +#define I2C_CMD_SENSORBOARD_CALIB_SCANNER 0x06 +struct i2c_cmd_sensorboard_calib_scanner { + struct i2c_cmd_hdr hdr; +}; + +/*****/ + +#define I2C_CMD_SENSORBOARD_SCANNER_ALGO 0x07 +struct i2c_cmd_sensorboard_scanner_algo { + struct i2c_cmd_hdr hdr; + +#define I2C_SCANNER_ALGO_COLUMN_DROPZONE 1 +#define I2C_SCANNER_ALGO_CHECK_TEMPLE 2 +#define I2C_SCANNER_ALGO_TEMPLE_DROPZONE 3 + uint8_t algo; + + union { + struct { +#define I2C_SCANNER_ZONE_0 0 +#define I2C_SCANNER_ZONE_1 1 +#define I2C_SCANNER_ZONE_DISC 2 + uint8_t working_zone; + int16_t center_x; + int16_t center_y; + } drop_zone; + + struct { + uint8_t level; + int16_t temple_x; + int16_t temple_y; + } check_temple; + }; +}; + +/****/ +/* requests and their answers */ +/****/ + + +#define I2C_REQ_MECHBOARD_STATUS 0x80 + +struct i2c_req_mechboard_status { + struct i2c_cmd_hdr hdr; + + int16_t pump_left1_current; + int16_t pump_left2_current; +}; + +#define I2C_ANS_MECHBOARD_STATUS 0x81 + +struct i2c_ans_mechboard_status { + struct i2c_cmd_hdr hdr; + /* mode type are defined above: I2C_MECHBOARD_MODE_xxx */ + uint8_t mode; + +#define I2C_MECHBOARD_STATUS_F_READY 0x00 +#define I2C_MECHBOARD_STATUS_F_BUSY 0x01 +#define I2C_MECHBOARD_STATUS_F_EXCPT 0x02 + uint8_t status; + + uint8_t lintel_count; + + /* flag is there if column was taken by this pump. Note that + * we should also check ADC (current) to see if the column is + * still there. */ +#define I2C_MECHBOARD_COLUMN_L1 0x01 +#define I2C_MECHBOARD_COLUMN_L2 0x02 +#define I2C_MECHBOARD_COLUMN_R1 0x04 +#define I2C_MECHBOARD_COLUMN_R2 0x08 + uint8_t column_flags; + + int16_t pump_left1; + int16_t pump_right1; + int16_t pump_left2; + int16_t pump_right2; + +#define I2C_MECHBOARD_CURRENT_COLUMN 85 + int16_t pump_right1_current; + int16_t pump_right2_current; + + uint16_t servo_lintel_left; + uint16_t servo_lintel_right; +}; + +#define I2C_REQ_SENSORBOARD_STATUS 0x82 + +struct i2c_req_sensorboard_status { + struct i2c_cmd_hdr hdr; + + /* position sent by mainboard */ + int16_t x; + int16_t y; + int16_t a; + + /* PWM for pickup */ + uint8_t enable_pickup_wheels; +}; + +#define I2C_ANS_SENSORBOARD_STATUS 0x83 + +struct i2c_ans_sensorboard_status { + struct i2c_cmd_hdr hdr; + + uint8_t status; +#define I2C_OPPONENT_NOT_THERE -1000 + int16_t opponent_x; + int16_t opponent_y; + int16_t opponent_a; + int16_t opponent_d; + +#define I2C_SCAN_DONE 1 +#define I2C_SCAN_MAX_COLUMN 2 + uint8_t scan_status; + +#define I2C_COLUMN_NO_DROPZONE -1 + int8_t dropzone_h; + int16_t dropzone_x; + int16_t dropzone_y; +}; + +#endif /* _I2C_PROTOCOL_H_ */ diff --git a/projects/microb2010/common/i2c_commands.h.~1.9.~ b/projects/microb2010/common/i2c_commands.h.~1.9.~ new file mode 100644 index 0000000..d3072bc --- /dev/null +++ b/projects/microb2010/common/i2c_commands.h.~1.9.~ @@ -0,0 +1,291 @@ +/* + * Copyright Droids Corporation (2007) + * + * 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: i2c_commands.h,v 1.9 2009-05-27 20:04:06 zer0 Exp $ + * + */ + +#ifndef _I2C_COMMANDS_H_ +#define _I2C_COMMANDS_H_ + +#define I2C_MAINBOARD_ADDR 1 +#define I2C_MECHBOARD_ADDR 2 +#define I2C_SENSORBOARD_ADDR 3 + +#define I2C_LEFT_SIDE 0 +#define I2C_RIGHT_SIDE 1 +#define I2C_AUTO_SIDE 2 /* for prepare_pickup */ +#define I2C_CENTER_SIDE 3 /* for prepare_pickup */ + +#define I2C_COLOR_RED 0 +#define I2C_COLOR_GREEN 1 + +#define I2C_AUTOBUILD_DEFAULT_DIST 210 + +struct i2c_cmd_hdr { + uint8_t cmd; +}; + +/****/ +/* commands that do not need and answer */ +/****/ + +#define I2C_CMD_GENERIC_LED_CONTROL 0x00 + +struct i2c_cmd_led_control { + struct i2c_cmd_hdr hdr; + uint8_t led_num:7; + uint8_t state:1; +}; + +/****/ + +#define I2C_CMD_GENERIC_SET_COLOR 0x01 + +struct i2c_cmd_generic_color { + struct i2c_cmd_hdr hdr; + uint8_t color; +}; + +/****/ + +#define I2C_CMD_MECHBOARD_SET_MODE 0x02 + +struct i2c_cmd_mechboard_set_mode { + struct i2c_cmd_hdr hdr; +#define I2C_MECHBOARD_MODE_MANUAL 0x00 +#define I2C_MECHBOARD_MODE_HARVEST 0x01 +#define I2C_MECHBOARD_MODE_PREPARE_PICKUP 0x02 +#define I2C_MECHBOARD_MODE_PICKUP 0x03 +#define I2C_MECHBOARD_MODE_PREPARE_BUILD 0x04 +#define I2C_MECHBOARD_MODE_AUTOBUILD 0x05 +#define I2C_MECHBOARD_MODE_WAIT 0x06 +#define I2C_MECHBOARD_MODE_INIT 0x07 +#define I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL 0x08 +#define I2C_MECHBOARD_MODE_GET_LINTEL 0x09 +#define I2C_MECHBOARD_MODE_PUT_LINTEL 0x0A +#define I2C_MECHBOARD_MODE_PREPARE_EJECT 0x0B +#define I2C_MECHBOARD_MODE_EJECT 0x0C +#define I2C_MECHBOARD_MODE_CLEAR 0x0D +#define I2C_MECHBOARD_MODE_LAZY_HARVEST 0x0E +#define I2C_MECHBOARD_MODE_LOADED 0x0F +#define I2C_MECHBOARD_MODE_PREPARE_INSIDE 0x10 +#define I2C_MECHBOARD_MODE_STORE 0x11 +#define I2C_MECHBOARD_MODE_LAZY_PICKUP 0x12 +#define I2C_MECHBOARD_MODE_MANIVELLE 0x13 +#define I2C_MECHBOARD_MODE_PUSH_TEMPLE 0x14 +#define I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC 0x15 +#define I2C_MECHBOARD_MODE_EXIT 0xFF + uint8_t mode; + union { + struct { + + } manual; + + struct { + uint8_t side; + uint8_t next_mode; + } prep_pickup; + + struct { + uint8_t level_l; + uint8_t level_r; + } prep_build; + + struct { + uint8_t side; + } push_temple_disc; + + struct { + uint8_t level_left; + uint8_t level_right; + uint8_t count_left; + uint8_t count_right; + uint8_t distance_left; + uint8_t distance_right; + uint8_t do_lintel; + } autobuild; + + struct { + uint8_t level_l; + uint8_t level_r; + } prep_inside; + }; +}; + +/****/ + +/* only valid in manual mode */ +#define I2C_CMD_MECHBOARD_ARM_GOTO 0x03 + +struct i2c_cmd_mechboard_arm_goto { + struct i2c_cmd_hdr hdr; +#define I2C_MECHBOARD_ARM_LEFT 0 +#define I2C_MECHBOARD_ARM_RIGHT 1 +#define I2C_MECHBOARD_ARM_BOTH 2 + uint8_t which; + + uint8_t height; /* in cm */ + uint8_t distance; /* in cm */ +}; + +/****/ + +#define I2C_CMD_SENSORBOARD_SET_BEACON 0x04 + +struct i2c_cmd_sensorboard_start_beacon { + struct i2c_cmd_hdr hdr; + uint8_t enable; +}; + + +/****/ + +#define I2C_CMD_SENSORBOARD_SET_SCANNER 0x05 + +struct i2c_cmd_sensorboard_scanner { + struct i2c_cmd_hdr hdr; + +#define I2C_SENSORBOARD_SCANNER_STOP 0x00 +#define I2C_SENSORBOARD_SCANNER_PREPARE 0x01 +#define I2C_SENSORBOARD_SCANNER_START 0x02 + uint8_t mode; +}; + +/*****/ + +#define I2C_CMD_SENSORBOARD_CALIB_SCANNER 0x06 +struct i2c_cmd_sensorboard_calib_scanner { + struct i2c_cmd_hdr hdr; +}; + +/*****/ + +#define I2C_CMD_SENSORBOARD_SCANNER_ALGO 0x07 +struct i2c_cmd_sensorboard_scanner_algo { + struct i2c_cmd_hdr hdr; + +#define I2C_SCANNER_ALGO_COLUMN_DROPZONE 1 +#define I2C_SCANNER_ALGO_CHECK_TEMPLE 2 +#define I2C_SCANNER_ALGO_TEMPLE_DROPZONE 3 + uint8_t algo; + + union { + struct { +#define I2C_SCANNER_ZONE_0 0 +#define I2C_SCANNER_ZONE_1 1 +#define I2C_SCANNER_ZONE_DISC 2 + uint8_t working_zone; + int16_t center_x; + int16_t center_y; + } drop_zone; + + struct { + uint8_t level; + int16_t temple_x; + int16_t temple_y; + } check_temple; + }; +}; + +/****/ +/* requests and their answers */ +/****/ + + +#define I2C_REQ_MECHBOARD_STATUS 0x80 + +struct i2c_req_mechboard_status { + struct i2c_cmd_hdr hdr; + + int16_t pump_left1_current; + int16_t pump_left2_current; +}; + +#define I2C_ANS_MECHBOARD_STATUS 0x81 + +struct i2c_ans_mechboard_status { + struct i2c_cmd_hdr hdr; + /* mode type are defined above: I2C_MECHBOARD_MODE_xxx */ + uint8_t mode; + +#define I2C_MECHBOARD_STATUS_F_READY 0x00 +#define I2C_MECHBOARD_STATUS_F_BUSY 0x01 +#define I2C_MECHBOARD_STATUS_F_EXCPT 0x02 + uint8_t status; + + uint8_t lintel_count; + + /* flag is there if column was taken by this pump. Note that + * we should also check ADC (current) to see if the column is + * still there. */ +#define I2C_MECHBOARD_COLUMN_L1 0x01 +#define I2C_MECHBOARD_COLUMN_L2 0x02 +#define I2C_MECHBOARD_COLUMN_R1 0x04 +#define I2C_MECHBOARD_COLUMN_R2 0x08 + uint8_t column_flags; + + int16_t pump_left1; + int16_t pump_right1; + int16_t pump_left2; + int16_t pump_right2; + +#define I2C_MECHBOARD_CURRENT_COLUMN 85 + int16_t pump_right1_current; + int16_t pump_right2_current; + + uint16_t servo_lintel_left; + uint16_t servo_lintel_right; +}; + +#define I2C_REQ_SENSORBOARD_STATUS 0x82 + +struct i2c_req_sensorboard_status { + struct i2c_cmd_hdr hdr; + + /* position sent by mainboard */ + int16_t x; + int16_t y; + int16_t a; + + /* PWM for pickup */ + uint8_t enable_pickup_wheels; +}; + +#define I2C_ANS_SENSORBOARD_STATUS 0x83 + +struct i2c_ans_sensorboard_status { + struct i2c_cmd_hdr hdr; + + uint8_t status; +#define I2C_OPPONENT_NOT_THERE -1000 + int16_t opponent_x; + int16_t opponent_y; + int16_t opponent_a; + int16_t opponent_d; + +#define I2C_SCAN_DONE 1 +#define I2C_SCAN_MAX_COLUMN 2 + uint8_t scan_status; + +#define I2C_COLUMN_NO_DROPZONE -1 + int8_t dropzone_h; + int16_t dropzone_x; + int16_t dropzone_y; +}; + +#endif /* _I2C_PROTOCOL_H_ */ diff --git a/projects/microb2010/mainboard/.config b/projects/microb2010/mainboard/.config new file mode 100644 index 0000000..18a0d9a --- /dev/null +++ b/projects/microb2010/mainboard/.config @@ -0,0 +1,252 @@ +# +# Automatically generated by make menuconfig: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +# CONFIG_MCU_ATMEGA8 is not set +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +# CONFIG_MCU_ATMEGA128 is not set +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +CONFIG_MCU_ATMEGA2560=y +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +CONFIG_MATH_LIB=y +# CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set +# CONFIG_MINIMAL_PRINTF is not set +# CONFIG_STANDARD_PRINTF is not set +CONFIG_ADVANCED_PRINTF=y +# CONFIG_FORMAT_IHEX is not set +# CONFIG_FORMAT_SREC is not set +CONFIG_FORMAT_BINARY=y + +# +# Base modules +# +CONFIG_MODULE_CIRBUF=y +# CONFIG_MODULE_CIRBUF_LARGE is not set +CONFIG_MODULE_FIXED_POINT=y +CONFIG_MODULE_VECT2=y +CONFIG_MODULE_GEOMETRY=y +CONFIG_MODULE_SCHEDULER=y +CONFIG_MODULE_SCHEDULER_STATS=y +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +# CONFIG_MODULE_SCHEDULER_TIMER0 is not set +CONFIG_MODULE_SCHEDULER_MANUAL=y +CONFIG_MODULE_TIME=y +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# +CONFIG_MODULE_UART=y +# CONFIG_MODULE_UART_9BITS is not set +CONFIG_MODULE_UART_CREATE_CONFIG=y +CONFIG_MODULE_SPI=y +CONFIG_MODULE_SPI_CREATE_CONFIG=y +CONFIG_MODULE_I2C=y +CONFIG_MODULE_I2C_MASTER=y +# CONFIG_MODULE_I2C_MULTIMASTER is not set +CONFIG_MODULE_I2C_CREATE_CONFIG=y +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +CONFIG_MODULE_TIMER=y +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +CONFIG_MODULE_PWM_NG=y +CONFIG_MODULE_ADC=y +CONFIG_MODULE_ADC_CREATE_CONFIG=y + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +CONFIG_MODULE_VT100=y +CONFIG_MODULE_RDLINE=y +CONFIG_MODULE_RDLINE_CREATE_CONFIG=y +CONFIG_MODULE_RDLINE_KILL_BUF=y +CONFIG_MODULE_RDLINE_HISTORY=y +CONFIG_MODULE_PARSE=y +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +CONFIG_MODULE_AX12=y +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +# CONFIG_MODULE_ENCODERS_MICROB is not set +# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +CONFIG_MODULE_ENCODERS_SPI=y +CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y + +# +# Robot specific modules +# +CONFIG_MODULE_ROBOT_SYSTEM=y +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +CONFIG_MODULE_POSITION_MANAGER=y +CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y +CONFIG_MODULE_TRAJECTORY_MANAGER=y +CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y +CONFIG_MODULE_OBSTACLE_AVOIDANCE=y +CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y + +# +# Control system modules +# +CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y +CONFIG_MODULE_PID=y +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +CONFIG_MODULE_QUADRAMP=y +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# +CONFIG_MODULE_DIAGNOSTIC=y +CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y +CONFIG_MODULE_ERROR=y +CONFIG_MODULE_ERROR_CREATE_CONFIG=y + +# +# Programmer options +# +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/mainboard/.config.~1.5.~ b/projects/microb2010/mainboard/.config.~1.5.~ new file mode 100644 index 0000000..860d979 --- /dev/null +++ b/projects/microb2010/mainboard/.config.~1.5.~ @@ -0,0 +1,251 @@ +# +# Automatically generated by make menuconfig: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +# CONFIG_MCU_ATMEGA8 is not set +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +# CONFIG_MCU_ATMEGA128 is not set +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +CONFIG_MCU_ATMEGA2560=y +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +CONFIG_MATH_LIB=y +# CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set +# CONFIG_MINIMAL_PRINTF is not set +# CONFIG_STANDARD_PRINTF is not set +CONFIG_ADVANCED_PRINTF=y +# CONFIG_FORMAT_IHEX is not set +# CONFIG_FORMAT_SREC is not set +CONFIG_FORMAT_BINARY=y + +# +# Base modules +# +CONFIG_MODULE_CIRBUF=y +# CONFIG_MODULE_CIRBUF_LARGE is not set +CONFIG_MODULE_FIXED_POINT=y +CONFIG_MODULE_VECT2=y +CONFIG_MODULE_GEOMETRY=y +CONFIG_MODULE_SCHEDULER=y +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +# CONFIG_MODULE_SCHEDULER_TIMER0 is not set +CONFIG_MODULE_SCHEDULER_MANUAL=y +CONFIG_MODULE_TIME=y +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# +CONFIG_MODULE_UART=y +# CONFIG_MODULE_UART_9BITS is not set +CONFIG_MODULE_UART_CREATE_CONFIG=y +CONFIG_MODULE_SPI=y +CONFIG_MODULE_SPI_CREATE_CONFIG=y +CONFIG_MODULE_I2C=y +CONFIG_MODULE_I2C_MASTER=y +# CONFIG_MODULE_I2C_MULTIMASTER is not set +CONFIG_MODULE_I2C_CREATE_CONFIG=y +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +CONFIG_MODULE_TIMER=y +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +CONFIG_MODULE_PWM_NG=y +CONFIG_MODULE_ADC=y +CONFIG_MODULE_ADC_CREATE_CONFIG=y + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +CONFIG_MODULE_VT100=y +CONFIG_MODULE_RDLINE=y +CONFIG_MODULE_RDLINE_CREATE_CONFIG=y +CONFIG_MODULE_RDLINE_KILL_BUF=y +CONFIG_MODULE_RDLINE_HISTORY=y +CONFIG_MODULE_PARSE=y +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +CONFIG_MODULE_AX12=y +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +# CONFIG_MODULE_ENCODERS_MICROB is not set +# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +CONFIG_MODULE_ENCODERS_SPI=y +CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y + +# +# Robot specific modules +# +CONFIG_MODULE_ROBOT_SYSTEM=y +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +CONFIG_MODULE_POSITION_MANAGER=y +CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y +CONFIG_MODULE_TRAJECTORY_MANAGER=y +CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y +CONFIG_MODULE_OBSTACLE_AVOIDANCE=y +CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y + +# +# Control system modules +# +CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y +CONFIG_MODULE_PID=y +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +CONFIG_MODULE_QUADRAMP=y +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# +CONFIG_MODULE_DIAGNOSTIC=y +CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y +CONFIG_MODULE_ERROR=y +CONFIG_MODULE_ERROR_CREATE_CONFIG=y + +# +# Programmer options +# +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile new file mode 100755 index 0000000..5867b4e --- /dev/null +++ b/projects/microb2010/mainboard/Makefile @@ -0,0 +1,30 @@ +TARGET = main + +# repertoire des modules +AVERSIVE_DIR = ../../.. + +SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c +SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c +SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c +SRC += strat_utils.c strat_base.c strat_avoid.c strat.c +SRC += strat_static_columns.c strat_lintel.c +SRC += strat_column_disp.c strat_building.c strat_scan.c + +ASRC = + +CFLAGS += -Wall -Werror +#CFLAGS += -DHOMOLOGATION +CFLAGS += -DTEST_BEACON +LDFLAGS = -T ../common/avr6.x + +######################################## + +-include .aversive_conf +include $(AVERSIVE_DIR)/mk/aversive_project.mk + +AVRDUDE_DELAY=50 + +program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep + echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + diff --git a/projects/microb2010/mainboard/Makefile.~1.6.~ b/projects/microb2010/mainboard/Makefile.~1.6.~ new file mode 100755 index 0000000..1a53746 --- /dev/null +++ b/projects/microb2010/mainboard/Makefile.~1.6.~ @@ -0,0 +1,29 @@ +TARGET = main + +# repertoire des modules +AVERSIVE_DIR = ../../.. + +SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c +SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c +SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c +SRC += strat_utils.c strat_base.c strat_avoid.c strat.c +SRC += strat_static_columns.c strat_lintel.c +SRC += strat_column_disp.c strat_building.c strat_scan.c + +ASRC = + +CFLAGS += -Wall -Werror +#CFLAGS += -DHOMOLOGATION +LDFLAGS = -T ../common/avr6.x + +######################################## + +-include .aversive_conf +include $(AVERSIVE_DIR)/mk/aversive_project.mk + +AVRDUDE_DELAY=50 + +program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep + echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + diff --git a/projects/microb2010/mainboard/actuator.c b/projects/microb2010/mainboard/actuator.c new file mode 100644 index 0000000..19d2fc7 --- /dev/null +++ b/projects/microb2010/mainboard/actuator.c @@ -0,0 +1,77 @@ +/* + * 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 + * (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.3 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "main.h" + +void pwm_set_and_save(void *pwm, int32_t val) +{ + /* we need to do the saturation here, before saving the + * value */ + if (val > 4095) + val = 4095; + if (val < -4095) + val = -4095; + + if (pwm == LEFT_PWM) + mainboard.pwm_l = val; + else if (pwm == RIGHT_PWM) + mainboard.pwm_r = val; + pwm_ng_set(pwm, val); +} + +void pickup_wheels_on(void) +{ + mainboard.enable_pickup_wheels = 1; +} + +void pickup_wheels_off(void) +{ + mainboard.enable_pickup_wheels = 0; +} + diff --git a/projects/microb2010/mainboard/actuator.h b/projects/microb2010/mainboard/actuator.h new file mode 100644 index 0000000..9c7174a --- /dev/null +++ b/projects/microb2010/mainboard/actuator.h @@ -0,0 +1,25 @@ +/* + * 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 + * (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.h,v 1.2 2009-04-24 19:30:41 zer0 Exp $ + * + */ + +void pwm_set_and_save(void *pwm, int32_t val); +void pickup_wheels_on(void); +void pickup_wheels_off(void); + diff --git a/projects/microb2010/mainboard/adc_config.h b/projects/microb2010/mainboard/adc_config.h new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/mainboard/ax12_config.h b/projects/microb2010/mainboard/ax12_config.h new file mode 100755 index 0000000..072e8fb --- /dev/null +++ b/projects/microb2010/mainboard/ax12_config.h @@ -0,0 +1,7 @@ +#ifndef _AX12_CONFIG_H_ +#define _AX12_CONFIG_H_ + +#define AX12_MAX_PARAMS 32 + + +#endif/*_AX12_CONFIG_H_*/ diff --git a/projects/microb2010/mainboard/ax12_user.c b/projects/microb2010/mainboard/ax12_user.c new file mode 100644 index 0000000..575f5bc --- /dev/null +++ b/projects/microb2010/mainboard/ax12_user.c @@ -0,0 +1,176 @@ +/* + * 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 + * (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: ax12_user.c,v 1.3 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" + +/* + * Cmdline interface for AX12. Use the PC to command a daisy-chain of + * AX12 actuators with a nice command line interface. + * + * The circuit should be as following: + * + * |----------| + * | uart3|------->--- PC (baudrate=57600) + * | |-------<--- + * | atmega128| + * | | + * | uart0|---->---+-- AX12 (baudrate 115200) + * | |----<---| + * |----------| + * + * Note that RX and TX pins of UART1 are connected together to provide + * a half-duplex UART emulation. + * + */ + +#define UART_AX12_NUM 0 +#define UCSRxB UCSR0B +#define AX12_TIMEOUT 5000UL /* in us */ + +/********************************* AX12 commands */ + +/* + * We use synchronous access (not interrupt driven) to the hardware + * UART, because we have to be sure that the transmission/reception is + * really finished when we return from the functions. + * + * We don't use the CM-5 circuit as described in the AX12 + * documentation, we simply connect TX and RX and use TXEN + RXEN + + * DDR to manage the port directions. + */ + +static volatile uint8_t ax12_state = AX12_STATE_READ; +extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */ +static volatile uint8_t ax12_nsent = 0; + +/* Called by ax12 module to send a character on serial line. Count the + * number of transmitted bytes. It will be used in ax12_recv_char() to + * drop the bytes that we transmitted. */ +static int8_t ax12_send_char(uint8_t c) +{ + uart_send(UART_AX12_NUM, c); + ax12_nsent++; + return 0; +} + +/* for atmega256 */ +#ifndef TXEN +#define TXEN TXEN0 +#endif + +/* called by uart module when the character has been written in + * UDR. It does not mean that the byte is physically transmitted. */ +static void ax12_send_callback(char c) +{ + if (ax12_state == AX12_STATE_READ) { + /* disable TX when last byte is pushed. */ + if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM])) + UCSRxB &= ~(1< AX12_TIMEOUT) + return -1; + } + return c; +} + +/* called by ax12 module when we want to switch serial line. As we + * work in interruption mode, this function can be called to switch + * back in read mode even if the bytes are not really transmitted on + * the line. That's why in this case we do nothing, we will fall back + * in read mode in any case when xmit is finished -- see in + * ax12_send_callback() -- */ +static void ax12_switch_uart(uint8_t state) +{ + uint8_t flags; + + if (state == AX12_STATE_WRITE) { + IRQ_LOCK(flags); + ax12_nsent=0; + while (uart_recv_nowait(UART_AX12_NUM) != -1); + UCSRxB |= (1< + * + * 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: ax12_user.h,v 1.2 2009-04-07 20:03:48 zer0 Exp $ + * + */ + +/* This is the ax12 user interface. It initializes the aversive AX12 + * module so that it works in background, using interrupt driver uart. + * + * Be carreful, a call to AX12 module is synchronous and uses + * interruptions, so interrupts must be enabled. On the other side, a + * call _must not_ interrupt another one. That's why all calls to the + * module are done either in init() functions or in a scheduler event + * with prio=ARM_PRIO. + */ + +/* XXX do a safe_ax12() function that will retry once or twice if we + * see some problems. */ + +void ax12_user_init(void); diff --git a/projects/microb2010/mainboard/beacon_test.png b/projects/microb2010/mainboard/beacon_test.png new file mode 100644 index 0000000000000000000000000000000000000000..c669f2ef73c6149895b7ef434f64f91d247cf15e GIT binary patch literal 47409 zcmeFZg^WX;45UL_oj+rMp48K|&e2pYy%ne{jzH@W%wR_kFK>t#z%qmM=Bc6>zaAu>b(TRZ^7I1^^fx06+~e(7<=z zdOmXof1$YDSJJ@%AO0BdaPT#zv!bCJ0N|V7{Db66<=FrL9iSvDqvM_SbJojWNB2x> zcTg=41w#aqgDIm7kf1_kn<703aU$|B+&$FXOBQNtKg*Kor{`CTtW8O~-$B8SKsAoY zlF`zH-ZfwYzY+g7IJ2CXrKU+LV@8!FMJYAK$Vzf&!p`K?%)Odp9M)UgZi7A<+4)H1 zv+$dTB08bGFyi;qR&*Ev!_8v^_y2$L|7{_ty&lG;(}>6-pGCP*2wMaaDKFeoN{)D= znHl)LGx~OzTqG&#vcXsJpRlD8{HRVzUYhe}Z}0_bDZY2xq*U57IoX>>tvPW2|9GOM zV>R?9B8v^u3&C72$@iH=@35pLj}0&rKMMW@zE~1W@^MB9`K^-fZG;4`%w^TUOTZoL zP4@8>q<_D!D2(%ed2*jze_i0uI{v$;eY1ZTLBjRQTo?6bPi{+xZ-Ydj|L5xdvtFWB zgRPdEJ^lAu;ETTJ5Ddbb$9kx4v1+I;$<31bq9^RG7BT*Oz|ymZrn$>zn6{}1E1uuZX7EkLHsG(I{`V6O1C}-{-1jg3mFyoen^>lx|3{AZQ)buD2zFxra|D*O zZxG;{aiRZi^nX2Jfy3g`(x{AsE)Unn3dk%hEaKwg5WPH2RP%x74(Stt*O$lD%Lyt3 zw6w7yAt+^whw$~$V~kSFV7Ai7#mah9&Wtor18VyPA(YOjEIPc%gDe>?GM%a z^vHa=&C$nsiAn{&&K~`)%%;5fW zk}z8#wxcJJ)5GWb_d!_^mu@16Nl57L-W|3yGBl*{J>HHEygZ|4Wkm;^XFVnsH;bC& z&CTf_RXO4UlMQ~t1LFIt7neIhod@fq5FiYnR(|61m~UlI^=bF#udjr!4vT`! zmpY=?=9{E|g%8Olt)|y!`pZ9lFfcQt0-9P{!DWMzw?7$Gb-Wd{^RK*@DTpj1&W{a+ z9}|IUCa6SL6j_ZI9(3OmCC76Y0+f`LtW8((9R2(T1DtD&4NBpdEUIrbsr>=zI9;%iqr0XeWoixewTQ&h~CBn61c$j-&B`uA^yK=>lH z7*m=i@Wi|?RTzIz@|+6|pT^$Z?Z8~P7V-tj`u}o1EN0-FIWPHYA--KbJ+0+7qr1zm zG%WU(yT_*Qxvv4z@?5r@+}xpAY>MEW0v*6(R#s6VBqP&bt#}ZIcPR3?$Z}k~h$kY8 z?#`Wl%jxg!k&sOF)>lX9{S%%}osI)8)eSz=cKo#xGXq;UKaPE#4(|-7ZhfA%vs*jd z|Iy!8x6^3U@VJhK@jApGNJvc7z4YE~L6A;*AMv5$79C>Hy85pbD2#plkIm#JhexHCIP>_wbRf?tA#yW%1atujA^G z=FIk{U0DVH>ngkX)DIGi)eVi;`h8#3!H>)wowhxO4<}4bpGql|U)&mbSWFXi;R1j1 zWD(RqGEkU{V&~aeS#e28p{UrT3n#yp#-_b?=AubB^%g*D{5&V*dtm*gy`BEvz3CZQ9kk0V= zK_Py&mQ60 z*TJYp_S3-(kHDjLGSe@GhGoTe6Ws3GGZf&%d+mJ~0R_eSuiC6E2Dp`#MRx+Th|eAq zsC`kLv_Vv4kKMHzwtn0Xx$z#soi8x@89~!2z_JEf@xvDXZ=e|Bj`t6|q+rA1G_7vb zw~q1uqQ3uuu>XZ_H~9A=bg1=Ty#61=VFWkL1Oj8$4;QiYu0D6Ottqwq{P6qm9Bg)W z*n%^>)vAnQR=iZ?{|ZQkAMw-FS>>bNgtxY`LPP*IfTP66jYjX&+whs0({Y^pKfy3l zpbtWSa|hd$lo;fKr3ljru$ zm#_Zj$aEYw$3KZUFVG=UAzvVDFS4q6eGi=b((Q(c>fs#!EExmX-Kv`ZezEU_wkwoF zlvD`GLfZXW&l5HWzl$T?<@6^^0&akly4fhd4y_9VaMIgi-HXk7WTS0Cu1U;nwl53w z;j*RqCp!9r;ROf5SqHFUin-ohJ*Z+R@~Y4&8yYfU@S}zBcMOjtiaZ+!;xA>d%mJdF z8`*n%efghQCd1<_6X);~gyH;7&ws)Q0QmJw3m89{tjo2PiVq@35ZpLWU}03l6ykgf zF}7IF849h2jUh@vb5+5Ru= z2LB0wcKg@e8TdzoE9`{dXE9*K$8+c8;2_%KDTWebzWt6Q2WR zyA}*s*zZsl5MM8gzWnI-F{rR*aCKWn7r>xru=_}XkacdvK_w6R5FXyDX_X6m@!}!i zk7CA!Z~;C8yM$QSI|^30Z9_+d@g+xFZb8(1jC62HDo~lR9u(+b17_5~ARz%AezQ)Y zN&opn*+u&ulT116%$yJi1}e>U80Hht?Ay{<*S5D~0v2Y2VPW(woz9B8kgg!iHi&A$ z6fOBb`uns(a&K7HyGf4sjBL(e`3xHQ3 zS>J~TEIc+gwyRc3%8(PZ-*|vA_3woM2q8HuUkK5zxpT&QWl5OH0JH z2Jswy7J{K5%{Fv%@a<9E7S=pYN{a=<0{fzT`lBi-LS)+_KOikL*76Qy!$Uh&0iz7N>Mc}R}{ zTp_)OTvRc_P*3iEw7N{2)*6E0Ce3D04f;4O@}6E!MB%kdO>#qGS8wVEx5)4=uA?oO zg$3ehYsC+{T^&yNo@g{d_(rUMZ&G6%pI?NP^b;TF-)nv?M4*Of0v3Or0C8wm7Kf%O z3D-9b*+pycj8w>^BUA3P0Q93bDFSime?0^n)m`AdndR^I|8WU&vm6HV43bxPdMylC zmX82!!^1p!P_*3wC!8d|jjT}TkV&TmzrO67{hNEoF0yj7IA#QheIN~Wfu8E#kw(sk zS=78yia57_-0qFD04oVpU2x<6q%eEifRp7uhBMQ8oCi2SoaN(4`V^5NN^ z%&Kq%U2p=xtO36KHF!20CFi@XopZ!MD(q22a(JN(wzO>wf)_mm8r`HVD{G$r-a{Ip znDm_)>w%zxbUD*O90laBq$nSuqlgS>*UOq)&r2FY8ss7gKxdZQ0x-S)7VQWae%w&X zx1jZAZE2^Z^_$4_uw$i)=|iMN{*#P=i}5>#Qr{DckH3p5JST$IEMn}^P7a5wVj4UV zZ<_?KU_(5{VW?$(U+Xo38Hu|0H`T0n``4NgOc@H&@{+EEv{Dpy6OCbD7>d4Z{Kt|K zCCT|aUKTZFguh~a7qa}EJ+2AYM^X801c*k(-|RbtN>jX}pU_Z2{?XKnB{-OVdI_cC z*3|5~?Du0sav8aQ_w2tv*)!TB_ zR1O{Wq*3;2!x`y*XO3Froa}dNQDxzz-1==Kdj=V)P3#Gi>X}4u6irvp`Jjfv3u)H=IoE)Z zD$FC}d4v*0e)p~!iPLY7jFLRcuA80ksx#AR-bGRUvyosARsfwH;;SkB;K-lN^+E1Z z=viaj`7K=k-mW*#e*VOagO5(R#%DAFZa?~V&;O|eJnMX{_4x{^rMLF{1$J;j(Ogu1 ze!+rkfAG^6^@8CMJs=k&LB0ZXq52C@5_2 zB1(^m$9O;I@u@^ITh~z{B7^4xVKFqQF|_FF87Rss!F#UVE%}A+JV1U`z&Rl21LK}> z17=9}mVM}rKR;b`t~Xa*g0}qDeJTQmEZ#BA()B?pg{+_;y1ONd3U!{wcTy+^-M%(M z9KM?E{}3;HBzQO9PAwlvY$_6HOw$>2dhEZZDKqA`%YsVlzl$PylDWR6LrllOz#H4u z)zRR1XbTL@4X&6x(tTN9Z>Sc;FI(~3VLnVy-MIJdpnR0W`E*5ED_*?P;nmZ;hudF$ zX?m%nChc+2xgM?0OM@u&-=K@>)<%Di+gItOn(+{!%fS^anYk#JQ2!Hgw$F;KG6ZMzVuEaSwE~MlibjAZAy2WfKzdia#^R@&n&0^9y zH|cSUzBllTRVk0-e55M?GynEx zx)+D0&houDr|C+lWv9@UxBS_m{zBajhw}kjfti-`-5Y=aVku|CcRQE_mki(IN7cIR zIK#h>YOsD^Xt9YiCb;v>{Z2vx&9k<<^P7Y0SaET5v5f(@C1bPLCaW|KF*MWE2N6$} zO-` zT7BpBea?>>OvC{alKAilWOK23Hv~8E?G!q=09;L5DckMjbu&xIiAh6Ez`4MT*Sz+m z{k9hAR*i#^-H#$`nFp587p(7eocH^YgA+n5*4Cr)+$Xnv9oGa)N}8L4EwwZWC<`3w zq}t|SKAgHOwxej(JCdw(fpv?=mU8tSeE2bc{QZ>+U&godOm?@$=k#Jzp=)atY#6F7 zoJHVLK0WGQisGkqvrobE+^&hAtmr+aK3wW#{B^P4yx8W!Q=XlzMg8V0o_f85-DvJ9 zMCHkco%qm>D9Nw>TP2o<`YI7Io~8%*lsOjXo~Mlgewkp610CnrFCqaHK1I8q`N1TU z_Cm`)2Eb?t=HsK$yn>|Ys^b~&zp7F^Pv4lXa@pxj_!rG^%B`Y>$mSXtYA$R%A&MOv zV^U~`7tGAkk)$#ba+)@yEsrV-rQSDs?O@qYF=1A>xAQ&Aiq87#?y*TPtFjAX zDZ>zgwGn24IsBB{H-+r<*GW*ROO)l+I$Eh~D$E4}#7jW@R(`Pf&zTxx^*nxSFX~S; z2y4pP?76b1e2phrE{r&=8a;|~s!RYK%w=> zGfx*gq?C`TCoRwqB6uCi$B#;vn8?WY^{I!qg>4hq<*v%@cDjXg95ih*H^~+&P zt4qyJdTM%`5!H6{%5any<45Hw&{i8a|LEVAUaH%H=VaF#lLEJS0dg^7az0s_=V&!N zkNEj=>F)VaBX|}9&W2=wWIC~0qS?B4!EeX~Gf7If>+lUKary@!AM>4yiWWGfPp0RI z#di@?W|}S+-j}Np(VwKgiSmuS6ttt7Y-$1!Gg>?IQWVECuUeOK2l2kXsI6np37$QzKrkyZ_*jk(EK zO)}3jONl0H$EdBBS7A~Ybc~o0(1V>FJb_Rg$be+smE19yg4Vj%;N+=aE;#VaJS-FE zgnMj@le`NMqJ^PPz4gEQ5fxV<7Aic8;(sZ)d%D`s*|6~m?xsFjv&JN|P zn7se2l_irlh6{Bg`=?3P4{hDd;vqN6vE%vPc3~jRC->eEC~T2csv1p4WMhDW<>t7C zmKO>f3sC@x+TD9t1CRpF8s_VBk2w@PcDU!yu~;avD9d#-FLBAq1;}^4vM&t1??C5j zy3{)+zw=0dTXVP(uY=>5*k9lD&Z&BD62)x70_L&@A&lS}jN-yEi0psPZBf{Rn$OcU zAoAc@RM4dgG{$WZb*cipuSOw;D&&(<1GDBt(<6c1kj|vpx)m*>>B{D=_>t`pltF3p z5vCN5O3zPzeh&&_UU|qWz|!yaX5a1UWj6cBqxr28_vFavbk3+Wpf@Rc&{RoPKm$*^ zdb*B`Tx4iXPqX?aq&ByimmIXwIX!hv_#&roiQy5?;iSqU!6M(T>6znfE*_ z@`1FJso}<4kQ3N(nLc|&zlP5 z$jByem4zJLHJD+D{lQ}#RWv-D84UdR+`_``avrCa$Rp!@FP+g^k5On+waDds9!a&n zb5W{g(rbaS5zdVBR(3GE1-`)F(2;Q_$FRYkY$-PbKLBBa5a zQ{OW|(;Xuk$phX1J9b9P3=#n{Wxa2Z9J{?drPb8HMbkBFaCg!j?G#L))?*(>84Wjq z@OADFk6kx2@q!~68=EC4>6V>S;@9%Mp6GN^(n2g7t4-L)X0o&<9;U(h0R3RcWp!s~Kcj9T9d4^eADh%CTb1cTWXe}M{<1>Z%p- ziS)4_ed`-?L#AJaW#XpC(@w67di=bn2M@i7T>a}+r;*fe%AE!WgDFB(Xv?kXbr;H& zopFbuq^1v>)705j3LNhFly2Xhs&p`JF7+yk*xPGH;}Uce;o!(3pjh9lQIFFbbLyw0 z6DDKhijoZwwc^%?FHqW&-sGW_Xmql0u@4_g<4pV0qMtzxpEGi+_sp7jQt)~pQK$mW zd!7a99i8tqPdmYaF6~%;^reJ92+M`6p)S|}sX*F&_XA1ez*9Mh8o`}A4#wI}b)E^V zu2-kq4oIw@-U;m1gs%_$7|L7N0 z>2Uj9lSSF-XXVwDwnaoz+uu4|TH5Jk&1mS1@u~4=!zJ$W;VK>iGX3CEK8Ny(N@ZC& zU4d=d7x@$*ip9V0>0KG{?KiV*L+gGJ9+ULqVnB+v`q%_hDB;zEGF1>b6EghKM@!MHhtlMEh7 z%IY!M94GlHZG;fBn|k4UeI9fKh4m}#slCSvj~U>>=gm$mj}y<$Z=eQ z4Q!LZOAjst^RD2t;TkteSJxykuh7Xz%yuO*6txKBx0)uZHO zf_EY#JCYV06pg8PEzz8hX9H<_^3Kkrb%SB(@Qi@|%$!VV`4Aw}va?o3$(2uk#u1Ct zAMgHS5Iw;J4{UtJ4_*eGMYSc}X*`B)Qzg{bL#vJdMzr*Gco;X`$A`t?T~3c&EkPBS z74uT+lMG<$s{TRgLnCfrd;02)pb9tg77-J<&nh(?C+7)fR$`)Sywh^4W5jopkLR&L z?oM7ch^^^ayCnNbfXf6&{5b8RksWKO&TF=riM{*mR9ZHgUX=1`#d*@cGLExCWxV-M z;o!yhSwdY@&z&Q#fI7$YnF@3`&2=1oa~po0KB3d}c-PU}I(*NaB<#&(6R3%|OhRQ* z4cZ%_{_DudQ0x8v=hO&+Jp8OIcz=B0CBm-A!%52JzPy5MOj2LQorl_h4^MYDcyck+ z4at~}$|Urh{O7B5R4BFhVBtqz2I402_TTErMupS6rvvhVjdcmi6Q7CTGeJ$Nd15=_ znGet{r|krJnz*uMvK3UaF5B=`Qq(25xeu_Up&xZQPaoeF`|9SWr8PT~a6IpotR}G~ z;3V_%R!d`@xSO#BTO*Az9Xm9`cFn+S~6I7ZHp_-4p zCL^1!QW2dY;Y4UCV7$$MvV%)d}W`T(n3 zinUv;r%wt7}?=W{gs_lO+#gMk_x>Sy^+2<5MsQZa-k2 z&9CMn1rwWefz>7uW4cXIFdm(w-0w(EJ7vO z?j+8if+`qWXF3(>t#Rl7q zG7ovW<2iGd44SLPj)sm%8OC`lUHd*Q9C1n>Y;g^Ge(yjRU z9iw?}Q$<>K`o|)eSaRtZE!hJv2n{NF(lx$+ys99#uo&TmcWE;*bv<*%#N0iZns)Q| zUyyngy#-Te#il8{x`V}flNPeP54$M?W}02O+>}k327F&VySQK_AYtBAQBCI{s|z?s zE0X3_!j;;j`28wjME>5Ury~5L!#Q;*mRPDhRxY-2f zU+^Sl-jqIR+!fvLQ!F(3q+zsC=X|-&ovT+zzd{uU_ueygMwx>#ob&~5;D`D6ma*opywGktR;361whg>FP-+=PKqSCz1*)# z>jtIftJ#HdQPb5Prw!%v)gw7bCj3z;rrz=)bkH|vjpTPaVm#2vm6TkEf8v9z2e2-w z`|?WTE0W=B^?ZM54|6PKVwB@7{8Iv`UGV`kBAD{r^_m5QINIlF z0;DdNujbCzRLo$;3=F0qd>g(_A$^a$9=O2hDyQFS6|LXN*VE~0&u4nw3x+Q9Gu7{K z-G?ap2;+K`4Ijv3bJ(uJzt#iWBXZ9o$_CL`RCjUszAwS%E8ku`#;kp>_4;`J9e%2s zJZIx6+R6O&@~^$#y*c^VBr|y5n_N_#2!^J6pNv%4S8~NXklSl2zP?OHJNU-`hzjge zqVMJpM`Jj(sA}_%GU(vWrl8k9u3mFf7_)!*tT(B9inyqWn{4_OOSc@{5`|;!>z;?? zx&18!8xHkUy^RYANy5Z}195iokVGq#ZEg60+|FFW!Bw7A&GRUe26#`LuAnC|Gl#M= zbu1++Sk`I=V@*C5; zof{D+{ao;fS1QIz`UjdlX+_f2XT09QoSGgZ)YBP;8yoIMYT}aflA@wEazY?BaqI>e zX9pBvcZ@bTpKnN&1>ki=#jh(=&0$>}7HLyfzK>K>3y8eM!YuJMpWQ+2an<*)>?w^) zce+5ob_8>U%$DUnr3pP+n;0|>FcSi+;E|+Cyn8FjdrrPb-?q)*yJmy_Nk0lDM&1m{ zJV-q3)7G9u_8nERQT?Lq6$c4eIAjbtxl;{B+-AvlUbX*>(!74rYYSjQzBW34lwBRL znXGYRx3GLZC;M=S^vdtj!>j|xtQlBn!%y|Z*%~-T>Y{?7D+*oYEgJDRFTZ>VeLm3f z+KEQUUfv_F#4b6?9ngOKQJ-f=M*eu)RX_d0?I7Pj>H1j8@cmWQJi>8y#)mQJ1m=9c z)q+X9f$d!9;fJv~{^IEX=?gq=S!)`4<4anRzzY#Retws4Pm)`#%D{du6zsu!m?VSD z7O=T47Q4J$J4&IRF7~3ZkSoolgCaaK^1=1h{?UPndCVwbG;hz?K_hfS&@5rn-E2&9+<+TJ?btT*wA-)efo4^&M!3(=gw1*dGf(}+LI6tw?W(6zwKqtb}Dw$Ril$?0!)MM z_|PwPs?;p3^fD8O`e^GBnAVef9~mxuu47tLQ?}S8WOJATCLW{L@5e;FzGI4K?2&kp zYU-Jyn)0YP^Gfoyq@$RFn4#|(l!-O(y~aD2wRtIAhiPuGTD2b^*SzpN9LkvECXGie z7NTz&K(d8H~dfWEG#X5?F3!l&X$jE^_mX|lMFaRIqKJ* z9O+3A%1cns{F;W|3}{2smQ}=I2`xC$t$e$)L@QH zkGXs5-0yBTp!9aX(!R7EVOmJk&!MujMFw^HFX47GLYB9yg{YB|(d_f}=Ffw+vYNs4 zV6T%xkw8w_3weD1@=|h;u!}ob!oF6xu;KhO5{mRa$REB#h!=*ev|GJu%+FU;(`YhP zQ>V#3T7Q+x!WyeJI=Z7D84-arEpvK@yxM<t1%2FvoVhzDH8H&eY5_|{u#fN5)tSlhuTI(ry1F;? zD*M5%E@_b51TwDl8XQ3KRrOj{c+He2`Q+>8kP~!c|LPt-JVeMy&6zbUEM7R+h zwHTAz{%s>A_Q~(xXI36Vj;Y7gQj;utljJ7LZP>xQa`A3*z1!1;Zq^T3BM0kzTkvLp z?hdX&g)(=+Ta{Qz>u@BL|EV(c?0b7D(Xq$GY0ykQ8-?hI_=R(u6k}rP_}S#`w7q_- zwda~6nzYbvgGQ?ACA`zeDr$*ff4b^)wS1a+f>olyh0#HWObY!U(iCPYkV6lWl-4Fo z7_+jnz&!}h1RT$T1g-E+J0ZuJ@GoCvnOInS?P&$92>>&YayQmF1seN+*k+;FDusZx z=3!u#w1|JTOM&F(2e5fwSm-13z+c>XK0v$GW80Wr|2*iLbySL!+nx1+k(wj-?ow>? z;RMX7k-g^D-Mhh}M>$Hs0tzoGr(TrIV8&8hnm8H-#lr=6vCaB9uSRu8-BT+kuoKR5 zcxYZ^f^U9xp{km}B@6@14sCt5r%1>}2)ut#bg$b)fDM*=MuD;Kd^reqo`ttkzzBfV zbT1kc??qHF-&HWEab?~HClu=NSgjGE)KX4Z0CJi?RoQp}#BXv1JR@Q>EuF!5g zqe3PUIWb{7T|R-9C~=aPc_t{ZM~V@KN0EB1mFhXXWCF`t2b_EhNldmob9b;0W&{1`Vz*^`lWsv&fnwr1&pWL@`c>l6B_NFQ zs)e^gyj29$)zu&Po;}O_r0NtZiNSn$xcF9Z-(}WAsP8SakHYCGAK!RG-9byPTJzNovUQ?QLCXuWUU{j3l2%W8e(12T(k z!);JPzhP@cg-7@NcxUJE5Tq1tP`c&pO*xH_ckf&tiQw&G*Q%V+3S3B*Ybv)T46W4Zx7P=5bpxdJ`UJvq=^LY?YX+= zBcH#Giv#4eG>wgoS1YC}@eaSL>ZUp;f0?k;)qRnm>!q$9P_*i4=U5~PwFo`3ZeLPX ze&%-JFb&62(jy(Jt%#U*d^HnKxvrOc;x$x%L^+i8BI5c2y?S4LA;S;q=g(gA2y;g_ zo&#je{+RexBOy>lM%F9FxYK~n^_XM2taKRyJj?}7PdNr8Cicm}c2KPcM=0*A;a3W@ zg1MIn+{|VcNFZ$-2>fwy685loL+d=*btqHDSu9<|dpEQ52v=8-6CIBVL%Qeq zXnXedWoj4EY|Od6hjwZ#&CgcojzVu7U>Fd%-*`}B!n`uUt+u&5i{r9Rau zbiJpu)_7$6wqsmcQY1IDJ}30lC+1A0v?q{3V`~~FJcq$$$nb7VLlR;;uAg!jM$U&4 z9v;^$Dy*N24{felg+9!KYKYT|(u4excHm1`dnDNsNbYM3CuBrcJ@y#W1u1}F$MVqI z!P8UdQKkLM;cNwcr`uA2;*A%3DnGxK<@rvRSauehwLqh83*!+Iid~FOKM%q}zBAD+ z?t2Z(ibkMt0>kbgb@tg%HAty64gD_g-`q6yaM~2KVwLdpzObfX~MEQC47>1R7$=$jU1pRu(lS9ZDy> zV&3K9nOMXP0Rr*<;aA_g31jv}hq2NQgAsCwk9O|m3 z3gK=|RTKnnT^`TJR?YchTXx2@TgFO+zIj8g`c9I1Wo4y!daL&8wx%`zCWd0ac74m& z+6q;>x8fAu*8$Co0GJODS@Gj?tS!5X#iy);T`n-pwz@KcSmV7OMimYI`(7 zTokCNb`YH7QelxN9_;RNvOrsC_tM9_9ERD}7kh@m&o$CawOJhh5b!*x~2fs9W` zR*em}E09?!$i9A`q)T@;vRyY{`t#e@?(aLHQNa@P8_0d@RijUyYXm3zS|_ezx*h{m zx?3Z1pUx!q{rxXWm_FW0At7TBz?aiajr*l4lgFm~Dq(0-LN5t=sO9OzHeIK0d(QKi z<6&%{*{XVXcUL!ryXMPe}}w4kJ;GqJ}9# z)XfUzAZ;pK#l!>x>bb0(94Z3tzVZ5{XeE0P^9FVwZXSl$U6$f1{VY9Xj`%?+u)o?XhCX4!SrF>A!(Bt( zbU~EnrmTx3ruBP>JVMv&AN$dC;Yx`309Y+FLvH`*$39sPAId^?ZYN0yxoX4DI9E>t z9BY-JgmMrQZ%F=IR5e)?1Oy}ggUzPlTXuBWBBp~}MdgHcz-tHX>KFdPrz-e#wD5R? z-zHCXf%ao(nf{$Luz$|3s7~{|F<{e_K0Uu?KvV$>q?rOfKFvvYnsb!4FOO#!PZ=T@ z7+Dlz$S3p|_|X3lM%gBHHTuM%S^7l24{VQu`&l-re$L^FpL6Z@3%mXj@DWPi2#+&Z@I!}Mukdnvtq!r%rw0ht9MHcf+S2x$H;UK|pK220QPn{^rI8e0MGHmV>3q$r_I$Vpo z<~?`CT+@{Gj<;D|Y~hCT(i@x4^Vt^8HiLJX&?T>_6cqs$b(FKCMC=c*SW^QV*Va^J z6*NFvjNyAK-N70~WnTTBHYwmR-P8D_7yc+Gt1Ex`cpkl>rgmwy8Rc(>o~zH^4q^TMd!GbUJ2eYPHsMID z1BoDl1lx_b*M?!4-%SJXg~*xIQw<3<~;oJgVV}BNvaVAa$<#uJTrV z;7zIG%{M)n39yBbK^T`Sc5J*b>-B`A`}#bn8H`)=E}ep4o?FQ4r=~t~bLWq}z717u z;^gFTwO2Ys7Tb(!gHGi^)dCE7RL*;iwNC5{7JfO#2huZYXb-iB4Q(30LbyVuJ%qbf z@rZ;(DJChai}9Wjl$4=V2<+96b`Zubtbe=Z{Yy^CV6gACar|6E@NLI@BTr%gY;pcN z8VpJr`_^=FUtTwgp@ya7N0z3yvU*Z{+tUn)UXkP3;PE=+JK8%hFExT(cY3?xhHuHm zk4=2K@(N`${@c;=)oJ0|#0WR!#<|#fKB^9*3DAMYuf9vJ*;>um%qab!wrNr<&7H)e zkyJs5M=K-*xQOt@zlPg$M}&?AMdf+XUM$f{D@PE^TB~y>vfL%~K6f}xqn5%0i_w1b zUq9|u$!ozwN=qIxN4E|JWB#Vi{T6D9f^?x6YHxDVo#49d1UcM!KYTWBH14`Si6@(< ztHD~?2ndXc*JccheB0r!d-@+zhjzkp7O({5n75(?xM{J8%gOh+e>4qBtdN4f*X2}l z*4L$Qxfl$ZIXCgwawKKLa_$$DHa9mLP?Bx=%lli`LhX%Wq_cYajATtRJR(8a5R5w2 zax4{6k}9mC`_#a4JTy+~0uufbbM0elpH-RpY%N7x;xHLKpDQ~W-dABVRI@``e%>tn zvm?|DZ>aaSsM#2@ndY7t<;nT{EJ?+jk3uD+S%J3OT}ck2{cxqs@O)xeP-_uNpv7&Kj?+0U>kRn>Vyo~F@HgVc;%`xJAIuCv4KqM%dMlQX-esr!q} zeA=W`Gj;l|LeK6nKu?xvSB~pTiMsj|jftf>+@f!zyKikso70o6Gw~7qzBrVi45a?3 z014^1X=13VZ<#hjqo_F{la{d%T7No{fO2*tc?yB9{qvvp58ld{BQE^+)w}nVA|&_w zGSjlYfgrIh{n^;YUFF#{dSaqsX`&5$D&YIlp{VdRDaGKLWPagm23)_Ru5jbc@lZk3fb*EP@lTQd z+dJR(W5dHmbOn_YpcKp>QR2dKaz6HRe>ZcdB>;O+A&j7nwi6O8fwdO!)Q`{mTx(1YzFjSLjY3(Y3CTVSB$R8Ri_ zN$Wo}S=q&q)=0KKQ?pN(XHAGo@rOQ5+*ij4f^5*n))eX%alchAUwzBZ=PCS8${7W9 z8?V1(`!t?d3{J?&E87g!i<`A$f&Gz%j@uu!2loei&(K;(Nmfo9GDeHNlZU*)d>Qm0 zc^OcverV&xNVTUxx#CkW9;1AlHAV$~Sp4)44;iing3;Er3W0^tVq8e09a%B_WmKT` z=toyq1Xx5ENN>1~%IXq$tsxnIx)yZuoU3rW;ok0U8{ab`RdoE?CX<>GD$G4%6VPo8 zHs7gIB#~mgxm7YZ^&-t_{v9Z{ehy7P6P682y*qtY2k#~&owT=Vi|^?Zuz|9m5tOi_ zs|$_m_#7BGZrGgaq)ZT(fhT@mh{E7ChiN}glP1K<;v0DVBeg{oKc0t>uz9nncfP1k z)p9EH&PWPK_XM5)+DN-w{J^9^W+VF}z88_<7cD?YS5{vwmYCURc7qnpM_6DM6cs9}gc374|V_6du%VTB$ zWNI}yoVpsGUR+-l`E;r(#(5BVZb40aQ-gk8*JOMC7lgg7VDT*-9L0fSJf1ND z{Ym-$pcgt_MV>W#kH}5K=Z4myvP%@t`akheai18Jh>Z22pD}l~5z&d{$@V=T>knQK z_Pja^ROfEy;CmWyCg;Pk(uDn<#M0KYeqorB65o^FcPMk;EsiH>1IaJoW_s6mGbqj% zqWBRYQkevhoc!dsoKARqL$ZSqE{Mu){wyOizc~2-)(%5WTf!*L>3;r;9Irv-cvK#@ zs@Vri5$tXFUc|0|6o7M_gCXa8uf8CZF+`5Mg4C zm67it7D*p46G-pkl#73c2ab|}HG7vWMP;a%r@KZQvLTka;3YH?jBQbE9$NQFLWyi4 zUALXO_CHnmI+`{b6WKC)Jfg|8)Bng&?kBuEX~`!x($u8-?NM3o;yW*~ba0lG0f7l5 zW#zyLg`7-o=*~AYB+m&(bdR^SL(igxa~7Tv63fZU_@mO*d7-mhM}(vN^(lo&)|8HnJ5qf>~=S73DT^hmOLh1Bf8n&R8&9`8ZY zTaHn7J9Nqhzuu=d6WrG^PDhd$*#Gv{+)s+dQ$0|ai1W+>_FftzzN$*1BZ?Xm7MS8N z*VIUEb7u3QqBeKZ_7XRg3z!YjeRS15f5yR61V~53W`6+ZPDWz06bS;pqwF3x-5w47 zs&krpczK*L{VHKOWwbaP8un)%3rlb8R#`0K4LhcUW&p1pDX%*lul<|Vqcp0_ry9p{ z5%CNR2+pF{vQtit@%O4IC`KCd-gy>PEx5|HO6=XeEv*3rfYTD0v{+a|=HQ$v#Ez`s zntTu|V5ov~cgC$$`p5V7vv)zeW~bA-ew2D(V}AMkHp1XN{{4(U13&So)RJ6YIJbzR z9AA397lM{Lq`t=dByk2&1T4NB6olsFLa)!?MMXr>+#xBe^>~H#NE#I&A@Uc;U3d%_ z0fp$h{0OhRK6|4O-235_OFvRSw9YtG`6NY-KTwUDG0Ue$$jT}b3@RJQ27|-bUC;2S z6BZs6<*epnRWKcVD|=Eq{pDrGbkL^;P}uc#jZt>25d*s5Noxg0EF&YGYpG-KwD!gwm1pZ&kx@2A57Pq0cE3KR{hsNpCI(R6K!8As~z!-i^6PxsAzK> zz4ZA}VETQ1hiR~ZBlL4hkd_E>a(4VOf9jf)MdPT5gY@QTYy+H^qO}zlWA4q!2_Jx6 z;u)QM30zKgOpt5j^`xew_z`Ug(_bHo&3BFwUO7@ZrXa1On=n{Zeg6kyLdTGukxpK( zX-{F|vk>qETc9_tB? z^zZ3)P2K{t6dx2O4rLlEa>%bde7z#Hz_V_@z02l0Wchm~~ZJ9B$K2L-q|k7T+^{!Cw$(t+v>@Zt7uFw&J- zb-xI!qGM!5xBf;%E~;woy4R9%bNssN@0>Y3r++#(I_^Z!NG~`k=UiC4>zOub0|q~4Znnf|Ajz2YDxeKN?eEoe-uJsA{f>jMhkt#+WSJmK~%K# zyLVyGb6Du3`jf}*YXp!5G~kkc6LC;zp& zXRPp<*5Ps&p;Am`D@Hg)u9;vSzA?8FqT!ePPNYQO`6m>5J$z{Z^S-Ma+0uvPD?Bs5 zu_b=s8ht^-d;PI=jhMzJdz+9}A7sw;(|Z%ltStKA>C_Z|uU7-b?i3o^XUsQn30 zpWP(dpywj`>Awx~urQfKiFH8N!U2Hz?3~(BtZq$iL8Nb#A;)*!dv;+{yXji~F<& z|2m>}XB4JtEGz^Gr+;i9l^6&P_gZqw!V9p%}{d%F=QoI2Tk<}A+%Owu4UW!>z3yX+B z)7BP;#hEnx=i6X(31ZJ}~H=Vz{PZ!)aATRNHhtTHtusoz)dhv)hiWYyy83r=Zni~UJL_1Gcd*`P=8O=^#d<-G64KP#s7lbTd=ng?P zqY>cbNmK0x$9(qf=_**$Me7AV13%bZq4-#Td9%~w7o()9JG{w27R&g zNVqtoexFcD09g73iR_0(m1(aS-3G6B!%suYVH;F5LxL4Y4nv`51 z-;u%BO+gy`gsbz<=v$XXAs^HtiLX9d&@Fe_Gm3KMToCkl;e>o-Y2i^`D~Zi&!jPcz z)WX?Y)~oMKg#mUBXbd&>MNN1s=1nBHh@rT!^^Fnplh^Eup96j}_Q{@#dJ_X*Dwx_Q z)VyLr4dl?Xf0-b=Fapx;Dz z`|md2JxT;+(He$>=?rTFK+nwc^?V_M6LH`Uw!ec-gK%eD>b#P(h^GV!@u(3(4sVmg zX*L4PF#kQAy=f<+5Ce0RFC1XW`wTo;o8kNNWE>n(4hjzfc-?YQNV~fmP2GNy$EEhJ z>-V>BNOfR`Nm@^+%kxW}<9J^lHd6}=>gJl+3hTTz!sMhnYd-k5LbW_Z?^D@>fdRZu zq?nF5yEw`2+q&8`-3*$YFjXNE0}sQk?!t9p+$i{b`<0*{l>Yyr>MOva?6$TUx>KaP zl$MfiK|rKKq+7bXyHmOar5mIh1f(UTyIZ>XH@@#V-}${R9UaD*XZEw#+AHq+UYsgc z-$0G_Qc6k_C7@3Suqqk5&8WzI8uImqQsEp9kRyoH9``_^MJh0lYZKDfoarhdA!gOW z$*NR2%!^_=`BMO`*x9c$XmrYST0aJ@VvcQ(EL)-{RD}G@S8bMb*6!2 zJ>$~-s)G8%k8arg7?BkR-anlv=ul}k9dHUE;r0|XMo@sH3JN(|bF0M|Rnwt~EbsCNcZ-w+cBHf$zmrX|03x-Rd4!$NQcjE`DyM z`4s+(T$W&ne#5Ji@o_mB(VNn`pIh9W(NbH5g~FTXW!tG%vZOPHPR1($KZNwbFWB9 zNYDccH+=(h5hQ#+vx@|5Z+3D=t4(0Ah<((w3+E8V#%xWOT8U=sj^LO1i3Qv+h3+aK zLJwC=Zs$KGrKQ>6(Fg;kr>DuA(~fbXoEO*Dz7kH+Bte09a9)_aGe9F_h78s%K?Bv5 z7B@}-C?udCMDpJVs}f|Xpz#0|9NqG=Z@^u2WYdMy{JG6A51@6Nx4l={82;qe$UMYW zdVhPl7B8o&+ZDvt{o_I`niK};m`u+{a#`|zkxh$w!6Xp0(DYh=?mHSjhbhC4hz86I zK4T-onkEv_H%PFP~woh+AYR*IOIoPXu`PIbH2WqOS`NN`;zT`*|9zGiMSP@R_!-E0zf~J zqlsjNalSY_l8^=^O7KeeH2?jl*OaQ|iC8g8`91>fgpa2yiwNKT8!BNH(L+K&2Krwy z3*odQc_eZ?DJcv9pW-O7Jl%o;ggd%s3L~Q`Xt#RVHeQ!w@#3Y;}`;kWX$MBM1EDZ+kbspWc?=FT{uF53T&5o`w?mK6t8W*@B_twKMNCcW<%ai4kX2=n(9ldK z3J8b=%+?R;Ud!h>ZaixirDrX)+GJ3D0r|5UUdk7oh`{DjQ4}_bZ5>u30;M}P2PGl~ z8$#}Y$*YLxkUQ45^w|1Lz4vsaoR-C!gh7>Yvi4EZi3L=4k-LE33=@* zgV19`gn(b2pWJU$L)-^Z6d|Wx-4bR2<_nhg?QwIFv<@GKwi{9qs=`<97QIvBNug|} zp#ffG6%`eas}=Z*Lo|c+z9WRZt5HNWG=uJtqiSbsd@`?k-y-2_7#vGUp2k~Pm&NL;BiJ}x1b`?4E61HT;2R(rV^2ST; zqo|pxH#GOoL3h!hC;Z)_Klfu@acJ1?VUl^xcjQFAFS+icrE{=?4=&~F{w}`Cx@LnD zg~xk3T}pBtt)7cyc!bDOQuG+&dvL3kX2JjeZ43*Wh;J%r_ZHTYBuB)QZxPxYxfJwZ zV#kq)R8+C{BM9#5NMxGhw*Q8Ae;YkMm;QXb?UDO^)*)bM%~5x#nZ62tUdSIs_Nb0~ zb(~nJq!JsZ*OgtP?5YkYVoE?M>_-s32#?s>{BjpjKsXf}5qiA~gT&*$_OgE~8}_Zq zu@?~1sy0`{@`u-w2_cKu@&Fwc0tyNWR8kM7Ca0Ock&(**PCqy-)1lBX#T@k}=Pnc$ z7w>uS^%k7ZSH3O=>Gc!BLOpLm!{qp9cThQqSWAlsJn)dSw7`ajvg-LcpQ50w^u;dY zksBEqdEEVBMh$ue8QaUPb=Z3z+`$?YEwuRN!-tf`%`4cnp&u+;BU`(q0$19G)JUBk2P z*IP?&=cu1QrQOt2Pp%s5JD6U-ep^42jQVVQcV*kAGk^y|x#!=$z8W{AG@5u}oceAsjU&{XoA@1k z&5}A|3~UM11N<|{^~?-YYo@ZUot!>p@{n#N0KjB!>DZ$Y3zjrfEFxom@P}%DvK)zJ zr!;KX94wMCm@g*ieE@lY$6T|Y%+l&pZQ`%vv%Ynd+uV#%SxK^w@e{4G(v#b*<~1DL zEkNbPEN<5K7+1%pe(sLz6fJpCsQ^O_6|Y6iPVfT0QDi`x3rX9cvOSfvNMt375CrPv%56)ZBFCsl1{T8iGi4w!hw^lH(kL;Wuo$)6bX{g#)p->aDfziF z{#j|*(Be>&!j9?#StL&DqcsKY3_IAtHTMHuxOJu<3LFW}94rg6@c>QN3f)Fu9 zwxK>J(#wfTp89Ol2n*h#+Sz|h^gx&PnYZ!3rOP780JvJ)n;J;2ZT;f6E8<*R?tD2S z?unlzy3WvIry7H@awb|LkYWsCT}wb28XuREl{Sk->>n96j*I^z-9E`=hGv7b4TDM~ z)Y?-ndmM#?v;4YEA#{g_=Wy$N26Db!@evIM*eCy7imBFU56miI+K`0CtmwOMDTApA zqT=1}7Cr29e!mXFV=3Cy?;!PZ+@1JjX13}_dENYSWhIuI*fyQlv9YJu4I6(%;I2Zc zyvI=2R!=V+yG!iADL*r6u^k<+cOQ!0$78vegbzqhk-6S{!e^HV)NeqcXxU7?Ir@s7 zNM3{bWFn9k^ony)n~=~DQ!xs(|F8(!9mI($(kUiWjoFhL+RI6Xp=s&_tRbwcsG$ zbBAVVMkql^SN?(pFZ4Uax3E(%GT;Jq#zZSG_A6tP3rNi(2owDJIpS+-Zh{FlND)Jx zEGdjS2Z8YgKwlV3hrLAU_rutx$7)LQF+<-i3xQr9t5Zqr;lj#1&k-!p z*MZU%IShveZV;T(*|~1Qvkb5de0*e@f>~1+O?2!^fjV&ZUk8Dx9kX^oax(fw@C3BJ zzMGQ9ZgyT?p4DKyGuZx3sG!}fx*{l)J4*x_;4lC?c0K(nGbEe}g&Qpo+TPbu__+9AD+n=17wc2GYjlda;jr|V?ITCAashyoY$wR85qRMoRF_?@ZrZ4|{3c1ay zXqKcJy^*|KcQ}x6N5X6pc=r3;pSbo`>-39_KvAE z!C=5+T)tgsst(z#Yf$oevpm6|BsRdH*B^hGxbl$mBd_skC-imrXO6ry$AnTp;}Ral zd^$&zM;QcM!=7hNGwzD0Dy_NcD4)s1$p)+7s-ANst!=g4>=NV7sSw*FxI<~>KzKT z-r)pf3)>09=D>_%x%&v;a#Fr~#}S*g#9Gu!!3XH^4FX=L#`UYn+jGBJHsMeKz7HSu zm0Q}1Gine%GiN=~L^IK_6`i@yZXEXD$GSLVMhSsS^umQY9G73ZFwfweWC32L{>8(d=dofj$2$L0F_hTz)WPKVYv)-k7m(kGs_?!HNu-gk{x zgps5@3Pz;?;U zyX-zAC|0esJ28xNKc}SOO0m zo##4{7xGlR8k{t+g#k2(Od|Z`9T9Ma7uoEsC4Q4w0^|oY|KQBXbvRnJoVaq9ykyR=Pm{flVb~Ix>>t}EN{PPbh)K2FS`p$I zstiGnpxJ!BnIMWJjA^01v#IjNK0doRh~TmHyG4b+;Uf-!G4Sp6>3Um`kO}q)xTcwz zaBgSj&p32NDFSod#pT8m_e0=n?d+7XHuAI3*MO-Tv`C`(Y5ylJtJ(OJG^HtbURR;0 zptn5V@WXK%<{qZ3#ZDN?&Tir&jDaxPUBiFyok~9~{ZF|dILwumoQa8gxxPz;gPnV* zg2F;twyHn9X^CfO1#u=wNPv5^-18{k#nwuw@wG_O*HxI~JQGV+yc;@6-nxfnk~IJP zZW_tM{+N7H*yF&KR+rF*2@{KG04ms@OvhWmfKIk(fZ!9;$$HBMe&mnF2a$vXOuYOD ziy9--8&WL|v+V5lbN8D6<^aH+DTF`GYmT^*+z96j2$1veNQFP&u;wK^7aUvZ8nTms^$ZD-cROEjR$0V{ z?RBpq-wp%;45EB5vS@ro61+yK z)#zX=^pXi}sysNJPV4;`Z^2`m{Z=Ycgz`j}@>X3|#j}l}D}ti9$;ft<0(k$AK5Cl8 zb@(*uCuvc?HzN>lURjgwFO?IUViF&X`DNpYFbD&|+gE8r^L7JFz%`1Bin?6Pm}Cs! z1GVN9K~G+-Ca14J5_K?xFNsPriaX;r2~>pu)O-A^3|k7Kmb&~I^#s{h^zzqBjw^5c|*fcS2=%dN?}pBmScjq1v1BzqH>2#BB9#Q ziJwnusu^lf_1T3o*I`-HguwxzXl^{KBbo3$sBgW02OTwGr6U=Ne&FONcTmqx3xVS( zSK&gMzS>M11cgAborSX9MTA0t=?eXW0YPq&75WZ$N9t4`2Lg!22R$*W7nQ6FtXO`0IYHAy_ z$w#$$$W>H|QWMxAt5#@i{&kJh|G@&VtDJU_9}fHuS!{8~A<+{1Ff-%SatMP_qk}M1 zRdo~+RrZHb7<{1yd%d2po8(klm)&gb$fmgwH>}uz6EhE#XOR3B?nBl{vDPr(oP>l@ z^nSAe_t@-^5TBXqy{-z48&anKoU@3t)Mpf8K{gj9H!)1i%$q7j3Z@3ajV|(3MfLEozPYxniVpwE-XB~t z7Z(%Mtf z8Soc#;Y3(?d+deO66p4w_{@Ko86D0yIIhCz0r4hd=kp%+If_Zec=y<0uMH-*%&3(e#t(MI+>*@zYY3lHDn#t;N zXPPx?-HjU<7^vCk5c=o+p$0ZK zHaBQKC)EC-ayPFY2LB`+0^;9fo;PO{EG*?#79eeB9zo(rWk4(&-5q67ufUFfnG#E- z^iQ?TDELJO$^nBgJcjCP_(gz^ionl_+IxpeJRN*;LOFuN_W~EUN7Jjp{U~Z~J__WP zm-8?{T)K;rvd_*oig9<~*eZYLfQ}!@v@VNJ{+e6}rnQ*F*=r8@Q^5m8xq{9IbC@Ew zsg3rf!t!Lt)jq6#xKAd8s$IL|`G4GN;(|%`1T-+uC{4vGnse&w@!JcFTh+?7!JQh; z6p#Tn*wfP^E+K)vk@o?Ji3#it!^tZuuAZc$l`|k_nyLr4Qek63CMSQ6FOTqtr>9TR zWINLy1xvJf*#-m{DygW5xVYRpzqzDSQNhJW&ENUWGIr4XltJLW&46W`twG|l`~p8Y zvIduW(W&Z6u$7ye!tcr^v{30UXk?TQ=a$P)Oi;UI0+EBTKxaYO791C$DsxX!Di2F? zvciWzKZu_m?no7MJ2Mn@P8lxO7SSVh{em!|GO%F$h%EMwo>JsDwFiwMF;3Q99Zp z<5^uEr5H_phcJ6Il(hylxVPlI2{GoE7&)Y6o*xygwOs|$w=@|_eaQXutqp;xPVCf+ zgI@-*qSa|>5R`vFD_uPd8+p%siyyuO`Yo3A?$mL$Kd^TX%4m1?tgIbC5=d`2oS5N~Is3 z_Bv;qY1SuzIWrk)erX^&w><$ZUL2>W_=}&kF)t6+zvJ&s+f)Va6%f;m&o^YGrIWdB z^>a@fc}=?Z8o$ukekt4JB}UK3eLX*vWV;kigc>AafA&4UrFY3=yko(+n6)Gbx;yE& zfG6S2x%oci6L;GRq3N8OjtXfCk=1dFfUh7a*q}jZWR-eipQ=5}>^Nt6AKGEyA1+=8 zFAbOf8PI9%E%~sJ>ROccKw2}8ZemghF}~RCX^cQsv}#RheG%=?E*c)DJHtjSK@|`X zqo}-Y$NWMrnBWSUhT6<@U!?)El~7A8J3MCNq3XdepQdch6<=ph*H0y>kAY^Sr=g*x z=sAA*MPe)I&+|z0$qJ_fyO^cy$=<;fP!P5`&h9c2Q#SdH`1$uF!HRoQHkflMB#DC&YP5nhhl^wTG-v9X>A0`{M4 zno9a#2B?oSe0J_!fj^nY`w$4&_Bo*7lJ2XNUqH(F^T4BYLl6F^KN}9N1}FT+D+u?- zPO7YzpxZt^92^{dp`bv9BOF|bpp7tZ!EK7`X?C`7px&p!)UV(7e)9=QUT9FJdz?v|bz6&AF~czD@=P2-c7z}mgwfSz`lKlib}NU;a#1- zG57k@m9lNNFVP+{!#!^4bZJ1$S{Tl;b;xgp)M*G_|YyM{vM#$V$`Wsn{|z zzq1tXF*NjAtj>y_KnrlhCZ`ghVmT3^o;n$(u(eP#K;r+XME0ycKDoZaJycXpn*Bj;fWsKk^tno;B827@U=qjTw8Pwwzdk4Wofi-LCG8xR$; zRaL1s=t4l&_L536j^E)0gKxyr!}GNPmaYhbWas_m&}97IqCKZ05S)~*vFE+d+rpyVL4sZen7r;cWwWGnq0Es(2aBxF;TyG@tKU}SWzys^~ z^B=OW7rH^vg3s$f1ELYQM#whRW*FRwB12!Sytm$G3y9sOB&{Mzz z=3$fp%!=1#(~>RNO^vlPTIp6YulUW||GXPzE+2(VH-8X#$?t)me#dtWiVw-WxDQK@ zr&bTABVAwKNC%ig7qTm=s{Sx9Z|QoyglckiY<|X-FH6i@6FI=bvJR#TQ1kLOs%%wc z@c27Uyf|sSjbd#Vw7@qgZ;EmsGENk_z`y=jC+fYuI?H@Lfs#a&;rYXT=}ik_*@tR?DN{sm{FZIp>kGB7iEkPhxsL=AvS%4)SmEzxCcMMhY!m( zF>W(PjD>}TY4(K4Fn3slB(Gtn*Eijka$DD2iRDq>A=vpC$qpw(?h?d7h+(~;?%?W6 zFEmrL`+FQs&JE*39e;WM+)`E+UTOth9{-*%Kg4}txhRF9gR{{)cF&$lN!$A>Ypb z=lo>L(9w}dL&hc1Lc?*XsS@^yIo*=uYn`@PZ?$o;I)(~@{FJDuLd8Qdi{tHOzqLO0 z8tNh*@4;^kK)d0ilG>~A-nz#Mg+wTm#ss;NYoM^2Jif1jhxoj)hJi%`B>?g(H191R zQ``z*d}(c#X;p7vLpp(XTZ{bttIisUPVR6Lw3_Ts*0onxhwZ?#-*+~dh4|(S&;zS^ zu4`{C3yup@3bd!`Y_i@8t{pagxS6+)fFuS5pZhbMwvqQzTLb zmSWS=OuD>ss{}nOo;pGayr3ve5}%BO_b=V{yIe96vm$KfuV3%3#^plWT@h6>&?b^r zB4OhczIq73J~*txuF=X_r7K&rRoZnx17E{R7@6_V(x)?TC$nazeklFt=YKk0ypB|& zBA9nF?uapoXOF-_Rodd-hWvP!&JDQv?<)*?;X%I_Oqcq+ADGjQ^j* zrpi(+88dNM8G44w#DrIi{ntb@J|~$GA{&0mKK#m$n0z*NUGQueq}`?*-%skuYl4FG z)JOPp?FONyO8tAue*fkYK{V`!p+xjsI?e}FFb!Ja_gSi^GwNju_cw2!#wr z`WTZ}WzN1Rs-AngKK3pgj|eHLusyf-x5ez^njrZ%r^l{#84fwsENDi0M#II=FF_{% zmVh%Xg%$^8{MS1C^vJv&N_Y44TPZ0TffjAT=g&%>`WRADg;nr=qp&Z#h;*-NP|H7u zjAsnD!qdrh`ap`-OP>=RFmPGOBV}c}MVOk$B*N=>KpGlaYpJHWJ#RRyTN^UV+ql@w zCN7cCEOUb)DsaW6uH=)GD*H>1SYkob$wJ<=wzj1VL>ARE6{$n(2lG<@2N^e)Qg*$ z&2FP)-Q@}wc(3?biqVe}3) zUA8GKqJ`e?qP?!jtBCO+-15LzklIlF4D}KjNl#c7O-*UZ22(kK$(c0iwBWcTsNCA* z(@P~YowoImD=^~8W5RlYAii%{dm?iaQM8?kLcJpbc*%g_2Z!z@Xu~^~5u^?DV1Lis zA$y`ifH8l;T&&igx z+A!)Yme6Bkr$JJrD3`H%-6{GVN-jrwl?yGyO}`IwaAxNCWZ-z#T)n*h%i#^JX`+%o zY~obr2qtb%Z>`Yl>N3VXxZ?1fhYB#AZ*Ez<}Y7AI?PdIy!lksp$PF?|;pV<`9 z;+U{)IU7D#GCnS8kCm>~RU5PBp4O5TziQ3ZHV36P%bLpaWRe!vx5_tuef{Os^zc>Y z>$7JiUuBY5>i5bR8rGtoH8vL4?ZYje-I|6C85`(nr2Z^+Igv6B3Sd2I*iw%`PSOaw zA@@X_jOE9|$0Hq{b6)6ipP9kGy$D1g3&`Lj+VD(PAQ{fs%n*NrrKWv&<}{UJGu8)P z=2Xej-?N9~A?8x={t5rN>cW54`RNcEf?3_8K0C3SZX*K(>!$0J6jmeT?(VCpC|xjX zDF6wFmKDfLO_eYG%oI4nCg4eh!2B5dDl^yZ!@#W7*dB`GURW?KDJn``ZA>#>t|OF{ zK9}Z2hGvUf#d&|Mx2kY2lsHN~k5rF$br8-{WJ8xG>)3vGrX)%dB|)J^P}|Eas?xy= zJ(w(*5h3)L0;i_woBQosO3UW#-UB)fea*Lk5AFBu`AjaJen)Sgnk{WOTRh}X&6e6a zDL>575GX9z4(o~Ua>oq_)IwxtX3eKQ$|@anS?$(dYafYtah?RGF6Y^k1l%tw>`erN zF^+mx@B!ce$zgBmCZ-@LUC28%p6(J8+b$qSj4FlWr7wu>D5p%TT4=Mk+E@YfX$a`#PTIva= zT&kBE3*K`ibOqz(-hE2tn3wXni&rltxFLLj2nh<%g=2rhd7x%AuVr*(i-(lMfuoxD zvfegxOF|sK-bUm^Xja>e60)#}wD1hNXJ{XU?Fl>p$XSJmG#aG;GpCG6fP3d<>lV1v40?KcAZ-l- zwPh1BAPx}Ktwx`ad5y8741YsQXper@`#s_jm!X5)q*lXG4*NJ zMyEgs%$Jbp=q|(54v+-iyTu!S*N|HH9@!HZD4VHJ>_tY<_9(!A{!G)AhW`W+lvkNg zS?!KT9}W62T{du|e?P)hB)iFlp(SIWLO+DsV+Ld)Cc6o#5ifM&GW93&V{=jjT?F#$&# zzlzGFvT9MYe@F;xXO~!L_}&j6i096nnrAI%W%E~4Pj1bN+kyfgJZ+yh;mIm0JsNbb zR9w1*6?m`jh2GIlO}Z)0Yd!Xj<*svK@}ekcM=#~@lUj|mnP3D26?N=efC&0@59dzW zmFMgL#o$MKnU>l9vIxGQrcM)VAj?rWxc)gzj1Y1}ub$te02<;dOzJstI&Z%<;W;D2 za(#H?XW!^_0Hjy7LZhR9&poUArxCm2CTd8{{|6RGH+x~leA9WKk#Q|Nh zK$%3=)9F`=qGBnbyIv0icOJ%NA0>f0=RdufpdsVan`VjJ*oYEF)%R%3w(USguaXmART#87_#0WF;SkY^Jd8Iam>{3>-P*p}KrwCZJ~E;PR~IroON;0)zY2 zOR6L#GgoJTg4V-ph;JQI?!B#3vOYgSZ$yFp;!U>aNC_VF<J`d%wvPTjo9P-fk)*9J^?_i@4;ppUOpm%Iv*3lw!)-TGfG5g92z0efi`eu>75D=)&N{WYh^9KB633E4(Ag5T6 zMAb9{Y81BbtBkbIyVN}Cm!CWZo4Zrge6nsvol(_lHLLSfUL2K|uhJoAkYItH z|I@Iit*`_rk3Y z8kRJuGMLZ)lSY%Y+(fe+tDuK;<#gix07qSUGx2vz|DEyLHDEm7a?5>Z%r-)=t|YWZ z$Y36brr)o<-~Q?{)6x7g73xEqwrAGoJTs}Jq`E$s)pv{v=1SG@L4Hfu=jD?Z3(k7u zvu(DDVBlnTV?HPc>?L%f-N$BaRAH|MIG|JJ7X9mCg zO;L)vI_u5uTo&OI*Egpk!SPVFHcAnfo^C>?Yj=)N(z9nVr+U=N@Iu~z>DVW^R23kn zip?zwI9cbB_zB45g35XQkhN_$63lK-26hEXXQDurrMPXqRSdnX+_d}41KqX=lIGGjO5zOjgCVI zP*Kqv>DRN+Al%gJVwI5WYWHQT8c$JYqoIvkDMPJT#shfVIUKsrP$_(BNHJSkm-Fvb zF#QPrrm46(NHOWeDxo#r-j}Cr>?&6Y9`z`0%c@ZP7x+JtwEcR?g2A6OG8GFhRE0|4 zILz$S4XR@^GT)p1E;B?W3VM}iH1=yTjHcrel}!8bzUinl!?|x}T-hX8EtEVe-j$(T#Rix-47sN_@S#d;K*;VVs)$Auu2Y zkdI=`@r-pZX%~qbg#$Mh3#)YpW>prBQ8wKaY1{G<>vd;2L=n)680O4j6nz`od38BsK588_ zgg4^#5!MI+H=mcM{62kD^$b-qYQpAl{&f0)h{r~k6Nr&X$z{nXDi(#rT>OQnJ|Q77 z@4{GM`!WU0_J(0rZ3^x%fIo7eCcHQ>b;NU|Ab*us3Q#ke3cndN)$8%AKhS?h%0T${ z-Xg}#Ya*g3EuyIr^B2t00(0fXM!R*YX$)_~j4?EMt)+0uP_l_4x+Pqsi}Z9R#lpnJ z=XP!aVZ@O^W&N$2y*Vc)5mUMUepuyPT3Nv@?(hgkkFNYt%F?6YyYzxEGyKRR?~v~Y zlPIEM_ttIXS2$132hK&pnu_?QFhR#-=3cSu~r3tLuJ048f4@i-F!_O z^;0^(uE46Pz}>e*h1<`c8kU+5nAkGt+ zY*zeKzvv?Aer=o;c63BD88)nTG^eMMGzKVi2@9?l4CY=J$@nx9Zf6y?$GY@JM!WpA z8741Y^wF}jCtNNL=&rw|5^xiinH{lMYRxb&yMA>HPe1rB!J!+ z6GNkp&o9mRW@b#9$Q_{OofM*@>8KcFtU!N51GfZ#Aq&z@^goBF2Vhg2GZ2$K=FJiP zxjw}^xtO!JW!A3Ro^z%--D26;E!G>vpR76G|1e3^Z|whtvexscC6JhV`G&s&TV2|; zfH#;ST6(;4EVoxd%~87D?b_Z*57iwPu;sVI(z`i zzc!oi?vtFSZJEsqNSqyW1w+9*E^3P-Qi5_mFpAEuxx|3Xv|{X6x99c(bnG}*%?c_% zu>+#sb=ffHtQl|+rU@*J^!YR$`k@i&PO}~jkMrlWP*4=WI&XpFoF(w~R9P(;8z;B1 zE;zuaBl|mA8!9~Rrivyi4UNqXws0Q8t)>blQlY1%sEL?h*^}o2u|PD)=8azU5N2YJ zvS|8{@GbY?eYDOcM-+8`Wu7RF?0qC~{JmD@G~gm=g4OsP=i_yBQPkH^*hmz-<>GQ> z?a&^@p$7Z15nv>f9_I?WN}M{eF0`d^TyLmKTHJhnyF5 zc{4N^fUT`9Br{<7LvfC z3?RU@{75A{oZ+78|| zVu*)-Bo$gbPajH4xa^vQ(f$B)ANzEq!Q6kUhp#pkNn)l)HD3`O>D4G5sGJ*DP${ZO z^G0)w*HN!e4koRta@bKMMz`Ts-Zo!5iUh;^;AN6DW}4J!egp@pvRkEx29phEOJYz- z#_Lma|Lh6C_aIH{=gJT=4Gkf%H!w2c^a&{%mcQ#^zND`M(Q7^LS)C0tcqoA&UgzF7bKuqnJ5@qi6F5@!vA zENHS=U@pg0R@4L0*7m-C;Pi%4G=}PL{qEOuNL(5Ut&%6cni8O|WJaf1nYDQOkt4dX zT9XcDm3LMIp*srIwIV0praca@U~C+DmzQf#+U$KAoHH;UD%u>S7$I3mi;}5o7{&aj zd=B_DSr3MAC-sMotc?zp?4N$Am#9{&SvkugZKtHrf2eY{UYcP<^cy9ecS^g|Q$JU! zm{;rCohnWJX!WO|FcQpU!3joh*xhc0Q8_Pqd5~_w49owYkX~ku=fv7Z#Zk_6EUc{tjXHk4_+xqlPB4(0nAm zFyBnpFt=`v1maFb5uGF)6Ypfxx$BOTf$hbXVk5rn=UM>PS48O*= zR_|VZc#iVC#9}JDnBw-bcs_b4EhAWcW4++8wd}&&VUrW_8Tc?s@R{kQ9Ura#fn!jJ zX_6HSjJ{;_KvfTtKFbVni_(PICl`EpSyeSlSNL%cCN3jm=|$nZMJryhqieOTb<5(z z-&7OpDcgPI$>dyikw{b<1v5)bKqc_GS`IX=5}T{*o+jzzFdq=%Fd6o#9w`MCjr>f% zY7-TFf8b~&*E-?OAHJJn@n0n0Hq6RC3O!!qPEPiSi%;+Tq5;+9Ms_1<(f0Xu1h&>@ z7Hr5bE^0tcfNhh{;@F9ZnY}OT@An;Gwr3U;-tg9R&IrK(_Q|Z(G^N&cLj43k28_Rr zwdj>A+jE<5?w1#pN|+vz>rWmRZ#SMcW2J#o+Nx~tE?OoDAOeAnBl?lU@&fa=TGR19 zShyRVHFCjoOoGB9Jy(B3ObUEV z2=?F9_v!9ys!9LR#QK^i9*AF$4eAc`91Ij4)g&3NXwY!~TC1A*Xl?Q3WQFkg8Wa|J zY3CDN=%0AZZ_E_b*Zld8G?A#;b%l|^T7r9erd~(`${1QmF+*v3H)l5(`XZPCQG?B{ z754egdg_$I0lLC*qyB$cZ5b`1L1U)t<=$F9`HtFk9+B&o4i{{Bc<8D1*84VZ!@^g> zS(dkiE)O*`Jkg*b|6Y-^c}%({SMx`jt4PoE18a#1H=|^64c73E3W^N;PJ69B% zv*7ym>$yJLkl{+_GfGP1lVTmFZsTfKN5TK|zJH9-nvuHVsL#0m%SJ`?%Kr*2j?z{& zV*!{vzzZb#`8j@0Ec@2%!L+kuFC{H~t`PwCo&#*7qyLfj5}~2+5n6*#lTgV=Gfo-|msfC~TzcvAG z9ou8(^sLc&wzm@)qbEC+;V+mP2wnmTAQaieqpW3(jJux~~E z%rPVtV%LFp9(5h-tazXK3?l@S_&*GXw;Ke3`E1QRJY`+w*_Nc3|F>s94G;q5H$6Q7 zR)T#?^gpe@wP6@Vg`CFhCLHor2=L+T=Kk5MPutp7EvPOVYTxP66_m{|{d`?d zm#asVdxXKJcd+;yu{+PZ3fCa3BOwi6@)1A1iAb^lk*FxLdYcy^Yg8ysIW3zVF&GVy z3s7~|r?5G1{`Yd}AlPIWT1~u5IvPctHF?OYy>I>v07}g(Ja09MicO%o$g!JR{C@rv z1Q8|FTJI7wWjE2^df0z5W&3x_Sbp#aFZcC^j#pTPnPAeyu7JK0 ziXvgKDh}<4{=ecWiJFm1+x_#fZ~?V;=%*s{sH?)NepPr&yf5H=g*er`#heAbFp%gw zYl0Pmb?|n3T0&k*1dPril?po=WmO6Jqia)XL`>g~dR35Kz=PgquOz`i&PEO=3Wu8N3K+->E&tQjx?m4wgncD`G* zS46bpzb296Dx4Z5Y>u>j=p}1;0HHMhYY|l7VR$P08+=uj?usbBAaPkUHkdhBtJR<= z;N$w}iO-hzx}vKm#Tm+|MEgEWXb1!S-vT5=5M(HTY?1C|WMc9;k^9dJPwv38xccoZg=;`S|+`&?-__1 zDC!G}{D#t2wuFKKWAdfaa@M1A#<0*00B;$pYOPQ0QMMbul20i z&ok%z&4q6Tc4J3-V@GJH+GT@+P1!fp&vw^M7(Mo%Sys+ouwOz!p^Y2vw@!~k8`ymg5Ar07T=3hsGtZAc`H;|xJM&{JEzg~6b>g?3xt*pG*9!d} zJKsCq@ui>&^J^6Muhx#;nPkL$hfzCdg)G9hJZu&vbQb~qO&zl|Aq5?AaK^9M~+NMVU-J{Z^_cio@<0=13V~afX;(}j*!SmIVUIK zvx1iEog@_A2w5AlYdPK#VaXD%O#wC@=DOmL*&@VziRObM20&vXW`Nc&fFgF>HThOX zzPdW@Q0JAMXBxdt;r&47m;xT7>$3ExhK5;GjVQFKJOE&SyFC~vLGsa&9cP}1h{$fQ z1m>G>rdo~Fwxc~)gRyoyk>CakyS&r2M~_CBE@3li$9S6pGe57G^o|#Pg;aBR+2ilL zY!Zb<6M}UBKD}P5Nr4N8ci0+kr~Q?tohHe{`soCZj!Pe^KS9OiTWAZ3y=H;?zFfzS z22ez`izZz*=CsNTV&~2Em6S-KkU0RoOLc4SzMOS{X1tm)geM&OstjGyM3F^Csl<2W ze|vLRC_h@>8*BLN#{&o*kEl-L9c_77&r!Or5{d2ekkV;-`2R4h}V4tm%2rPDTLd4z1HLTW2c& z%jXP62Eec2{0S|kHMx}Dfe?_NfoU*2aJxh33(`}h%8KJ@z>UFm`-T08#GBs zCv)EP4yT+tcq=~q(PgO~PBt9eEfP>4vWtQQzx3`y7k3!Up~$wpU&VBK2sQ!Sun_<3 z_VD)h_NSot0h>!Oro&P)<9?98KdDNB1oSf9Pj-Uh@eX6fiW}NC3}gf^jbe|ADq^`O zc^5ydFq9k=@#7R426+Aai9V;J+^M0NQc6H3^zP25o;|yka%fMm4$!`nrB8oJ`x60E zM#?JeqVu)FC)%cq(Ak?BCeNeYJE0eniD>IV<*)5j7mUJ)GA z0RX^;mnX1Qf>0z90G$)q>gw8qP2EUt1AW0Ia;OLgQu3{#+b8P@n0^9E{XSZ-4lp|s z)mrqzT0X;Aq$a*jF&Sgyi%^Pz9fed4gPtx!BN?N(^UMnGacRMr$cg?_UE<UKcm9JBl)vw9mes+OCZCxZt0JdMn0P}d?$ogpSR83_5)70uEb9fe)*q~OrB{mKW_eElohuhX0i3x8B^{PH) zfB59KRUx@J;rnp@PKI)VDb=5dAkOYYAfpyERpohOhR;D3|Jlehf)RI7QkpLJQriIm zLe`&*m#iuN#ucb{lqdF(3!@}tzSE4%@5Puc+K>Lkce9tx#;v8KJx_t8W%X2qgjj#& z%6AoJW%u&E9`0V8Ym*}_^=rHJJ6E6nknoOWo-?tqea`gc>(@gA-nU^l(KLY^t(upG zX~%FfSY((<_OlF?RhQ^RPi>|s{+g}NSw!p5`H>f+rFUE;~odw0c)XIl=4#kW;8s_KZ9G?t|h?;C5@84@c z;&6KI2a1jlnQ8Tl!B&xJNBe}~WrY%gxDZwW*#>&Ze^Aj)5cxI;3jdA28SGBny!EgF ze`-pniu%g^+sC;Ut5p2}i$YVuNdA3tp7ia9zpw~?J(@uo3HX%LoekS7@j)D1S}$a= zXhIbiIXnf-!qX>{gTkjLTQ5x%G=vlpV-kV(Fb`oHWLBJCl`e5Y|)guA(w^upou^T=7Kxy%*ReZO%yma0$U_LCp!fU} z%o4b!yR$th#_aF3wxARzLY>|3Uy!>u4a!sFspV;@=zpp?DH!e zyj-Mu60YY1wOE=O;^I@sWslW1^YrS6W=g(h;67A)AyqRfgruwJr8S$UH2Z)$89s#j z1vl^_etlz#jxO7ctnQn-xmSo@NV}K+TswGNbT;zIIii-GccX3+hV~C6rx#YguMIWC zZ@G8um@Br>3tYe%wUg-zWj_`X-(=K0sj1&Zqa+Xsg;$R zNy!CjB3mQ33s%OC7&yV>T5O#{4%0U%`o;Ie7CqsaeChV2%d+czmmPzuI9q${;Eu@D z6v;-PMiHxCe@hP?bcOaF9WJqq?mA3zdBq1Rtp9xGR!}2)+KQI-ydWPTe(IpUnPkn} zWRT3@^HFEBne(fPVb|+m1o=1eg-M#Pq8FvaUdFK$1WWIEuJCpCU7}Iy7py(m=S>vH zL=!}76Sldq<8^)Pb3&F@bab!nR77leL~4dE3<&C?u8oKg9xCsCw{lf~`v5s}((jRv zJZp-`)HJuB@cHiErZ^LqttCn2vawo4Jc5^bqqICyA$7h>4pRhd-^mx|=Sc$@BT6k# zP%)epd}&xnckQyKb=x}*Jsah{CzLI(4?ao+7F2uTetzt1H=SYo#Ql18etH!_;~4|j zFuJpXUtDW#I%^cmP?d@X?i>ul&(Xtvr}dW1OidTgr(N<Q*iKBv5M|I|Td^F861$Rw5EgYkhM3`_qSCVX($qrGD50pGS@5ialQS!c%pcEF zm6i6(uAKhv4OPj}4UePj4?}N4@~FAk$5h;uY}RH6kLI%Tw%VuXb;4mh9iva)ml>r3 zeaDBHeXM0*B60Ne_YEO%dZZcl|;XOAmvjxD?WQ^f4 zF+*1E?cG}q76Ad6Epn#CYVGN`!Hp!gk;t$HoLq~&EnhjIfn1URM;K zuNx1GAEO|@D1}&$uB^;sqPf)*!on4b3fAmt*|jR>TBU}DE+{DTu+vDz2YG=I3!*S8XKZW?X7spWkIZemv!%Ev0 zww}qLTcQrsGtow-{X5NyxvyUnpWlKAO7>FAvJzV%FHAmrG$D5R7$Gp5O6*E=-H9LEn2lF`_*ZhZA*Z} zV^4RcFUxNJdXY|Qs^Zv=N``fugdUyPH+yoN&wRweH_%b*r-$PdEL+?IjZNofRac0( ztfqzTh7NGrQq(9VzL@k*!BsSy5)!nnB3}uYR#WrbT$mktb>nR(JMHQ4_4gv8{mS>! z*4NpF3S>C11iG8+>&Kg#4rC=I-L$j&N=ce_QDDOx2(PKt@1n76FNoJ78=lewfh=)#0|{ z@+Kx3gwjjPyY9iHLXE3ra#E+F$LtA$7mkDhIM28#FfZW;y4YEzGoRU$k*H!{*shvw6gVW*X7%K9QO`)IydW>#J=sUP5rd} zU`#7?du~jUA9o-c*5l1bsEl}qDbWHf#*+toV~M?)hHZqS7gQDw9uc0?A0rQa0gIq$ zBz)C4j8BL_&_pTTkk)MCIl+BOd7D4DvQj*q{FCZyK0a9%Id2l`*FieY&oP7x3d$!AI2|3vTIB<6bB&#rYL>@TC2>9r)`lb0G)vH)2H)dYObs@`1+^mHnPMsr+$*sP!)L|e8k%?6-DyM zfor0rJ4Hgfu4EO6WYxu=c>FFhvMW4aKiHhsQMyU3U^#ri|MmAz?hlxnbWrt#3g_2` z7ZKZ?EfT!>L8K!Ns~I}d_52`S{8W^VMw*vt3z!^{zZfDrgFvvpUsOWiAS@ppJBB!= zkTd3kKzzKkoq>xyb{z*7u`D8y^-F|4=CE(F>Ei2OmI~#<<35kK*ZgOBzAWry~k{N;cFquq@I7J9I z_&C3var24$Fax3}{#z8hT>(O|l(hO4=>iKL`7*rJC$Vj{oOQh!+lHey_KfY=HD8ajosKbykw|OyXA&`s2$8b5h4KmQ^%uvhj2yGQDr=9D^ zadC$A@DR*5voK@`M28A9#qIxySgMvv=SCgo|3oZWU{5fXxCmyylN5+oF2DVqR_LcQ zmAkBZsf~?O0KQW=D2za;NaKVy1J4kK;sxBaycyZUESW-$JwHmPHnH55$KMR`B-jS> zJcQS#mb+D8kfv|dyQ2gi3NoiBk;)5yY?Mh3z%GhanmqoCg6#HmZg-4i- zz(ERWYicrQUTf?bDxE?g9Pxm3pDVJmh#}?((PYUi$t}@R4_IA`0vwA^`qK!6(D7ea zU@?+vQ|$@7Ygh(oA%p8+aww_zc+dz&B3Ky ziKa7|?kIA}d-4@dWKE5O@7+sTFdwcV`iC=nzby@d*>ii_fB`Z7%vF<(7wC_)l3%z5 z?9;&tt^SRF-=1+l12?!0*ArQLbl?-3HrFm_r6ygk84<;;$xM+yT2em#1@{;N8J0S< zh(LG)t0KbN)E-X~D{6^2_o>OqWYf|8iYBjQEaHMdAqQ8~j(_BXxMS#bPaARp{nq@r2XD-q=PC}+HqOoR_Fg^ z)!Mi`74ivSsr==YP#h(`9|FNBkAwS>FDr{PJfZG}dO*rR>A)RDf!0m~;1{t=+?&(- z^f)+(@h7gDILrROnbz-dI4z>s8y@ZS25SRz%o)Iwo&2RTzoO(HLMk8P*oR^}ed5n~ z#S`JD7W2=>n5D#tb}m+ga(OMe$`6$)o0^1RBkK$QEb~|ByPZc0itl*LyHgf{)K){X z{;#|P$rWRxAe`A7Gq`Ayg>qe^rlM5(N2At?rI_EZuKGA1Y+#kGtoDmN9>P%rt7K9- z|Dj{4$?^;W{KZ*`i7KM@Z4=kmX;elnLz8P6@AV zWNAmkH%kQ4@%kTaYkJmEed_$&+@=C&U_o)Yh0_*jn_I!Cvj*>zal0oEm}q~SJ2@=X zu}bIR(6_v%$aSU8Dv(QC}w**TYn3ti#NogG!{*?SLA z!5aTE_W#&P2yEe}{3YlS0)aI9`|)o6A76sK)!(}|U5#|$!Nr9O|6jUg3VBI!@eImk zsXzFf4B!r=u1q$W!Qk4`DlGAI!c431{LEqK_)v*i6eC1X;2%@()i{@+JL0kj2JW=S zi4NLt!Ig-?t0Z@rzE?XHS=R0tiUWv244;uo%E8OCmbcy9YBn*$>FSzg8o|wx;*`v8PXJ7qPTbkn zS1s_N4SThShE#9|WnlRP=dL!iYy}LUFH(DCXC%mY&0~88jb*eD2*MImo`4 zr--|Xy)0^wopA%q#v8f0xeR$586Q4qKYW-7NDZmOR&_NEljRviASA;=`l6!mUi8ZY z5=Z*Wm-r>6rE(>8FvTP^1%SslotQrLi!Pk|ONBoKfgSd4Gvs#qdU}!o;h{_oqbj}u zAtr4SKziH~$Ak4RD3*+R-mn9d0@Kx>EisZFwPXw;Df|1bSfT@WqKch2OtrJDU*co_ zCCP8Ocai-^zXMtbm}P+4@ZPy=un-%PLIi+n|Ye*WnRXla82$+Yqx^ z8UOuceF6d(;PP!GE03Yee_GE(^XgWARotCc!4t9{SFk`=UUj|~N z-+J@sW9Y}XnEJoE_*X4q|Nd7GPrDEi!@>)7 zJtn^`z~3)o)fW3I8lPc-j!eE?;!z;euNq@%;WmE$W>VPO{WlAM%UWOjKj;P3U{-ea z)Y^_nO@DvCY%UbTb7zAreEbv9Fl-scfD-NGw7TP(m6NkzI|_wKot1~X^ATZT$#8Df9wTsz3G^*}{&2xa zvKNkj!=s}!A)PhjNx*oUhh`xkWt89saS1Qz3WunuuC{?&NvXx~K=z03`N|mSyLa!V z@5xF_OZ(bYt;HX~3}}8Ew9*tB&B@L#pPHKLJP&1H2X@Z=1G%~a4vY5#Xs%wLotr=YzLWn5 zmc9-MNQEu!?E=|%a(RI;=s?j$p1{es0%il>7VnKXZ}e*iObVeBeHnR6N=noK123}K z8Lqn^AkYulu?FlJwc*BGzu`<*Vp@KF7rm(cR;-Iy%M! z9}KLnS)t6ahx9g!71J>_P#pH%Y%}zGkx?uxHpT$j9}FS;W(Zhg*ri_J;%YY^F6Am@ z+>;ucHN{`Rb4#zR9J;KN|50pf#0CwZCxYBCfEWOi1%awhzedUqJeGW=O36(01Poc@ zGkR#Q>y#~tij0f_WVg4FUeuYp>q>Hlxo&TCNu>%^R=dH)XTR_*1G5kLxNf1q$B?>R zQ5aqvtqSOz<|7whzuh;vRK2ExZR71785ybKW)8%R)k&i3mtcH*`+I4xHaJW>X|gwy z9HivzTzR+HSgp_IiJ@T(6X2+-1sMQOR(4zsA?p`XnsBr~ja^gYS*sgEFY4dZ@)`Pb zT)DE?9z`vv5>;RAw5Ft@(m|V$EV{JgLvkht5>7>sa#R?G)Pxf3BOp!Kakn~jdu0~p z#~F({uV>JT*k)9KdazhvNs~>RPhP%MIiJUO2F}8MO6*Cy{-De`_CNa&zj|-tO_ivF zd4r>#JiV&*nk6??34=XLEp_#zTii98fP5BIZ1F8$x{)RVKuyHuL5~CJg_Gcw3#{~| z=wFxAc-6l@T=td3iiODZJDc+ucy!_gZCTz3+YSbVquX1#Kk_a5~_+of!s^9-Ua04mjnd`R~u!Le*6J{%%Z3CDM ze#hkpscC7Z-|`<;PRG000<=0J}J$6?D{Tg=k-CAL5WH41$afdQVU1j1amkuR2!~^>et?l!lI&x_;?i+ zJ?k_qYhWw~cD;c0!U6OI6Ui_D$vlfi*#9OTMo8tS*aXmT4y>by^SW{R ze0+5c0U15bny~7j!^z>kOpS=x*t2%+ceTgXDk>_T)BBRDsDlHX`Wj{S4+g5k<- z7nmf+st9;iCEdB+(?Tjwh9Mt2%n*rKJS z-GA-}?uxC%Aj)R0^5}4@uD`FZ?_8`-Cr5~~cAWE7OIR$3LFM+3H;)aw!193`ObT+w zh=VH$8HP;mnG%1uj|}Nw$8En6>~>IT-%o^~7c*QIbif>6Av{IBI{;$qKPhAgA+s7r z5HRMkhRVdkq6B!g_T`Mh+c#G#oA0~tOofy}1P8u>M&N{%vU0#P`lkKWs5<}R{=vbs z37tj|TTf(6SI_5FDcReX0t}4QSRGR6B;m0~DmbD>9rS#T%i}8P`uD3HpQYqhRD1`l ztLUAzX?gKONJ8^VBy{rg^WPZ4mg7tc1!m^v1{Ax2%QXbong)@opuxQnAwB#!D91^L zCyPOeOGxQr_4FC8drz>=Px5djNI}<;xTnHqI({nKkBMAt83`ATcpM#E9el_%qpR2Q zo)7E}tZ$JQRLk(_@Su@q)5{SI&jS$Kc0kZ>JFP1A5JHef@{^k{oYv6MFwda~G_&Y& z6WQ8&uWBqr?P2lpmikUp5x;#uIK8%a!o2|a)(?d{L!@V~wE@#R=q267=7g7E23|9{ zx|X(fEZeO}8Z3>Vc%{rBp4|df{ax&6e+@lYcUXd5rTwr48qA1br{q!f4#Z;tR~upe zPgUIEc3&6ZIxs0cgxa2<{cTMN3 zer+!qTsm&lIu3xQRoJ)&DiR``fSdsPKI}iXBnV~zVicZG4IPjlPpOahVNRFfNcl5E zu;GSK7=XrlCYZpa8pHHZx8iv!tPwzPzf`X@o=r zV3*L|5hvOX&vfClLWxZZsq`(m@josixAu?D=PZ6w6^kMrwoBsFu42mzwHv_ zaIYJ?Z`&<wy`x4D-Y-xF|H(gcmjGk?XqN=JOEiJ7fw6CFS_ZInl$r$FAT}Pcd1+QI;fuP|n zI4$f``J7gttU=}3#>TBpe{8U4u@+_}k-%-L2f=A~1Su%z0Jv5YEM{$3Y^<5Oy1J63 zWnMJDN$A}gXS-i9YS800TKA{K*l^#O=u9GZD`kBGj)vd|b~V^P|NrCv4K?;Xj*n<; VPR5{Mj4=YCD64WK@A`um{|BO`cs2k4 literal 0 HcmV?d00001 diff --git a/projects/microb2010/mainboard/cmdline.c b/projects/microb2010/mainboard/cmdline.c new file mode 100644 index 0000000..7c3ec9e --- /dev/null +++ b/projects/microb2010/mainboard/cmdline.c @@ -0,0 +1,172 @@ +/* + * 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 + * (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: cmdline.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "main.h" +#include "cmdline.h" +#include "strat_base.h" + + +/******** See in commands.c for the list of commands. */ +extern parse_pgm_ctx_t main_ctx[]; + +static void write_char(char c) +{ + uart_send(CMDLINE_UART, c); +} + +static void +valid_buffer(const char *buf, uint8_t size) +{ + int8_t ret; + + /* reset CTRL-C for trajectory interruption each time we + * receive a new command */ + interrupt_traj_reset(); + + ret = parse(main_ctx, buf); + if (ret == PARSE_AMBIGUOUS) + printf_P(PSTR("Ambiguous command\r\n")); + else if (ret == PARSE_NOMATCH) + printf_P(PSTR("Command not found\r\n")); + else if (ret == PARSE_BAD_ARGS) + printf_P(PSTR("Bad arguments\r\n")); +} + +static int8_t +complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize, + int16_t *state) +{ + return complete(main_ctx, buf, state, dstbuf, dstsize); +} + + +/* sending "pop" on cmdline uart resets the robot */ +void emergency(char c) +{ + static uint8_t i = 0; + + /* interrupt traj here */ + if (c == '\003') + interrupt_traj(); + + if ((i == 0 && c == 'p') || + (i == 1 && c == 'o') || + (i == 2 && c == 'p')) + i++; + else if ( !(i == 1 && c == 'p') ) + i = 0; + if (i == 3) + reset(); +} + +/* log function, add a command to configure + * it dynamically */ +void mylog(struct error * e, ...) +{ + va_list ap; + u16 stream_flags = stdout->flags; + uint8_t i; + time_h tv; + + if (e->severity > ERROR_SEVERITY_ERROR) { + if (gen.log_level < e->severity) + return; + + for (i=0; ierr_num) + break; + if (i == NB_LOGS+1) + return; + } + + va_start(ap, e); + tv = time_get_time(); + printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL)); + + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + + vfprintf_P(stdout, e->text, ap); + printf_P(PSTR("\r\n")); + va_end(ap); + stdout->flags = stream_flags; +} + +int cmdline_interact(void) +{ + const char *history, *buffer; + int8_t ret, same = 0; + int16_t c; + + rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer); + snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > "); + rdline_newline(&gen.rdl, gen.prompt); + + while (1) { + c = uart_recv_nowait(CMDLINE_UART); + if (c == -1) + continue; + ret = rdline_char_in(&gen.rdl, c); + if (ret != 2 && ret != 0) { + buffer = rdline_get_buffer(&gen.rdl); + history = rdline_get_history_item(&gen.rdl, 0); + if (history) { + same = !memcmp(buffer, history, strlen(history)) && + buffer[strlen(history)] == '\n'; + } + else + same = 0; + if (strlen(buffer) > 1 && !same) + rdline_add_history(&gen.rdl, buffer); + rdline_newline(&gen.rdl, gen.prompt); + } + } + + return 0; +} diff --git a/projects/microb2010/mainboard/cmdline.h b/projects/microb2010/mainboard/cmdline.h new file mode 100644 index 0000000..f80b9b3 --- /dev/null +++ b/projects/microb2010/mainboard/cmdline.h @@ -0,0 +1,44 @@ +/* + * 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 + * (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: cmdline.h,v 1.4 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#define CMDLINE_UART 1 + +/* uart rx callback for reset() */ +void emergency(char c); + +/* log function */ +void mylog(struct error * e, ...); + +/* launch cmdline */ +int cmdline_interact(void); + +static inline uint8_t cmdline_keypressed(void) { + return (uart_recv_nowait(CMDLINE_UART) != -1); +} + +static inline int16_t cmdline_getchar(void) { + return uart_recv_nowait(CMDLINE_UART); +} + +static inline uint8_t cmdline_getchar_wait(void) { + return uart_recv(CMDLINE_UART); +} diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c new file mode 100644 index 0000000..107606e --- /dev/null +++ b/projects/microb2010/mainboard/commands.c @@ -0,0 +1,219 @@ +/* + * 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 + * (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: commands.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include + +/* commands_gen.c */ +extern parse_pgm_inst_t cmd_reset; +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_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; +extern parse_pgm_inst_t cmd_stack_space; +extern parse_pgm_inst_t cmd_scheduler; + +/* commands_ax12.c */ +extern parse_pgm_inst_t cmd_baudrate; +extern parse_pgm_inst_t cmd_uint16_read; +extern parse_pgm_inst_t cmd_uint16_write; +extern parse_pgm_inst_t cmd_uint8_read; +extern parse_pgm_inst_t cmd_uint8_write; + +/* commands_cs.c */ +extern parse_pgm_inst_t cmd_gain; +extern parse_pgm_inst_t cmd_gain_show; +extern parse_pgm_inst_t cmd_speed; +extern parse_pgm_inst_t cmd_speed_show; +extern parse_pgm_inst_t cmd_derivate_filter; +extern parse_pgm_inst_t cmd_derivate_filter_show; +extern parse_pgm_inst_t cmd_consign; +extern parse_pgm_inst_t cmd_maximum; +extern parse_pgm_inst_t cmd_maximum_show; +extern parse_pgm_inst_t cmd_quadramp; +extern parse_pgm_inst_t cmd_quadramp_show; +extern parse_pgm_inst_t cmd_cs_status; +extern parse_pgm_inst_t cmd_blocking_i; +extern parse_pgm_inst_t cmd_blocking_i_show; + +/* commands_mainboard.c */ +extern parse_pgm_inst_t cmd_event; +extern parse_pgm_inst_t cmd_spi_test; +extern parse_pgm_inst_t cmd_opponent; +extern parse_pgm_inst_t cmd_opponent_set; +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_rs; +extern parse_pgm_inst_t cmd_i2cdebug; +extern parse_pgm_inst_t cmd_mechboard_show; +extern parse_pgm_inst_t cmd_mechboard_setmode1; +extern parse_pgm_inst_t cmd_mechboard_setmode2; +extern parse_pgm_inst_t cmd_mechboard_setmode3; +extern parse_pgm_inst_t cmd_mechboard_setmode4; +extern parse_pgm_inst_t cmd_mechboard_setmode5; +extern parse_pgm_inst_t cmd_pickup_wheels; +extern parse_pgm_inst_t cmd_beacon_start; +extern parse_pgm_inst_t cmd_pump_current; +extern parse_pgm_inst_t cmd_build_test; +extern parse_pgm_inst_t cmd_column_test; +extern parse_pgm_inst_t cmd_column_test2; +extern parse_pgm_inst_t cmd_lintel_test; +extern parse_pgm_inst_t cmd_pickup_test; +extern parse_pgm_inst_t cmd_scan_test; +extern parse_pgm_inst_t cmd_scan_test2; +extern parse_pgm_inst_t cmd_time_monitor; +extern parse_pgm_inst_t cmd_scanner; +extern parse_pgm_inst_t cmd_build_z1; +#ifdef TEST_BEACON +extern parse_pgm_inst_t cmd_beacon_opp_dump; +#endif +extern parse_pgm_inst_t cmd_test; + +/* commands_traj.c */ +extern parse_pgm_inst_t cmd_traj_speed; +extern parse_pgm_inst_t cmd_traj_speed_show; +extern parse_pgm_inst_t cmd_trajectory; +extern parse_pgm_inst_t cmd_trajectory_show; +extern parse_pgm_inst_t cmd_rs_gains; +extern parse_pgm_inst_t cmd_rs_gains_show; +extern parse_pgm_inst_t cmd_track; +extern parse_pgm_inst_t cmd_track_show; +extern parse_pgm_inst_t cmd_pt_list; +extern parse_pgm_inst_t cmd_pt_list_append; +extern parse_pgm_inst_t cmd_pt_list_del; +extern parse_pgm_inst_t cmd_pt_list_show; +extern parse_pgm_inst_t cmd_goto1; +extern parse_pgm_inst_t cmd_goto2; +extern parse_pgm_inst_t cmd_goto3; +extern parse_pgm_inst_t cmd_position; +extern parse_pgm_inst_t cmd_position_set; +extern parse_pgm_inst_t cmd_strat_infos; +extern parse_pgm_inst_t cmd_strat_conf; +extern parse_pgm_inst_t cmd_strat_conf2; +extern parse_pgm_inst_t cmd_strat_conf3; +extern parse_pgm_inst_t cmd_strat_conf4; +extern parse_pgm_inst_t cmd_subtraj; + +/* in progmem */ +parse_pgm_ctx_t main_ctx[] = { + + /* commands_gen.c */ + (parse_pgm_inst_t *)&cmd_reset, + (parse_pgm_inst_t *)&cmd_bootloader, + (parse_pgm_inst_t *)&cmd_encoders, + (parse_pgm_inst_t *)&cmd_pwm, + (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, + (parse_pgm_inst_t *)&cmd_stack_space, + (parse_pgm_inst_t *)&cmd_scheduler, + + /* commands_ax12.c */ + (parse_pgm_inst_t *)&cmd_baudrate, + (parse_pgm_inst_t *)&cmd_uint16_read, + (parse_pgm_inst_t *)&cmd_uint16_write, + (parse_pgm_inst_t *)&cmd_uint8_read, + (parse_pgm_inst_t *)&cmd_uint8_write, + + /* commands_cs.c */ + (parse_pgm_inst_t *)&cmd_gain, + (parse_pgm_inst_t *)&cmd_gain_show, + (parse_pgm_inst_t *)&cmd_speed, + (parse_pgm_inst_t *)&cmd_speed_show, + (parse_pgm_inst_t *)&cmd_consign, + (parse_pgm_inst_t *)&cmd_derivate_filter, + (parse_pgm_inst_t *)&cmd_derivate_filter_show, + (parse_pgm_inst_t *)&cmd_maximum, + (parse_pgm_inst_t *)&cmd_maximum_show, + (parse_pgm_inst_t *)&cmd_quadramp, + (parse_pgm_inst_t *)&cmd_quadramp_show, + (parse_pgm_inst_t *)&cmd_cs_status, + (parse_pgm_inst_t *)&cmd_blocking_i, + (parse_pgm_inst_t *)&cmd_blocking_i_show, + + /* commands_mainboard.c */ + (parse_pgm_inst_t *)&cmd_event, + (parse_pgm_inst_t *)&cmd_spi_test, + (parse_pgm_inst_t *)&cmd_opponent, + (parse_pgm_inst_t *)&cmd_opponent_set, + (parse_pgm_inst_t *)&cmd_start, + (parse_pgm_inst_t *)&cmd_interact, + (parse_pgm_inst_t *)&cmd_color, + (parse_pgm_inst_t *)&cmd_rs, + (parse_pgm_inst_t *)&cmd_i2cdebug, + (parse_pgm_inst_t *)&cmd_mechboard_show, + (parse_pgm_inst_t *)&cmd_mechboard_setmode1, + (parse_pgm_inst_t *)&cmd_mechboard_setmode2, + (parse_pgm_inst_t *)&cmd_mechboard_setmode3, + (parse_pgm_inst_t *)&cmd_mechboard_setmode4, + (parse_pgm_inst_t *)&cmd_mechboard_setmode5, + (parse_pgm_inst_t *)&cmd_pickup_wheels, + (parse_pgm_inst_t *)&cmd_beacon_start, + (parse_pgm_inst_t *)&cmd_pump_current, + (parse_pgm_inst_t *)&cmd_build_test, + (parse_pgm_inst_t *)&cmd_column_test, + (parse_pgm_inst_t *)&cmd_column_test2, + (parse_pgm_inst_t *)&cmd_lintel_test, + (parse_pgm_inst_t *)&cmd_pickup_test, + (parse_pgm_inst_t *)&cmd_scan_test, + (parse_pgm_inst_t *)&cmd_scan_test2, + (parse_pgm_inst_t *)&cmd_time_monitor, + (parse_pgm_inst_t *)&cmd_scanner, + (parse_pgm_inst_t *)&cmd_build_z1, +#ifdef TEST_BEACON + (parse_pgm_inst_t *)&cmd_beacon_opp_dump, +#endif + (parse_pgm_inst_t *)&cmd_test, + + /* commands_traj.c */ + (parse_pgm_inst_t *)&cmd_traj_speed, + (parse_pgm_inst_t *)&cmd_traj_speed_show, + (parse_pgm_inst_t *)&cmd_trajectory, + (parse_pgm_inst_t *)&cmd_trajectory_show, + (parse_pgm_inst_t *)&cmd_rs_gains, + (parse_pgm_inst_t *)&cmd_rs_gains_show, + (parse_pgm_inst_t *)&cmd_track, + (parse_pgm_inst_t *)&cmd_track_show, + (parse_pgm_inst_t *)&cmd_pt_list, + (parse_pgm_inst_t *)&cmd_pt_list_append, + (parse_pgm_inst_t *)&cmd_pt_list_del, + (parse_pgm_inst_t *)&cmd_pt_list_show, + (parse_pgm_inst_t *)&cmd_goto1, + (parse_pgm_inst_t *)&cmd_goto2, + (parse_pgm_inst_t *)&cmd_position, + (parse_pgm_inst_t *)&cmd_position_set, + (parse_pgm_inst_t *)&cmd_strat_infos, + (parse_pgm_inst_t *)&cmd_strat_conf, + (parse_pgm_inst_t *)&cmd_strat_conf2, + (parse_pgm_inst_t *)&cmd_strat_conf3, + (parse_pgm_inst_t *)&cmd_strat_conf4, + (parse_pgm_inst_t *)&cmd_subtraj, + NULL, +}; diff --git a/projects/microb2010/mainboard/commands_ax12.c b/projects/microb2010/mainboard/commands_ax12.c new file mode 100644 index 0000000..804e084 --- /dev/null +++ b/projects/microb2010/mainboard/commands_ax12.c @@ -0,0 +1,375 @@ +/* + * 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 + * (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: commands_ax12.c,v 1.3 2009-05-02 10:08:09 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" + +uint8_t addr_from_string(const char *s) +{ + /* 16 bits */ + if (!strcmp_P(s, PSTR("cw_angle_limit"))) + return AA_CW_ANGLE_LIMIT_L; + if (!strcmp_P(s, PSTR("ccw_angle_limit"))) + return AA_CCW_ANGLE_LIMIT_L; + if (!strcmp_P(s, PSTR("max_torque"))) + return AA_MAX_TORQUE_L; + if (!strcmp_P(s, PSTR("down_calibration"))) + return AA_DOWN_CALIBRATION_L; + if (!strcmp_P(s, PSTR("up_calibration"))) + return AA_UP_CALIBRATION_L; + if (!strcmp_P(s, PSTR("torque_limit"))) + return AA_TORQUE_LIMIT_L; + if (!strcmp_P(s, PSTR("position"))) + return AA_PRESENT_POSITION_L; + if (!strcmp_P(s, PSTR("speed"))) + return AA_PRESENT_SPEED_L; + if (!strcmp_P(s, PSTR("load"))) + return AA_PRESENT_LOAD_L; + if (!strcmp_P(s, PSTR("moving_speed"))) + return AA_MOVING_SPEED_L; + if (!strcmp_P(s, PSTR("model"))) + return AA_MODEL_NUMBER_L; + if (!strcmp_P(s, PSTR("goal_pos"))) + return AA_GOAL_POSITION_L; + if (!strcmp_P(s, PSTR("punch"))) + return AA_PUNCH_L; + + /* 8 bits */ + if (!strcmp_P(s, PSTR("firmware"))) + return AA_FIRMWARE; + if (!strcmp_P(s, PSTR("id"))) + return AA_ID; + if (!strcmp_P(s, PSTR("baudrate"))) + return AA_BAUD_RATE; + if (!strcmp_P(s, PSTR("delay"))) + return AA_DELAY_TIME; + if (!strcmp_P(s, PSTR("high_lim_temp"))) + return AA_HIGHEST_LIMIT_TEMP; + if (!strcmp_P(s, PSTR("low_lim_volt"))) + return AA_LOWEST_LIMIT_VOLTAGE; + if (!strcmp_P(s, PSTR("high_lim_volt"))) + return AA_HIGHEST_LIMIT_VOLTAGE; + if (!strcmp_P(s, PSTR("status_return"))) + return AA_STATUS_RETURN_LEVEL; + if (!strcmp_P(s, PSTR("alarm_led"))) + return AA_ALARM_LED; + if (!strcmp_P(s, PSTR("alarm_shutdown"))) + return AA_ALARM_SHUTDOWN; + if (!strcmp_P(s, PSTR("torque_enable"))) + return AA_TORQUE_ENABLE; + if (!strcmp_P(s, PSTR("led"))) + return AA_LED; + if (!strcmp_P(s, PSTR("cw_comp_margin"))) + return AA_CW_COMPLIANCE_MARGIN; + if (!strcmp_P(s, PSTR("ccw_comp_margin"))) + return AA_CCW_COMPLIANCE_MARGIN; + if (!strcmp_P(s, PSTR("cw_comp_slope"))) + return AA_CW_COMPLIANCE_SLOPE; + if (!strcmp_P(s, PSTR("ccw_comp_slope"))) + return AA_CCW_COMPLIANCE_SLOPE; + if (!strcmp_P(s, PSTR("voltage"))) + return AA_PRESENT_VOLTAGE; + if (!strcmp_P(s, PSTR("temp"))) + return AA_PRESENT_TEMP; + if (!strcmp_P(s, PSTR("reginst"))) + return AA_PRESENT_REGINST; + if (!strcmp_P(s, PSTR("moving"))) + return AA_MOVING; + if (!strcmp_P(s, PSTR("lock"))) + return AA_LOCK; + + return 0; +} + +/**********************************************************/ +/* Ax12_Stress */ + +/* this structure is filled when cmd_ax12_stress is parsed successfully */ +struct cmd_ax12_stress_result { + fixed_string_t arg0; + uint8_t id; +}; + +/* function called when cmd_ax12_stress is parsed successfully */ +static void cmd_ax12_stress_parsed(void *parsed_result, void *data) +{ + struct cmd_ax12_stress_result *res = parsed_result; + int i, nb_errs = 0; + uint8_t val; + microseconds t = time_get_us2(); + + for (i=0; i<1000; i++) { + if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0) + nb_errs ++; + } + + printf_P(PSTR("%d errors / 1000\r\n"), nb_errs); + t = (time_get_us2() - t) / 1000; + printf_P(PSTR("Test done in %d ms\r\n"), (int)t); +} + +prog_char str_ax12_stress_arg0[] = "ax12_stress"; +parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0); +parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8); + +prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands"; +parse_pgm_inst_t cmd_ax12_stress = { + .f = cmd_ax12_stress_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ax12_stress, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ax12_stress_arg0, + (prog_void *)&cmd_ax12_stress_id, + NULL, + }, +}; + +/**********************************************************/ + +/* this structure is filled when cmd_baudrate is parsed successfully */ +struct cmd_baudrate_result { + fixed_string_t arg0; + uint32_t arg1; +}; + +/* function called when cmd_baudrate is parsed successfully */ +static void cmd_baudrate_parsed(void * parsed_result, void * data) +{ + struct cmd_baudrate_result *res = parsed_result; + struct uart_config c; + + printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L); + uart_getconf(1, &c); + c.baudrate = res->arg1; + uart_setconf(1, &c); + printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L); +} + +prog_char str_baudrate_arg0[] = "baudrate"; +parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0); +parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32); + +prog_char help_baudrate[] = "Change ax12 baudrate"; +parse_pgm_inst_t cmd_baudrate = { + .f = cmd_baudrate_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_baudrate, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_baudrate_arg0, + (prog_void *)&cmd_baudrate_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Uint16 */ + + +/* this structure is filled when cmd_uint16_read is parsed successfully */ +struct cmd_uint16_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t num; + uint16_t val; +}; + +/* function called when cmd_uint16_read is parsed successfully */ +static void cmd_uint16_read_parsed(void * parsed_result, void * data) +{ + struct cmd_uint16_result *res = parsed_result; + uint8_t ret; + uint16_t val; + uint8_t addr = addr_from_string(res->arg1); + ret = AX12_read_int(&gen.ax12, res->num, addr, &val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); + printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val); +} + +prog_char str_uint16_arg0[] = "read"; +parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0); +prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#" + "max_torque#down_calibration#up_calibration#torque_limit#" + "position#speed#load#punch"; +parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1); +parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8); + +prog_char help_uint16_read[] = "Read uint16 value (type, num)"; +parse_pgm_inst_t cmd_uint16_read = { + .f = cmd_uint16_read_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint16_read, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint16_arg0, + (prog_void *)&cmd_uint16_arg1, + (prog_void *)&cmd_uint16_num, + NULL, + }, +}; + +/* function called when cmd_uint16_write is parsed successfully */ +static void cmd_uint16_write_parsed(void * parsed_result, void * data) +{ + struct cmd_uint16_result *res = parsed_result; + uint8_t ret; + uint8_t addr = addr_from_string(res->arg1); + printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1, + res->val, res->val); + ret = AX12_write_int(&gen.ax12, res->num, addr, res->val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); +} + +prog_char str_uint16_arg0_w[] = "write"; +parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w); +prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#" + "max_torque#torque_limit#punch"; +parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w); +parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16); + +prog_char help_uint16_write[] = "Write uint16 value (write, num, val)"; +parse_pgm_inst_t cmd_uint16_write = { + .f = cmd_uint16_write_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint16_write, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint16_arg0_w, + (prog_void *)&cmd_uint16_arg1_w, + (prog_void *)&cmd_uint16_num, + (prog_void *)&cmd_uint16_val, + NULL, + }, +}; + +/**********************************************************/ +/* Uint8 */ + + +/* this structure is filled when cmd_uint8_read is parsed successfully */ +struct cmd_uint8_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t num; + uint8_t val; +}; + +/* function called when cmd_uint8_read is parsed successfully */ +static void cmd_uint8_read_parsed(void * parsed_result, void * data) +{ + struct cmd_uint8_result *res = parsed_result; + uint8_t ret; + uint8_t val; + uint8_t addr = addr_from_string(res->arg1); + + ret = AX12_read_byte(&gen.ax12, res->num, addr, &val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); + printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val); +} + +prog_char str_uint8_arg0[] = "read"; +parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0); +prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#" + "low_lim_volt#high_lim_volt#status_return#alarm_led#" + "alarm_shutdown#torque_enable#led#cw_comp_margin#" + "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#" + "voltage#temp#reginst#moving#lock"; +parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1); +parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8); + +prog_char help_uint8_read[] = "Read uint8 value (type, num)"; +parse_pgm_inst_t cmd_uint8_read = { + .f = cmd_uint8_read_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint8_read, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint8_arg0, + (prog_void *)&cmd_uint8_arg1, + (prog_void *)&cmd_uint8_num, + NULL, + }, +}; + +/* function called when cmd_uint8_write is parsed successfully */ +static void cmd_uint8_write_parsed(void * parsed_result, void * data) +{ + struct cmd_uint8_result *res = parsed_result; + uint8_t addr = addr_from_string(res->arg1); + uint8_t ret; + printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1, + res->val, res->val); + ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); +} + +prog_char str_uint8_arg0_w[] = "write"; +parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w); +prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#" + "low_lim_volt#high_lim_volt#status_return#alarm_led#" + "alarm_shutdown#torque_enable#led#cw_comp_margin#" + "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#" + "reginst#lock"; +parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w); +parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8); + +prog_char help_uint8_write[] = "Write uint8 value (write, num, val)"; +parse_pgm_inst_t cmd_uint8_write = { + .f = cmd_uint8_write_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint8_write, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint8_arg0_w, + (prog_void *)&cmd_uint8_arg1_w, + (prog_void *)&cmd_uint8_num, + (prog_void *)&cmd_uint8_val, + NULL, + }, +}; diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c new file mode 100644 index 0000000..a39ab07 --- /dev/null +++ b/projects/microb2010/mainboard/commands_cs.c @@ -0,0 +1,677 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: commands_cs.c,v 1.4 2009-05-02 10:08:09 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cs.h" +#include "cmdline.h" + +struct csb_list { + const prog_char *name; + struct cs_block *csb; +}; + +prog_char csb_angle_str[] = "angle"; +prog_char csb_distance_str[] = "distance"; +struct csb_list csb_list[] = { + { .name = csb_angle_str, .csb = &mainboard.angle }, + { .name = csb_distance_str, .csb = &mainboard.distance }, +}; + +struct cmd_cs_result { + fixed_string_t cmdname; + fixed_string_t csname; +}; + +/* token to be used for all cs-related commands */ +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) +{ + int i; + + for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) { + if (!strcmp_P(name, csb_list[i].name)) + return csb_list[i].csb; + } + return NULL; +} + +/**********************************************************/ +/* Gains for control system */ + +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_result { + struct cmd_cs_result cs; + int16_t p; + int16_t i; + int16_t d; +}; + +/* function called when cmd_gain is parsed successfully */ +static void cmd_gain_parsed(void * parsed_result, void *show) +{ + struct cmd_gain_result *res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_gains(&csb->pid, res->p, res->i, res->d); + + printf_P(PSTR("%s %s %d %d %d\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_gain_P(&csb->pid), + pid_get_gain_I(&csb->pid), + pid_get_gain_D(&csb->pid)); +} + +prog_char str_gain_arg0[] = "gain"; +parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0); +parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16); +parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16); +parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16); + +prog_char help_gain[] = "Set gain values for PID"; +parse_pgm_inst_t cmd_gain = { + .f = cmd_gain_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_gain, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_p, + (prog_void *)&cmd_gain_i, + (prog_void *)&cmd_gain_d, + NULL, + }, +}; + +/* show */ +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_gain_show_arg[] = "show"; +parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg); + +prog_char help_gain_show[] = "Show gain values for PID"; +parse_pgm_inst_t cmd_gain_show = { + .f = cmd_gain_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_gain_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Speeds for control system */ + +/* this structure is filled when cmd_speed is parsed successfully */ +struct cmd_speed_result { + struct cmd_cs_result cs; + uint16_t s; +}; + +/* function called when cmd_speed is parsed successfully */ +static void cmd_speed_parsed(void *parsed_result, void *show) +{ + struct cmd_speed_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + +#if notyet + if (!show) + ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */ + + printf_P(PSTR("%s %lu\r\n"), + res->cs.csname, + ext.r_b.var_pos); +#else + printf_P(PSTR("TODO\r\n")); +#endif +} + +prog_char str_speed_arg0[] = "speed"; +parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0); +parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16); + +prog_char help_speed[] = "Set speed values for ramp filter"; +parse_pgm_inst_t cmd_speed = { + .f = cmd_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_s, + NULL, + }, +}; + +/* show */ +struct cmd_speed_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg); + +prog_char help_speed_show[] = "Show speed values for ramp filter"; +parse_pgm_inst_t cmd_speed_show = { + .f = cmd_speed_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Derivate_Filters for control system */ + +/* this structure is filled when cmd_derivate_filter is parsed successfully */ +struct cmd_derivate_filter_result { + struct cmd_cs_result cs; + uint8_t size; +}; + +/* function called when cmd_derivate_filter is parsed successfully */ +static void cmd_derivate_filter_parsed(void *parsed_result, void *show) +{ + struct cmd_derivate_filter_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_derivate_filter(&csb->pid, res->size); + + printf_P(PSTR("%s %s %u\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_derivate_filter(&csb->pid)); +} + +prog_char str_derivate_filter_arg0[] = "derivate_filter"; +parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0); +parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32); + +prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)"; +parse_pgm_inst_t cmd_derivate_filter = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_derivate_filter, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_size, + NULL, + }, +}; + +/* show */ + +struct cmd_derivate_filter_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_derivate_filter_show_arg[] = "show"; +parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg); + +prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID"; +parse_pgm_inst_t cmd_derivate_filter_show = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_derivate_filter_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_show_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Consign for control system */ + +/* this structure is filled when cmd_consign is parsed successfully */ +struct cmd_consign_result { + struct cmd_cs_result cs; + int32_t p; +}; + +/* function called when cmd_consign is parsed successfully */ +static void cmd_consign_parsed(void * parsed_result, void *data) +{ + struct cmd_consign_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + cs_set_consign(&csb->cs, res->p); +} + +prog_char str_consign_arg0[] = "consign"; +parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0); +parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32); + +prog_char help_consign[] = "Set consign value"; +parse_pgm_inst_t cmd_consign = { + .f = cmd_consign_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_consign, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_consign_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_consign_p, + NULL, + }, +}; + + +/**********************************************************/ +/* Maximums for control system */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_result { + struct cmd_cs_result cs; + uint32_t in; + uint32_t i; + uint32_t out; +}; + +/* function called when cmd_maximum is parsed successfully */ +static void cmd_maximum_parsed(void *parsed_result, void *show) +{ + struct cmd_maximum_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_maximums(&csb->pid, res->in, res->i, res->out); + + printf_P(PSTR("maximum %s %lu %lu %lu\r\n"), + res->cs.csname, + pid_get_max_in(&csb->pid), + pid_get_max_I(&csb->pid), + pid_get_max_out(&csb->pid)); +} + +prog_char str_maximum_arg0[] = "maximum"; +parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0); +parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32); +parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32); +parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32); + +prog_char help_maximum[] = "Set maximum values for PID (in, I, out)"; +parse_pgm_inst_t cmd_maximum = { + .f = cmd_maximum_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_maximum, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_in, + (prog_void *)&cmd_maximum_i, + (prog_void *)&cmd_maximum_out, + NULL, + }, +}; + +/* show */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; +prog_char str_maximum_show_arg[] = "show"; +parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg); + +prog_char help_maximum_show[] = "Show maximum values for PID"; +parse_pgm_inst_t cmd_maximum_show = { + .f = cmd_maximum_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_maximum_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Quadramp for control system */ + +/* this structure is filled when cmd_quadramp is parsed successfully */ +struct cmd_quadramp_result { + struct cmd_cs_result cs; + uint32_t ap; + uint32_t an; + uint32_t sp; + uint32_t sn; +}; + +/* function called when cmd_quadramp is parsed successfully */ +static void cmd_quadramp_parsed(void *parsed_result, void *show) +{ + struct cmd_quadramp_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) { + quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn); + quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); + } + + printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"), + res->cs.csname, + csb->qr.var_2nd_ord_pos, + csb->qr.var_2nd_ord_neg, + csb->qr.var_1st_ord_pos, + csb->qr.var_1st_ord_neg); +} + +prog_char str_quadramp_arg0[] = "quadramp"; +parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0); +parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32); +parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32); +parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32); +parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32); + +prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)"; +parse_pgm_inst_t cmd_quadramp = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_quadramp, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_ap, + (prog_void *)&cmd_quadramp_an, + (prog_void *)&cmd_quadramp_sp, + (prog_void *)&cmd_quadramp_sn, + + NULL, + }, +}; + +/* show */ + +struct cmd_quadramp_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_quadramp_show_arg[] = "show"; +parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg); + +prog_char help_quadramp_show[] = "Get quadramp values for control system"; +parse_pgm_inst_t cmd_quadramp_show = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_quadramp_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* cs_status show for control system */ + +/* this structure is filled when cmd_cs_status is parsed successfully */ +struct cmd_cs_status_result { + struct cmd_cs_result cs; + fixed_string_t arg; +}; + +/* function called when cmd_cs_status is parsed successfully */ +static void cmd_cs_status_parsed(void *parsed_result, void *data) +{ + 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); + return; + } + else if (strcmp_P(res->arg, PSTR("off")) == 0) { + csb->on = 0; + printf_P(PSTR("%s is off\r\n"), res->cs.csname); + return; + } + 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 { + 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[] = "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"; +parse_pgm_inst_t cmd_cs_status = { + .f = cmd_cs_status_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_cs_status, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_cs_status_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_cs_status_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Blocking_I for control system */ + +/* this structure is filled when cmd_blocking_i is parsed successfully */ +struct cmd_blocking_i_result { + struct cmd_cs_result cs; + int32_t k1; + int32_t k2; + uint32_t i; + uint16_t cpt; +}; + +/* function called when cmd_blocking_i is parsed successfully */ +static void cmd_blocking_i_parsed(void *parsed_result, void *show) +{ + struct cmd_blocking_i_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + bd_set_current_thresholds(&csb->bd, res->k1, res->k2, + res->i, res->cpt); + + printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"), + res->cs.cmdname, + res->cs.csname, + csb->bd.k1, + csb->bd.k2, + csb->bd.i_thres, + csb->bd.cpt_thres); +} + +prog_char str_blocking_i_arg0[] = "blocking"; +parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0); +parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32); +parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32); +parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32); +parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16); + +prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)"; +parse_pgm_inst_t cmd_blocking_i = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_blocking_i, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_k1, + (prog_void *)&cmd_blocking_i_k2, + (prog_void *)&cmd_blocking_i_i, + (prog_void *)&cmd_blocking_i_cpt, + NULL, + }, +}; + +/* show */ + +struct cmd_blocking_i_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_blocking_i_show_arg[] = "show"; +parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg); + +prog_char help_blocking_i_show[] = "Show blocking detection values"; +parse_pgm_inst_t cmd_blocking_i_show = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_blocking_i_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_show_arg, + NULL, + }, +}; + + diff --git a/projects/microb2010/mainboard/commands_gen.c b/projects/microb2010/mainboard/commands_gen.c new file mode 100644 index 0000000..b20dcc8 --- /dev/null +++ b/projects/microb2010/mainboard/commands_gen.c @@ -0,0 +1,586 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: commands_gen.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "main.h" +#include "cmdline.h" +#include "sensor.h" + +/**********************************************************/ +/* Reset */ + +/* this structure is filled when cmd_reset is parsed successfully */ +struct cmd_reset_result { + fixed_string_t arg0; +}; + +/* function called when cmd_reset is parsed successfully */ +static void cmd_reset_parsed(void * parsed_result, void * data) +{ + reset(); +} + +prog_char str_reset_arg0[] = "reset"; +parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0); + +prog_char help_reset[] = "Reset the board"; +parse_pgm_inst_t cmd_reset = { + .f = cmd_reset_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_reset, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_reset_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Bootloader */ + +/* this structure is filled when cmd_bootloader is parsed successfully */ +struct cmd_bootloader_result { + fixed_string_t arg0; +}; + +/* function called when cmd_bootloader is parsed successfully */ +static void cmd_bootloader_parsed(void *parsed_result, void *data) +{ + bootloader(); +} + +prog_char str_bootloader_arg0[] = "bootloader"; +parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0); + +prog_char help_bootloader[] = "Launch the bootloader"; +parse_pgm_inst_t cmd_bootloader = { + .f = cmd_bootloader_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_bootloader, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_bootloader_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Encoders tests */ + +/* this structure is filled when cmd_encoders is parsed successfully */ +struct cmd_encoders_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_encoders is parsed successfully */ +static void cmd_encoders_parsed(void *parsed_result, void *data) +{ + struct cmd_encoders_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + encoders_spi_set_value((void *)0, 0); + encoders_spi_set_value((void *)1, 0); + encoders_spi_set_value((void *)2, 0); + encoders_spi_set_value((void *)3, 0); + return; + } + + /* show */ + while(!cmdline_keypressed()) { + printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"), + encoders_spi_get_value((void *)0), + encoders_spi_get_value((void *)1), + encoders_spi_get_value((void *)2), + encoders_spi_get_value((void *)3)); + wait_ms(100); + } +} + +prog_char str_encoders_arg0[] = "encoders"; +parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0); +prog_char str_encoders_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1); + +prog_char help_encoders[] = "Show encoders values"; +parse_pgm_inst_t cmd_encoders = { + .f = cmd_encoders_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_encoders, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_encoders_arg0, + (prog_void *)&cmd_encoders_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Scheduler show */ + +/* this structure is filled when cmd_scheduler is parsed successfully */ +struct cmd_scheduler_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_scheduler is parsed successfully */ +static void cmd_scheduler_parsed(void *parsed_result, void *data) +{ + scheduler_dump_events(); + scheduler_stats_dump(); +} + +prog_char str_scheduler_arg0[] = "scheduler"; +parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0); +prog_char str_scheduler_arg1[] = "show"; +parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1); + +prog_char help_scheduler[] = "Show scheduler events"; +parse_pgm_inst_t cmd_scheduler = { + .f = cmd_scheduler_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scheduler, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scheduler_arg0, + (prog_void *)&cmd_scheduler_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Pwms tests */ + +/* this structure is filled when cmd_pwm is parsed successfully */ +struct cmd_pwm_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; +}; + +/* function called when cmd_pwm is parsed successfully */ +static void cmd_pwm_parsed(void * parsed_result, void * data) +{ + void * pwm_ptr = NULL; + struct cmd_pwm_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("1(4A)"))) + pwm_ptr = &gen.pwm1_4A; + else if (!strcmp_P(res->arg1, PSTR("2(4B)"))) + pwm_ptr = &gen.pwm2_4B; + else if (!strcmp_P(res->arg1, PSTR("3(1A)"))) + pwm_ptr = &gen.pwm3_1A; + else if (!strcmp_P(res->arg1, PSTR("4(1B)"))) + pwm_ptr = &gen.pwm4_1B; + + else if (!strcmp_P(res->arg1, PSTR("s1(3C)"))) + pwm_ptr = &gen.servo1; + else if (!strcmp_P(res->arg1, PSTR("s2(5A)"))) + pwm_ptr = &gen.servo2; + else if (!strcmp_P(res->arg1, PSTR("s3(5B)"))) + pwm_ptr = &gen.servo3; + else if (!strcmp_P(res->arg1, PSTR("s3(5C)"))) + pwm_ptr = &gen.servo4; + + if (pwm_ptr) + pwm_ng_set(pwm_ptr, res->arg2); + + printf_P(PSTR("done\r\n")); +} + +prog_char str_pwm_arg0[] = "pwm"; +parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0); +prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)"; +parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1); +parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16); + +prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]"; +parse_pgm_inst_t cmd_pwm = { + .f = cmd_pwm_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pwm, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pwm_arg0, + (prog_void *)&cmd_pwm_arg1, + (prog_void *)&cmd_pwm_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Adcs tests */ + +/* this structure is filled when cmd_adc is parsed successfully */ +struct cmd_adc_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_adc is parsed successfully */ +static void cmd_adc_parsed(void *parsed_result, void *data) +{ + struct cmd_adc_result *res = parsed_result; + uint8_t i, loop = 0; + + if (!strcmp_P(res->arg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("ADC values: ")); + for (i=0; iarg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("SENSOR values: ")); + for (i=0; iarg1, PSTR("level"))) { + gen.log_level = res->arg2; + } + + /* else it is a show */ + cmd_log_do_show(); +} + +prog_char str_log_arg0[] = "log"; +parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0); +prog_char str_log_arg1[] = "level"; +parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1); +parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32); + +prog_char help_log[] = "Set log options: level (0 -> 5)"; +parse_pgm_inst_t cmd_log = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1, + (prog_void *)&cmd_log_arg2, + NULL, + }, +}; + +prog_char str_log_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show); + +prog_char help_log_show[] = "Show configured logs"; +parse_pgm_inst_t cmd_log_show = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1_show, + NULL, + }, +}; + +/* this structure is filled when cmd_log is parsed successfully */ +struct cmd_log_type_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_log is parsed successfully */ +static void cmd_log_type_parsed(void * parsed_result, void * data) +{ + struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result; + uint8_t lognum; + uint8_t i; + + lognum = log_name2num(res->arg2); + if (lognum == 0) { + printf_P(PSTR("Cannot find log num\r\n")); + return; + } + + if (!strcmp_P(res->arg3, PSTR("on"))) { + for (i=0; iarg3, PSTR("off"))) { + for (i=0; i + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../common/i2c_commands.h" +#include "../common/eeprom_mapping.h" + +#include "main.h" +#include "sensor.h" +#include "cmdline.h" +#include "strat.h" +#include "strat_utils.h" +#include "strat_base.h" +#include "i2c_protocol.h" +#include "actuator.h" + +struct cmd_event_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + + +/* function called when cmd_event is parsed successfully */ +static void cmd_event_parsed(void *parsed_result, void *data) +{ + u08 bit=0; + + 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"))) { + strat_hardstop(); + bit = DO_CS; + } + else if (!strcmp_P(res->arg1, PSTR("rs"))) + bit = DO_RS; + else if (!strcmp_P(res->arg1, PSTR("pos"))) + bit = DO_POS; + else if (!strcmp_P(res->arg1, PSTR("bd"))) + bit = DO_BD; + else if (!strcmp_P(res->arg1, PSTR("timer"))) { + time_reset(); + bit = DO_TIMER; + } + else if (!strcmp_P(res->arg1, PSTR("power"))) + bit = DO_POWER; + + if (!strcmp_P(res->arg2, PSTR("on"))) + mainboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) { + if (!strcmp_P(res->arg1, PSTR("cs"))) { + pwm_ng_set(LEFT_PWM, 0); + pwm_ng_set(RIGHT_PWM, 0); + } + mainboard.flags &= (~bit); + } + printf_P(PSTR("%s is %s\r\n"), res->arg1, + (bit & mainboard.flags) ? "on":"off"); +} + +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[] = "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); + +prog_char help_event[] = "Enable/disable events"; +parse_pgm_inst_t cmd_event = { + .f = cmd_event_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_event, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, + NULL, + }, +}; + + +/**********************************************************/ +/* Spi_Test */ + +/* this structure is filled when cmd_spi_test is parsed successfully */ +struct cmd_spi_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_spi_test is parsed successfully */ +static void cmd_spi_test_parsed(void * parsed_result, void * data) +{ + uint16_t i = 0, ret = 0, ret2 = 0; + + if (mainboard.flags & DO_ENCODERS) { + printf_P(PSTR("Disable encoder event first\r\n")); + return; + } + + do { + spi_slave_select(0); + ret = spi_send_and_receive_byte(i); + ret2 = spi_send_and_receive_byte(i); + spi_slave_deselect(0); + + if ((i & 0x7ff) == 0) + printf_P(PSTR("Sent %.4x twice, received %x %x\r\n"), + i, ret, ret2); + + i++; + } while(!cmdline_keypressed()); +} + +prog_char str_spi_test_arg0[] = "spi_test"; +parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0); + +prog_char help_spi_test[] = "Test the SPI"; +parse_pgm_inst_t cmd_spi_test = { + .f = cmd_spi_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_spi_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_spi_test_arg0, + NULL, + }, +}; + + + +/**********************************************************/ +/* Opponent tests */ + +/* this structure is filled when cmd_opponent is parsed successfully */ +struct cmd_opponent_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; +}; + +/* function called when cmd_opponent is parsed successfully */ +static void cmd_opponent_parsed(void *parsed_result, void *data) +{ + int16_t x,y,d,a; + + if (get_opponent_xyda(&x, &y, &d, &a)) + printf_P(PSTR("No opponent\r\n")); + else + printf_P(PSTR("x=%d y=%d, d=%d a=%d\r\n"), x, y, d, a); +} + +prog_char str_opponent_arg0[] = "opponent"; +parse_pgm_token_string_t cmd_opponent_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg0, str_opponent_arg0); +prog_char str_opponent_arg1[] = "show"; +parse_pgm_token_string_t cmd_opponent_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1); + +prog_char help_opponent[] = "Show (x,y) opponent"; +parse_pgm_inst_t cmd_opponent = { + .f = cmd_opponent_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_opponent, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg1, + NULL, + }, +}; + + +prog_char str_opponent_arg1_set[] = "set"; +parse_pgm_token_string_t cmd_opponent_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_opponent_result, arg1, str_opponent_arg1_set); +parse_pgm_token_num_t cmd_opponent_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg2, INT32); +parse_pgm_token_num_t cmd_opponent_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_opponent_result, arg3, INT32); + +prog_char help_opponent_set[] = "Set (x,y) opponent"; +parse_pgm_inst_t cmd_opponent_set = { + .f = cmd_opponent_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_opponent_set, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg1_set, + (prog_void *)&cmd_opponent_arg2, + (prog_void *)&cmd_opponent_arg3, + NULL, + }, +}; + + +/**********************************************************/ +/* Start */ + +/* this structure is filled when cmd_start is parsed successfully */ +struct cmd_start_result { + fixed_string_t arg0; + fixed_string_t color; + fixed_string_t debug; +}; + +/* function called when cmd_start is parsed successfully */ +static void cmd_start_parsed(void *parsed_result, void *data) +{ + struct cmd_start_result *res = parsed_result; + uint8_t old_level = gen.log_level; + + gen.logs[NB_LOGS] = E_USER_STRAT; + if (!strcmp_P(res->debug, PSTR("debug"))) { + strat_infos.dump_enabled = 1; + gen.log_level = 5; + } + else { + strat_infos.dump_enabled = 0; + gen.log_level = 0; + } + + if (!strcmp_P(res->color, PSTR("red"))) { + mainboard.our_color = I2C_COLOR_RED; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + } + else if (!strcmp_P(res->color, PSTR("green"))) { + mainboard.our_color = I2C_COLOR_GREEN; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + } + + printf_P(PSTR("Check that lintel is loaded\r\n")); + while(!cmdline_keypressed()); + + printf_P(PSTR("Press a key when beacon ready\r\n")); + i2c_sensorboard_set_beacon(0); + while(!cmdline_keypressed()); + i2c_sensorboard_set_beacon(1); + + strat_start(); + + gen.logs[NB_LOGS] = 0; + gen.log_level = old_level; +} + +prog_char str_start_arg0[] = "start"; +parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0); +prog_char str_start_color[] = "green#red"; +parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color); +prog_char str_start_debug[] = "debug#match"; +parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug); + +prog_char help_start[] = "Start the robot"; +parse_pgm_inst_t cmd_start = { + .f = cmd_start_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_start, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_start_arg0, + (prog_void *)&cmd_start_color, + (prog_void *)&cmd_start_debug, + NULL, + }, +}; + + + +/**********************************************************/ +/* Interact */ + +/* this structure is filled when cmd_interact is parsed successfully */ +struct cmd_interact_result { + fixed_string_t arg0; +}; + +static void print_cs(void) +{ + printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld " + "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"), + cs_get_consign(&mainboard.distance.cs), + cs_get_consign(&mainboard.angle.cs), + cs_get_filtered_consign(&mainboard.distance.cs), + cs_get_filtered_consign(&mainboard.angle.cs), + cs_get_error(&mainboard.distance.cs), + cs_get_error(&mainboard.angle.cs), + cs_get_out(&mainboard.distance.cs), + cs_get_out(&mainboard.angle.cs)); +} + +static void print_pos(void) +{ + printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); +} + +static void print_time(void) +{ + printf_P(PSTR("time %d\r\n"),time_get_s()); +} + + +static void print_sensors(void) +{ +#ifdef notyet + if (sensor_start_switch()) + printf_P(PSTR("Start switch | ")); + else + printf_P(PSTR(" | ")); + + if (IR_DISP_SENSOR()) + printf_P(PSTR("IR disp | ")); + else + printf_P(PSTR(" | ")); + + printf_P(PSTR("\r\n")); +#endif +} + +static void print_pid(void) +{ + printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | " + "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid), + pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid), + pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid), + pid_get_value_out(&mainboard.distance.pid), + pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid), + pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid), + pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid), + pid_get_value_out(&mainboard.angle.pid)); +} + +#define PRINT_POS (1<<0) +#define PRINT_PID (1<<1) +#define PRINT_CS (1<<2) +#define PRINT_SENSORS (1<<3) +#define PRINT_TIME (1<<4) +#define PRINT_BLOCKING (1<<5) + +static void cmd_interact_parsed(void * parsed_result, void * data) +{ + int c; + int8_t cmd; + uint8_t print = 0; + struct vt100 vt100; + + vt100_init(&vt100); + + printf_P(PSTR("Display debugs:\r\n" + " 1:pos\r\n" + " 2:pid\r\n" + " 3:cs\r\n" + " 4:sensors\r\n" + " 5:time\r\n" + /* " 6:blocking\r\n" */ + "Commands:\r\n" + " arrows:move\r\n" + " space:stop\r\n" + " q:quit\r\n")); + + /* stop motors */ + mainboard.flags &= (~DO_CS); + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + + while(1) { + if (print & PRINT_POS) { + print_pos(); + } + + if (print & PRINT_PID) { + print_pid(); + } + + if (print & PRINT_CS) { + print_cs(); + } + + if (print & PRINT_SENSORS) { + print_sensors(); + } + + if (print & PRINT_TIME) { + print_time(); + } +/* if (print & PRINT_BLOCKING) { */ +/* printf_P(PSTR("%s %s blocking=%d\r\n"), */ +/* mainboard.blocking ? "BLOCK1":" ", */ +/* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */ +/* rs_get_blocking(&mainboard.rs)); */ +/* } */ + + c = cmdline_getchar(); + if (c == -1) { + wait_ms(10); + continue; + } + cmd = vt100_parser(&vt100, c); + if (cmd == -2) { + wait_ms(10); + continue; + } + + if (cmd == -1) { + switch(c) { + case '1': print ^= PRINT_POS; break; + case '2': print ^= PRINT_PID; break; + case '3': print ^= PRINT_CS; break; + case '4': print ^= PRINT_SENSORS; break; + case '5': print ^= PRINT_TIME; break; + case '6': print ^= PRINT_BLOCKING; break; + + case 'q': + if (mainboard.flags & DO_CS) + strat_hardstop(); + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + return; + case ' ': + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + break; + default: + break; + } + } + else { + switch(cmd) { + case KEY_UP_ARR: + pwm_set_and_save(LEFT_PWM, 1200); + pwm_set_and_save(RIGHT_PWM, 1200); + break; + case KEY_LEFT_ARR: + pwm_set_and_save(LEFT_PWM, -1200); + pwm_set_and_save(RIGHT_PWM, 1200); + break; + case KEY_DOWN_ARR: + pwm_set_and_save(LEFT_PWM, -1200); + pwm_set_and_save(RIGHT_PWM, -1200); + break; + case KEY_RIGHT_ARR: + pwm_set_and_save(LEFT_PWM, 1200); + pwm_set_and_save(RIGHT_PWM, -1200); + break; + } + } + wait_ms(10); + } +} + +prog_char str_interact_arg0[] = "interact"; +parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0); + +prog_char help_interact[] = "Interactive mode"; +parse_pgm_inst_t cmd_interact = { + .f = cmd_interact_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_interact, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_interact_arg0, + NULL, + }, +}; + + +/**********************************************************/ +/* Color */ + +/* this structure is filled when cmd_color is parsed successfully */ +struct cmd_color_result { + fixed_string_t arg0; + fixed_string_t color; +}; + +/* function called when cmd_color is parsed successfully */ +static void cmd_color_parsed(void *parsed_result, void *data) +{ + struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; + if (!strcmp_P(res->color, PSTR("red"))) { + mainboard.our_color = I2C_COLOR_RED; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + } + else if (!strcmp_P(res->color, PSTR("green"))) { + mainboard.our_color = I2C_COLOR_GREEN; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + } + printf_P(PSTR("Done\r\n")); +} + +prog_char str_color_arg0[] = "color"; +parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0); +prog_char str_color_color[] = "green#red"; +parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color); + +prog_char help_color[] = "Set our color"; +parse_pgm_inst_t cmd_color = { + .f = cmd_color_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_color, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, + NULL, + }, +}; + + +/**********************************************************/ +/* Rs tests */ + +/* 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_rs is parsed successfully */ +static void cmd_rs_parsed(void *parsed_result, void *data) +{ + // 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_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_rs, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_arg0, + (prog_void *)&cmd_rs_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* I2cdebug */ + +/* this structure is filled when cmd_i2cdebug is parsed successfully */ +struct cmd_i2cdebug_result { + fixed_string_t arg0; +}; + +/* function called when cmd_i2cdebug is parsed successfully */ +static void cmd_i2cdebug_parsed(void * parsed_result, void * data) +{ + i2c_debug(); + i2c_protocol_debug(); +} + +prog_char str_i2cdebug_arg0[] = "i2cdebug"; +parse_pgm_token_string_t cmd_i2cdebug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_i2cdebug_result, arg0, str_i2cdebug_arg0); + +prog_char help_i2cdebug[] = "I2c debug infos"; +parse_pgm_inst_t cmd_i2cdebug = { + .f = cmd_i2cdebug_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_i2cdebug, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_i2cdebug_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Show */ + +/* this structure is filled when cmd_mechboard_show is parsed successfully */ +struct cmd_mechboard_show_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_mechboard_show is parsed successfully */ +static void cmd_mechboard_show_parsed(void * parsed_result, void * data) +{ + printf_P(PSTR("mode = %x\r\n"), mechboard.mode); + printf_P(PSTR("status = %x\r\n"), mechboard.status); + printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count); + + printf_P(PSTR("column_count = %d\r\n"), get_column_count()); + printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"), + pump_left1_is_full(), pump_left2_is_full(), + pump_right1_is_full(), pump_right2_is_full()); + + printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1); + printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2); + printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1); + printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2); + + printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left); + printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right); + +} + +prog_char str_mechboard_show_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0); +prog_char str_mechboard_show_arg1[] = "show"; +parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1); + +prog_char help_mechboard_show[] = "show mechboard status"; +parse_pgm_inst_t cmd_mechboard_show = { + .f = cmd_mechboard_show_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_show_arg0, + (prog_void *)&cmd_mechboard_show_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode1 */ + +/* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */ +struct cmd_mechboard_setmode1_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_mechboard_setmode1 is parsed successfully */ +static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data) +{ + struct cmd_mechboard_setmode1_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("init"))) + i2c_mechboard_mode_init(); + else if (!strcmp_P(res->arg1, PSTR("manual"))) + i2c_mechboard_mode_manual(); + else if (!strcmp_P(res->arg1, PSTR("pickup"))) + i2c_mechboard_mode_pickup(); + else if (!strcmp_P(res->arg1, PSTR("lazy_harvest"))) + i2c_mechboard_mode_lazy_harvest(); + else if (!strcmp_P(res->arg1, PSTR("harvest"))) + i2c_mechboard_mode_harvest(); + else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel"))) + i2c_mechboard_mode_prepare_get_lintel(); + else if (!strcmp_P(res->arg1, PSTR("get_lintel"))) + i2c_mechboard_mode_get_lintel(); + else if (!strcmp_P(res->arg1, PSTR("put_lintel"))) + i2c_mechboard_mode_put_lintel(); + else if (!strcmp_P(res->arg1, PSTR("init"))) + i2c_mechboard_mode_init(); + else if (!strcmp_P(res->arg1, PSTR("eject"))) + i2c_mechboard_mode_init(); + else if (!strcmp_P(res->arg1, PSTR("clear"))) + i2c_mechboard_mode_clear(); + else if (!strcmp_P(res->arg1, PSTR("loaded"))) + i2c_mechboard_mode_loaded(); + else if (!strcmp_P(res->arg1, PSTR("store"))) + i2c_mechboard_mode_store(); + else if (!strcmp_P(res->arg1, PSTR("manivelle"))) + i2c_mechboard_mode_manivelle(); + else if (!strcmp_P(res->arg1, PSTR("lazy_pickup"))) + i2c_mechboard_mode_lazy_pickup(); +} + +prog_char str_mechboard_setmode1_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0); +prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup"; +parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1); + +prog_char help_mechboard_setmode1[] = "set mechboard mode (mode)"; +parse_pgm_inst_t cmd_mechboard_setmode1 = { + .f = cmd_mechboard_setmode1_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode1_arg0, + (prog_void *)&cmd_mechboard_setmode1_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode2 */ + +/* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */ +struct cmd_mechboard_setmode2_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_mechboard_setmode2 is parsed successfully */ +static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data) +{ + struct cmd_mechboard_setmode2_result *res = parsed_result; + uint8_t side = I2C_LEFT_SIDE; + + if (!strcmp_P(res->arg2, PSTR("left"))) + side = I2C_LEFT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("right"))) + side = I2C_RIGHT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("center"))) + side = I2C_CENTER_SIDE; + else if (!strcmp_P(res->arg2, PSTR("auto"))) + side = I2C_AUTO_SIDE; + + if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) + i2c_mechboard_mode_prepare_pickup(side); + else if (!strcmp_P(res->arg1, PSTR("push_temple_disc"))) + i2c_mechboard_mode_push_temple_disc(side); +} + +prog_char str_mechboard_setmode2_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0); +prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc"; +parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1); +prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center"; +parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2); + +prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)"; +parse_pgm_inst_t cmd_mechboard_setmode2 = { + .f = cmd_mechboard_setmode2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode2_arg0, + (prog_void *)&cmd_mechboard_setmode2_arg1, + (prog_void *)&cmd_mechboard_setmode2_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode3 */ + +/* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */ +struct cmd_mechboard_setmode3_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t level; +}; + +/* function called when cmd_mechboard_setmode3 is parsed successfully */ +static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data) +{ + struct cmd_mechboard_setmode3_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("autobuild"))) + i2c_mechboard_mode_simple_autobuild(res->level); + else if (!strcmp_P(res->arg1, PSTR("prepare_build"))) + i2c_mechboard_mode_prepare_build_both(res->level); + else if (!strcmp_P(res->arg1, PSTR("prepare_inside"))) + i2c_mechboard_mode_prepare_inside_both(res->level); + else if (!strcmp_P(res->arg1, PSTR("push_temple"))) + i2c_mechboard_mode_push_temple(res->level); +} + +prog_char str_mechboard_setmode3_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0); +prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple"; +parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1); +parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8); + +prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)"; +parse_pgm_inst_t cmd_mechboard_setmode3 = { + .f = cmd_mechboard_setmode3_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode3, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode3_arg0, + (prog_void *)&cmd_mechboard_setmode3_arg1, + (prog_void *)&cmd_mechboard_setmode3_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode4 */ + +/* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */ +struct cmd_mechboard_setmode4_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t level_l; + uint8_t count_l; + uint8_t dist_l; + uint8_t level_r; + uint8_t count_r; + uint8_t dist_r; + uint8_t do_lintel; +}; + +/* function called when cmd_mechboard_setmode4 is parsed successfully */ +static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data) +{ + struct cmd_mechboard_setmode4_result *res = parsed_result; + i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l, + res->level_r, res->count_r, res->dist_r, + res->do_lintel); +} + +prog_char str_mechboard_setmode4_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0); +prog_char str_mechboard_setmode4_arg1[] = "autobuild"; +parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8); +parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8); + +prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)"; +parse_pgm_inst_t cmd_mechboard_setmode4 = { + .f = cmd_mechboard_setmode4_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode4, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode4_arg0, + (prog_void *)&cmd_mechboard_setmode4_arg1, + (prog_void *)&cmd_mechboard_setmode4_arg2, + (prog_void *)&cmd_mechboard_setmode4_arg3, + (prog_void *)&cmd_mechboard_setmode4_arg4, + (prog_void *)&cmd_mechboard_setmode4_arg5, + (prog_void *)&cmd_mechboard_setmode4_arg6, + (prog_void *)&cmd_mechboard_setmode4_arg7, + (prog_void *)&cmd_mechboard_setmode4_arg8, + NULL, + }, +}; + +/**********************************************************/ +/* Mechboard_Setmode5 */ + +/* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */ +struct cmd_mechboard_setmode5_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_mechboard_setmode5 is parsed successfully */ +static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data) +{ + struct cmd_mechboard_setmode5_result *res = parsed_result; + uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; + + if (!strcmp_P(res->arg2, PSTR("left"))) + side = I2C_LEFT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("right"))) + side = I2C_RIGHT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("center"))) + side = I2C_CENTER_SIDE; + else if (!strcmp_P(res->arg2, PSTR("auto"))) + side = I2C_AUTO_SIDE; + + if (!strcmp_P(res->arg3, PSTR("harvest"))) + next_mode = I2C_MECHBOARD_MODE_HARVEST; + else if (!strcmp_P(res->arg3, PSTR("lazy_harvest"))) + next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; + else if (!strcmp_P(res->arg3, PSTR("pickup"))) + next_mode = I2C_MECHBOARD_MODE_PICKUP; + else if (!strcmp_P(res->arg3, PSTR("clear"))) + next_mode = I2C_MECHBOARD_MODE_CLEAR; + else if (!strcmp_P(res->arg3, PSTR("store"))) + next_mode = I2C_MECHBOARD_MODE_STORE; + else if (!strcmp_P(res->arg3, PSTR("lazy_pickup"))) + next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; + + if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) + i2c_mechboard_mode_prepare_pickup_next(side, next_mode); +} + +prog_char str_mechboard_setmode5_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0); +prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup"; +parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1); +prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center"; +parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2); +prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear"; +parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3); + +prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)"; +parse_pgm_inst_t cmd_mechboard_setmode5 = { + .f = cmd_mechboard_setmode5_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_mechboard_setmode5, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_mechboard_setmode5_arg0, + (prog_void *)&cmd_mechboard_setmode5_arg1, + (prog_void *)&cmd_mechboard_setmode5_arg2, + (prog_void *)&cmd_mechboard_setmode5_arg3, + NULL, + }, +}; + +/**********************************************************/ +/* pickup wheels */ + +/* this structure is filled when cmd_pickup_wheels is parsed successfully */ +struct cmd_pickup_wheels_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_pickup_wheels is parsed successfully */ +static void cmd_pickup_wheels_parsed(void *parsed_result, void *data) +{ + struct cmd_pickup_wheels_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("on"))) + pickup_wheels_on(); + else + pickup_wheels_off(); +} + +prog_char str_pickup_wheels_arg0[] = "pickup_wheels"; +parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0); +prog_char str_pickup_wheels_arg1[] = "on#off"; +parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1); + +prog_char help_pickup_wheels[] = "Enable/disable pickup wheels"; +parse_pgm_inst_t cmd_pickup_wheels = { + .f = cmd_pickup_wheels_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pickup_wheels, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pickup_wheels_arg0, + (prog_void *)&cmd_pickup_wheels_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Beacon_Start */ + +/* this structure is filled when cmd_beacon_start is parsed successfully */ +struct cmd_beacon_start_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_beacon_start is parsed successfully */ +static void cmd_beacon_start_parsed(void *parsed_result, void *data) +{ + struct cmd_beacon_start_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("start"))) + i2c_sensorboard_set_beacon(1); + else + i2c_sensorboard_set_beacon(0); +} + +prog_char str_beacon_start_arg0[] = "beacon"; +parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0); +prog_char str_beacon_start_arg1[] = "start#stop"; +parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1); + +prog_char help_beacon_start[] = "Beacon enabled/disable"; +parse_pgm_inst_t cmd_beacon_start = { + .f = cmd_beacon_start_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_beacon_start, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_beacon_start_arg0, + (prog_void *)&cmd_beacon_start_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Pump_Current */ + +/* this structure is filled when cmd_pump_current is parsed successfully */ +struct cmd_pump_current_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_pump_current is parsed successfully */ +static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"), + sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4), + mechboard.pump_right1_current, mechboard.pump_right2_current); +} + +prog_char str_pump_current_arg0[] = "pump_current"; +parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0); +prog_char str_pump_current_arg1[] = "show"; +parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1); + +prog_char help_pump_current[] = "dump pump current"; +parse_pgm_inst_t cmd_pump_current = { + .f = cmd_pump_current_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pump_current, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pump_current_arg0, + (prog_void *)&cmd_pump_current_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Build_Test */ + +/* this structure is filled when cmd_build_test is parsed successfully */ +struct cmd_build_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_build_test is parsed successfully */ +static void cmd_build_test_parsed(void *parsed_result, void *data) +{ + //struct cmd_build_test_result *res = parsed_result; + + printf_P(PSTR("lintel must be there\r\n")); + i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, + I2C_MECHBOARD_MODE_HARVEST); + wait_ms(500); + + printf_P(PSTR("Insert 4 colums\r\n")); + while (get_column_count() != 4); + + i2c_mechboard_mode_prepare_build_both(0); + trajectory_d_rel(&mainboard.traj, 200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + wait_ms(500); + + i2c_mechboard_mode_simple_autobuild(0); + wait_ms(100); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + trajectory_d_rel(&mainboard.traj, -200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, + I2C_MECHBOARD_MODE_HARVEST); + + while (get_column_count() != 3); + + i2c_mechboard_mode_prepare_build_both(3); + trajectory_d_rel(&mainboard.traj, 200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + wait_ms(500); + + i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST, + 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0); + i2cproto_wait_update(); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + trajectory_d_rel(&mainboard.traj, -200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE); + wait_ms(500); + + i2c_mechboard_mode_harvest(); + while (get_column_count() != 3); + + i2c_mechboard_mode_prepare_build_both(5); + trajectory_d_rel(&mainboard.traj, 200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + wait_ms(1000); + + i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST, + 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0); + i2cproto_wait_update(); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + trajectory_d_rel(&mainboard.traj, -200); +} + +prog_char str_build_test_arg0[] = "build_test"; +parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0); + +prog_char help_build_test[] = "Build_Test function"; +parse_pgm_inst_t cmd_build_test = { + .f = cmd_build_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_build_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_build_test_arg0, + NULL, + }, +}; + + +/**********************************************************/ +/* Column_Test */ + +/* this structure is filled when cmd_column_test is parsed successfully */ +struct cmd_column_test_result { + fixed_string_t arg0; + uint8_t level; + int16_t dist; + int8_t a1; + int8_t a2; + int8_t a3; + int16_t arm_dist; + int8_t nb_col; +}; + +/* function called when cmd_column_test is parsed successfully */ +static void cmd_column_test_parsed(void *parsed_result, void *data) +{ + struct cmd_column_test_result *res = parsed_result; + uint8_t level = res->level, debug = 0; + uint8_t c, push = 0; + + /* default conf */ + if (data) { + res->dist = 70; + res->a1 = -20; + res->a2 = 40; + res->a3 = -20; + res->arm_dist = 220; + res->nb_col = 2; + } + + if (!strcmp_P(res->arg0, PSTR("column_test_debug"))) + debug = 1; + if (!strcmp_P(res->arg0, PSTR("column_test_push"))) + push = 1; + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + + /* Go to disc */ + + trajectory_d_rel(&mainboard.traj, 200); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + /* go back, insert colums */ + + trajectory_d_rel(&mainboard.traj, -200); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, + I2C_MECHBOARD_MODE_HARVEST); + printf_P(PSTR("Insert 4 colums\r\n")); + while (get_column_count() != 4); + + /* build with left arm */ + + i2c_mechboard_mode_prepare_inside_both(level); + trajectory_d_rel(&mainboard.traj, 200-(res->dist)); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + if (debug) + c = cmdline_getchar_wait(); + + trajectory_a_rel(&mainboard.traj, res->a1); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + if (debug) + c = cmdline_getchar_wait(); + + i2c_mechboard_mode_prepare_build_select(level, -1); + time_wait_ms(200); + if (debug) + c = cmdline_getchar_wait(); + i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist, + 0, 0, res->arm_dist, 0); + while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + if (debug) + c = cmdline_getchar_wait(); + i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1); + + if (debug) + c = cmdline_getchar_wait(); + /* build with right arm */ + + trajectory_a_rel(&mainboard.traj, res->a2); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + if (debug) + c = cmdline_getchar_wait(); + /* only ok for nb_col == 2 */ + if ((level + res->nb_col) >= 7) + i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1); + else + i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col); + time_wait_ms(200); + if (debug) + c = cmdline_getchar_wait(); + i2c_mechboard_mode_autobuild(0, 0, res->arm_dist, + level + res->nb_col, res->nb_col, + res->arm_dist, 0); + while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + + if (push) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE); + time_wait_ms(500); + trajectory_d_rel(&mainboard.traj, 100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + else if (level == 1 || level == 0) { + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(level); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + if (debug) + c = cmdline_getchar_wait(); + i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2); + + if (debug) + c = cmdline_getchar_wait(); + + trajectory_a_rel(&mainboard.traj, res->a3); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + if (debug) + c = cmdline_getchar_wait(); + /* go back, insert colums */ + + trajectory_d_rel(&mainboard.traj, -100); + + return; +} + +prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push"; +parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0); +parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8); + +prog_char help_column_test[] = "Column_Test function (level)"; +parse_pgm_inst_t cmd_column_test = { + .f = cmd_column_test_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_column_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_column_test_arg0, + (prog_void *)&cmd_column_test_arg1, + NULL, + }, +}; + +parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16); +parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8); +parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8); +parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8); +parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16); +parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8); + +prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)"; +parse_pgm_inst_t cmd_column_test2 = { + .f = cmd_column_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_column_test2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_column_test_arg0, + (prog_void *)&cmd_column_test_arg1, + (prog_void *)&cmd_column_test_arg2, + (prog_void *)&cmd_column_test_arg3, + (prog_void *)&cmd_column_test_arg4, + (prog_void *)&cmd_column_test_arg5, + (prog_void *)&cmd_column_test_arg6, + (prog_void *)&cmd_column_test_arg7, + NULL, + }, +}; + + +/**********************************************************/ +/* Pickup_Test */ + +/* this structure is filled when cmd_pickup_test is parsed successfully */ +struct cmd_pickup_test_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t dist; +}; + +/* return red or green sensor */ +#define COLOR_IR_SENSOR() \ + ({ \ + uint8_t __ret = 0; \ + if (side == I2C_RIGHT_SIDE) \ + __ret = sensor_get(S_DISP_RIGHT); \ + else \ + __ret = sensor_get(S_DISP_LEFT); \ + __ret; \ + }) \ +/* column dispensers */ +#define COL_SCAN_MARGIN 200 +/* distance between the wheel axis and the IR sensor */ + +/* function called when cmd_pickup_test is parsed successfully */ +static void cmd_pickup_test_parsed(void *parsed_result, void *data) +{ + uint8_t err, side, first_try = 1; + int8_t cols_count_before, cols_count_after, cols; + struct cmd_pickup_test_result *res = parsed_result; + int16_t pos1, pos2, pos; + microseconds us; + int16_t dist = res->dist; + uint8_t timeout = 0; + + if (!strcmp_P(res->arg1, PSTR("left"))) + side = I2C_LEFT_SIDE; + else + side = I2C_RIGHT_SIDE; + + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + cols_count_before = get_column_count(); + position_set(&mainboard.pos, 0, 0, 0); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -1000); + err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR); + if (err) /* we should not reach end */ + goto fail; + pos1 = position_get_x_s16(&mainboard.pos); + printf_P(PSTR("pos1 = %d\r\n"), pos1); + + err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR); + if (err) + goto fail; + pos2 = position_get_x_s16(&mainboard.pos); + printf_P(PSTR("pos2 = %d\r\n"), pos2); + + pos = ABS(pos1 - pos2); + printf_P(PSTR("pos = %d\r\n"), pos); + + trajectory_d_rel(&mainboard.traj, -dist + pos/2); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + if (side == I2C_LEFT_SIDE) + trajectory_a_rel(&mainboard.traj, 90); + else + trajectory_a_rel(&mainboard.traj, -90); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + pickup_wheels_on(); + retry: + if (first_try) + i2c_mechboard_mode_lazy_harvest(); + else + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + first_try = 0; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, 300); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST); + + trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + goto fail; + + position_set(&mainboard.pos, 0, 0, 0); + if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { + strat_eject_col(90, 0); + goto retry; + } + + /* start to pickup with finger / arms */ + + printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__); + i2c_mechboard_mode_pickup(); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == + I2C_MECHBOARD_MODE_PICKUP, 100); + us = time_get_us2(); + cols = get_column_count(); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) { + if (get_column_count() != cols) { + cols = get_column_count(); + us = time_get_us2(); + } + if ((get_column_count() - cols_count_before) >= 4) { + printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__); + break; + } + /* 1 second timeout */ + if (time_get_us2() - us > 1500000L) { + printf_P(PSTR("%s timeout\r\n"), __FUNCTION__); + timeout = 1; + break; + } + } + + /* eject if we found a bad color column */ + + if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { + strat_eject_col(90, 0); + goto retry; + } + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -250); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); + + cols_count_after = get_column_count(); + cols = cols_count_after - cols_count_before; + DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols); + + pickup_wheels_off(); + i2c_mechboard_mode_clear(); + + wait_ms(1000); + return; + fail: + printf_P(PSTR("failed\r\n")); + strat_hardstop(); +} + +prog_char str_pickup_test_arg0[] = "pickup_test"; +parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0); +prog_char str_pickup_test_arg1[] = "left#right"; +parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1); +parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16); + +prog_char help_pickup_test[] = "Pickup_Test function"; +parse_pgm_inst_t cmd_pickup_test = { + .f = cmd_pickup_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pickup_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pickup_test_arg0, + (prog_void *)&cmd_pickup_test_arg1, + (prog_void *)&cmd_pickup_test_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Lintel_Test */ + +/* this structure is filled when cmd_lintel_test is parsed successfully */ +struct cmd_lintel_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_lintel_test is parsed successfully */ +static void cmd_lintel_test_parsed(void *parsed_result, void *data) +{ + uint8_t err, first_try = 1, right_ok, left_ok; + int16_t left_cur, right_cur; + + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(500); + retry: + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, 500); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err) && err != END_BLOCKING) + goto fail; + + i2c_mechboard_mode_get_lintel(); + time_wait_ms(500); + + left_cur = sensor_get_adc(ADC_CSENSE3); + left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN); + right_cur = mechboard.pump_right1_current; + right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN); + + printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"), + left_ok, left_cur, right_ok, right_cur); + if (first_try) { + if (!right_ok && !left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + } + else if (right_ok && !left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + strat_set_speed(500, 500); + trajectory_d_a_rel(&mainboard.traj, -150, 30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_d_a_rel(&mainboard.traj, -140, -30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + first_try = 0; + goto retry; + } + else if (!right_ok && left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + strat_set_speed(500, 500); + trajectory_d_a_rel(&mainboard.traj, -150, -30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_d_a_rel(&mainboard.traj, -140, 30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + first_try = 0; + goto retry; + } + /* else, lintel is ok */ + else { + i2c_mechboard_mode_put_lintel(); + } + } + else { + if (right_ok && left_ok) { + /* lintel is ok */ + i2c_mechboard_mode_put_lintel(); + } + else { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + } + } + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -250); + err = wait_traj_end(TRAJ_FLAGS_STD); + return; + +fail: + printf_P(PSTR("fail\r\n")); + return; +} + +prog_char str_lintel_test_arg0[] = "lintel_test"; +parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0); + +prog_char help_lintel_test[] = "Lintel_Test function"; +parse_pgm_inst_t cmd_lintel_test = { + .f = cmd_lintel_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_lintel_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_lintel_test_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Scan_Test */ + +/* this structure is filled when cmd_scan_test is parsed successfully */ +struct cmd_scan_test_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t start_dist; + int16_t scan_dist; + int16_t scan_speed; + int16_t center_x; + int16_t center_y; + uint8_t level; +}; + +#define SCAN_MODE_CHECK_TEMPLE 0 +#define SCAN_MODE_SCAN_COL 1 +#define SCAN_MODE_SCAN_TEMPLE 2 +#define SCAN_MODE_TRAJ_ONLY 3 + +/* function called when cmd_scan_test is parsed successfully */ +static void cmd_scan_test_parsed(void *parsed_result, void *data) +{ + uint8_t err, mode=0, c; + int16_t pos1x, pos1y, dist; + struct cmd_scan_test_result *res = parsed_result; + int16_t back_mm = 0; + + int16_t ckpt_rel_x = 0, ckpt_rel_y = 0; + + double center_abs_x, center_abs_y; + double ckpt_rel_d, ckpt_rel_a; + double ckpt_abs_x, ckpt_abs_y; + + if (!strcmp_P(res->arg1, PSTR("traj_only"))) + mode = SCAN_MODE_TRAJ_ONLY; + else if (!strcmp_P(res->arg1, PSTR("check_temple"))) + mode = SCAN_MODE_CHECK_TEMPLE; + else if (!strcmp_P(res->arg1, PSTR("scan_col"))) + mode = SCAN_MODE_SCAN_COL; + else if (!strcmp_P(res->arg1, PSTR("scan_temple"))) + mode = SCAN_MODE_SCAN_TEMPLE; + + /* go to disc */ + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + trajectory_d_rel(&mainboard.traj, 400); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (err != END_BLOCKING) + return; + + /* save absolute position of disc */ + rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y); + + /* go back and prepare to scan */ + strat_set_speed(1000, 1000); + trajectory_d_a_rel(&mainboard.traj, -140, 130); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + return; + + /* prepare scanner arm */ + if (mode != SCAN_MODE_TRAJ_ONLY) + i2c_sensorboard_scanner_prepare(); + time_wait_ms(250); + + strat_set_speed(res->scan_speed, 1000); + + pos1x = position_get_x_s16(&mainboard.pos); + pos1y = position_get_y_s16(&mainboard.pos); + trajectory_d_rel(&mainboard.traj, -res->scan_dist); + + while (1) { + err = test_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (err != 0) + break; + + dist = distance_from_robot(pos1x, pos1y); + + if (dist > res->start_dist) + break; + + if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) { + err = END_ERROR; + break; + } + } + + if (err) { + if (TRAJ_SUCCESS(err)) + err = END_ERROR; /* should not reach end */ + strat_hardstop(); + trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (mode != SCAN_MODE_TRAJ_ONLY) + i2c_sensorboard_scanner_stop(); + return; + } + + /* start the scanner */ + + if (mode != SCAN_MODE_TRAJ_ONLY) + i2c_sensorboard_scanner_start(); + + err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN, + TRAJ_FLAGS_NO_NEAR); + if (err == 0) + err = END_ERROR; + if (!TRAJ_SUCCESS(err)) { + strat_hardstop(); + trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (mode != SCAN_MODE_TRAJ_ONLY) + i2c_sensorboard_scanner_stop(); + return; + } + + if (mode == SCAN_MODE_TRAJ_ONLY) + return; + + wait_scan_done(10000); + + i2c_sensorboard_scanner_stop(); + + if (mode == SCAN_MODE_CHECK_TEMPLE) { + i2c_sensorboard_scanner_algo_check(res->level, + res->center_x, res->center_y); + i2cproto_wait_update(); + wait_scan_done(10000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1) { + printf_P(PSTR("-- try to build a temple\r\n")); + res->center_x = 15; + res->center_y = 13; + mode = SCAN_MODE_SCAN_TEMPLE; + } + } + + if (mode == SCAN_MODE_SCAN_TEMPLE) { + i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC, + res->center_x, + res->center_y); + i2cproto_wait_update(); + wait_scan_done(10000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1 || + strat_scan_get_checkpoint(mode, &ckpt_rel_x, + &ckpt_rel_y, &back_mm)) { + printf_P(PSTR("-- try to build a column\r\n")); + mode = SCAN_MODE_SCAN_COL; + } + } + + if (mode == SCAN_MODE_SCAN_COL) { + i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, + res->center_x, res->center_y); + i2cproto_wait_update(); + wait_scan_done(10000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1 || + strat_scan_get_checkpoint(mode, &ckpt_rel_x, + &ckpt_rel_y, &back_mm)) { + return; + } + } + + if (sensorboard.dropzone_h == -1) + return; + + if (mode == SCAN_MODE_CHECK_TEMPLE) { + ckpt_rel_x = 220; + ckpt_rel_y = 100; + } + + + printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y); + + rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y); + abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a); + + printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y); + + printf_P(PSTR("ok ? (y/n)\r\n")); + + c = cmdline_getchar_wait(); + + if (c != 'y') + return; + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + /* intermediate checkpoint for some positions */ + if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) { + trajectory_goto_xy_rel(&mainboard.traj, 200, 100); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + return; + } + + trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + return; + + trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + return; + + c = cmdline_getchar_wait(); + + pos1x = position_get_x_s16(&mainboard.pos); + pos1y = position_get_y_s16(&mainboard.pos); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 200); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200, + TRAJ_FLAGS_SMALL_DIST); + if (err == 0) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + trajectory_d_rel(&mainboard.traj, 400); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + if (err != END_BLOCKING) + return; + + if (back_mm) { + trajectory_d_rel(&mainboard.traj, -back_mm); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } +} + +prog_char str_scan_test_arg0[] = "scan_test"; +parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0); +prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple"; +parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1); +parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16); +parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16); +parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16); +parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16); +parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16); + +prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)"; +parse_pgm_inst_t cmd_scan_test = { + .f = cmd_scan_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scan_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scan_test_arg0, + (prog_void *)&cmd_scan_test_arg1, + (prog_void *)&cmd_scan_test_arg2, + (prog_void *)&cmd_scan_test_arg3, + (prog_void *)&cmd_scan_test_arg4, + (prog_void *)&cmd_scan_test_arg5, + (prog_void *)&cmd_scan_test_arg6, + NULL, + }, +}; + +prog_char str_scan_test_arg1b[] = "check_temple"; +parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b); +parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8); + +prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)"; +parse_pgm_inst_t cmd_scan_test2 = { + .f = cmd_scan_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scan_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scan_test_arg0, + (prog_void *)&cmd_scan_test_arg1b, + (prog_void *)&cmd_scan_test_arg2, + (prog_void *)&cmd_scan_test_arg3, + (prog_void *)&cmd_scan_test_arg4, + (prog_void *)&cmd_scan_test_arg5, + (prog_void *)&cmd_scan_test_arg6, + (prog_void *)&cmd_scan_test_arg7, + NULL, + }, +}; + +/**********************************************************/ +/* Time_Monitor */ + +/* this structure is filled when cmd_time_monitor is parsed successfully */ +struct cmd_time_monitor_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_time_monitor is parsed successfully */ +static void cmd_time_monitor_parsed(void *parsed_result, void *data) +{ + struct cmd_time_monitor_result *res = parsed_result; + uint16_t seconds; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + eeprom_write_word(EEPROM_TIME_ADDRESS, 0); + } + seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); + printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); +} + +prog_char str_time_monitor_arg0[] = "time_monitor"; +parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0); +prog_char str_time_monitor_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1); + +prog_char help_time_monitor[] = "Show since how long we are running"; +parse_pgm_inst_t cmd_time_monitor = { + .f = cmd_time_monitor_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_time_monitor, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_time_monitor_arg0, + (prog_void *)&cmd_time_monitor_arg1, + NULL, + }, +}; + + +/**********************************************************/ +/* Scanner */ + +/* this structure is filled when cmd_scanner is parsed successfully */ +struct cmd_scanner_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_scanner is parsed successfully */ +static void cmd_scanner_parsed(void *parsed_result, void *data) +{ + struct cmd_scanner_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("prepare"))) { + i2c_sensorboard_scanner_prepare(); + } + else if (!strcmp_P(res->arg1, PSTR("stop"))) { + i2c_sensorboard_scanner_stop(); + } + else if (!strcmp_P(res->arg1, PSTR("start"))) { + i2c_sensorboard_scanner_start(); + } + else if (!strcmp_P(res->arg1, PSTR("algo_col"))) { + i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, + 15, 15); + } + else if (!strcmp_P(res->arg1, PSTR("algo_check"))) { + i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX + } + else if (!strcmp_P(res->arg1, PSTR("calib"))) { + i2c_sensorboard_scanner_calib(); + } + else if (!strcmp_P(res->arg1, PSTR("show"))) { + scanner_dump_state(); + } +} + +prog_char str_scanner_arg0[] = "scanner"; +parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0); +prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib"; +parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1); + +prog_char help_scanner[] = "send commands to scanner"; +parse_pgm_inst_t cmd_scanner = { + .f = cmd_scanner_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scanner, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scanner_arg0, + (prog_void *)&cmd_scanner_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Build_Z1 */ + +/* this structure is filled when cmd_build_z1 is parsed successfully */ +struct cmd_build_z1_result { + fixed_string_t arg0; + uint8_t level; + int16_t d1; + int16_t d2; + int16_t d3; +}; + +/* function called when cmd_build_z1 is parsed successfully */ +static void cmd_build_z1_parsed(void *parsed_result, void *data) +{ + struct cmd_build_z1_result *res = parsed_result; + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 400); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + trajectory_d_rel(&mainboard.traj, -200); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE, + I2C_MECHBOARD_MODE_HARVEST); + + while (get_column_count() != 4); + + i2c_mechboard_mode_prepare_build_both(res->level); + time_wait_ms(500); + + trajectory_d_rel(&mainboard.traj, 400); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, -res->d1); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST, + res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST, + 1); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == + I2C_MECHBOARD_MODE_AUTOBUILD, 100); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != + I2C_MECHBOARD_MODE_AUTOBUILD, 10000); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, -res->d2); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(1); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, res->d3); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); +} + +prog_char str_build_z1_arg0[] = "build_z1"; +parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0); +parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8); +parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16); +parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16); +parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16); + +prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)"; +parse_pgm_inst_t cmd_build_z1 = { + .f = cmd_build_z1_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_build_z1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_build_z1_arg0, + (prog_void *)&cmd_build_z1_arg1, + (prog_void *)&cmd_build_z1_arg2, + (prog_void *)&cmd_build_z1_arg3, + (prog_void *)&cmd_build_z1_arg4, + NULL, + }, +}; + +#ifdef TEST_BEACON +/**********************************************************/ +/* Beacon_Opp_Dump */ + +/* this structure is filled when cmd_beacon_opp_dump is parsed successfully */ +struct cmd_beacon_opp_dump_result { + fixed_string_t arg0; +}; + +void beacon_dump_samples(void); + +/* function called when cmd_beacon_opp_dump is parsed successfully */ +static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data) +{ + beacon_dump_samples(); +} + +prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump"; +parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0); + +prog_char help_beacon_opp_dump[] = "Dump beacon samples"; +parse_pgm_inst_t cmd_beacon_opp_dump = { + .f = cmd_beacon_opp_dump_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_beacon_opp_dump, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_beacon_opp_dump_arg0, + NULL, + }, +}; +#endif + +/**********************************************************/ +/* Test */ + +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { + fixed_string_t arg0; + int32_t radius; +}; +void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a); +/* function called when cmd_test is parsed successfully */ +static void cmd_test_parsed(void *parsed_result, void *data) +{ + struct cmd_test_result *res = parsed_result; + double d,a; + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a); + printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a); +} + +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_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32); + +prog_char help_test[] = "Test function"; +parse_pgm_inst_t cmd_test = { + .f = cmd_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_test_arg0, + (prog_void *)&cmd_test_arg1, + NULL, + }, +}; diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c new file mode 100644 index 0000000..f3b38fd --- /dev/null +++ b/projects/microb2010/mainboard/commands_traj.c @@ -0,0 +1,1146 @@ +/* + * 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 + * (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: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cs.h" +#include "cmdline.h" +#include "strat_utils.h" +#include "strat_base.h" +#include "strat_avoid.h" +#include "strat.h" +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" + +/**********************************************************/ +/* Traj_Speeds for trajectory_manager */ + +/* this structure is filled when cmd_traj_speed is parsed successfully */ +struct cmd_traj_speed_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t s; +}; + +/* function called when cmd_traj_speed is parsed successfully */ +static void cmd_traj_speed_parsed(void *parsed_result, void *data) +{ + struct cmd_traj_speed_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("angle"))) { + trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s); + } + else if (!strcmp_P(res->arg1, PSTR("distance"))) { + trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed); + } + /* else it is a "show" */ + + printf_P(PSTR("angle %u, distance %u\r\n"), + mainboard.traj.a_speed, + mainboard.traj.d_speed); +} + +prog_char str_traj_speed_arg0[] = "traj_speed"; +parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0); +prog_char str_traj_speed_arg1[] = "angle#distance"; +parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1); +parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16); + +prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager"; +parse_pgm_inst_t cmd_traj_speed = { + .f = cmd_traj_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg1, + (prog_void *)&cmd_traj_speed_s, + NULL, + }, +}; + +/* show */ + +prog_char str_traj_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg); + +prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager"; +parse_pgm_inst_t cmd_traj_speed_show = { + .f = cmd_traj_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* trajectory window configuration */ + +/* this structure is filled when cmd_trajectory is parsed successfully */ +struct cmd_trajectory_result { + fixed_string_t arg0; + fixed_string_t arg1; + float d_win; + float a_win; + float a_start; +}; + + +/* function called when cmd_trajectory is parsed successfully */ +static void cmd_trajectory_parsed(void * parsed_result, void * data) +{ + struct cmd_trajectory_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + trajectory_set_windows(&mainboard.traj, res->d_win, + res->a_win, res->a_start); + } + + printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win, + DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad)); +} + +prog_char str_trajectory_arg0[] = "trajectory"; +parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0); +prog_char str_trajectory_arg1[] = "set"; +parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1); +parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT); +parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT); +parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT); + +prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)"; +parse_pgm_inst_t cmd_trajectory = { + .f = cmd_trajectory_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_trajectory, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg1, + (prog_void *)&cmd_trajectory_d, + (prog_void *)&cmd_trajectory_a, + (prog_void *)&cmd_trajectory_as, + NULL, + }, +}; + +/* show */ + +prog_char str_trajectory_show_arg[] = "show"; +parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg); + +prog_char help_trajectory_show[] = "Show trajectory window configuration"; +parse_pgm_inst_t cmd_trajectory_show = { + .f = cmd_trajectory_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_trajectory_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* rs_gains configuration */ + +/* this structure is filled when cmd_rs_gains is parsed successfully */ +struct cmd_rs_gains_result { + fixed_string_t arg0; + fixed_string_t arg1; + float left; + float right; +}; + +/* function called when cmd_rs_gains is parsed successfully */ +static void cmd_rs_gains_parsed(void * parsed_result, void * data) +{ + struct cmd_rs_gains_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, + LEFT_ENCODER, res->left); // en augmentant on tourne à gauche + rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + RIGHT_ENCODER, res->right); //en augmentant on tourne à droite + } + printf_P(PSTR("rs_gains set ")); + f64_print(mainboard.rs.left_ext_gain); + printf_P(PSTR(" ")); + f64_print(mainboard.rs.right_ext_gain); + printf_P(PSTR("\r\n")); +} + +prog_char str_rs_gains_arg0[] = "rs_gains"; +parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0); +prog_char str_rs_gains_arg1[] = "set"; +parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1); +parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT); +parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT); + +prog_char help_rs_gains[] = "Set rs_gains (left, right)"; +parse_pgm_inst_t cmd_rs_gains = { + .f = cmd_rs_gains_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs_gains, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg1, + (prog_void *)&cmd_rs_gains_l, + (prog_void *)&cmd_rs_gains_r, + NULL, + }, +}; + +/* show */ + +prog_char str_rs_gains_show_arg[] = "show"; +parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg); + +prog_char help_rs_gains_show[] = "Show rs_gains"; +parse_pgm_inst_t cmd_rs_gains_show = { + .f = cmd_rs_gains_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs_gains_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* track configuration */ + +/* this structure is filled when cmd_track is parsed successfully */ +struct cmd_track_result { + fixed_string_t arg0; + fixed_string_t arg1; + float val; +}; + +/* function called when cmd_track is parsed successfully */ +static void cmd_track_parsed(void * parsed_result, void * data) +{ + struct cmd_track_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM); + } + printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm); +} + +prog_char str_track_arg0[] = "track"; +parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0); +prog_char str_track_arg1[] = "set"; +parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1); +parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT); + +prog_char help_track[] = "Set track in mm"; +parse_pgm_inst_t cmd_track = { + .f = cmd_track_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_track, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg1, + (prog_void *)&cmd_track_val, + NULL, + }, +}; + +/* show */ + +prog_char str_track_show_arg[] = "show"; +parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg); + +prog_char help_track_show[] = "Show track"; +parse_pgm_inst_t cmd_track_show = { + .f = cmd_track_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_track_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* Pt_Lists for testing traj */ + +#define PT_LIST_SIZE 10 +static struct xy_point pt_list[PT_LIST_SIZE]; +static uint16_t pt_list_len = 0; + +/* this structure is filled when cmd_pt_list is parsed successfully */ +struct cmd_pt_list_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t arg2; + int16_t arg3; + int16_t arg4; +}; + +/* function called when cmd_pt_list is parsed successfully */ +static void cmd_pt_list_parsed(void * parsed_result, void * data) +{ + struct cmd_pt_list_result * res = parsed_result; + uint8_t i, why=0; + + if (!strcmp_P(res->arg1, PSTR("append"))) { + res->arg2 = pt_list_len; + } + + if (!strcmp_P(res->arg1, PSTR("insert")) || + !strcmp_P(res->arg1, PSTR("append"))) { + if (res->arg2 > pt_list_len) { + printf_P(PSTR("Index too large\r\n")); + return; + } + if (pt_list_len >= PT_LIST_SIZE) { + printf_P(PSTR("List is too large\r\n")); + return; + } + memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], + PT_LIST_SIZE-1-res->arg2); + pt_list[res->arg2].x = res->arg3; + pt_list[res->arg2].y = res->arg4; + pt_list_len++; + } + else if (!strcmp_P(res->arg1, PSTR("del"))) { + if (pt_list_len <= 0) { + printf_P(PSTR("Error: list empty\r\n")); + return; + } + if (res->arg2 > pt_list_len) { + printf_P(PSTR("Index too large\r\n")); + return; + } + memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], + (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point)); + pt_list_len--; + } + else if (!strcmp_P(res->arg1, PSTR("reset"))) { + pt_list_len = 0; + } + + /* else it is a "show" or a "start" */ + if (pt_list_len == 0) { + printf_P(PSTR("List empty\r\n")); + return; + } + for (i=0 ; iarg1, PSTR("start"))) { + trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y); + why = wait_traj_end(0xFF); /* all */ + } + else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { + while (1) { + why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF); + printf("next point\r\n"); + if (why != END_OBSTACLE) + break; + } + } + if (why & (~(END_TRAJ | END_NEAR))) + trajectory_stop(&mainboard.traj); + } +} + +prog_char str_pt_list_arg0[] = "pt_list"; +parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0); +prog_char str_pt_list_arg1[] = "insert"; +parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1); +parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16); +parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16); +parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16); + +prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)"; +parse_pgm_inst_t cmd_pt_list = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1, + (prog_void *)&cmd_pt_list_arg2, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, + NULL, + }, +}; + +/* append */ + +prog_char str_pt_list_arg1_append[] = "append"; +parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append); + +prog_char help_pt_list_append[] = "Append point in pt_list (x,y)"; +parse_pgm_inst_t cmd_pt_list_append = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_append, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1_append, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, + NULL, + }, +}; + +/* del */ + +prog_char str_pt_list_del_arg[] = "del"; +parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg); + +prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)"; +parse_pgm_inst_t cmd_pt_list_del = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_del, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_del_arg, + (prog_void *)&cmd_pt_list_arg2, + NULL, + }, +}; +/* show */ + +prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start"; +parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg); + +prog_char help_pt_list_show[] = "Show, start or reset pt_list"; +parse_pgm_inst_t cmd_pt_list_show = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* Goto function */ + +/* this structure is filled when cmd_goto is parsed successfully */ +struct cmd_goto_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; +}; + +/* function called when cmd_goto is parsed successfully */ +static void cmd_goto_parsed(void * parsed_result, void * data) +{ + struct cmd_goto_result * res = parsed_result; + uint8_t err; + microseconds t1, t2; + + interrupt_traj_reset(); + if (!strcmp_P(res->arg1, PSTR("a_rel"))) { + trajectory_a_rel(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("d_rel"))) { + trajectory_d_rel(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("a_abs"))) { + trajectory_a_abs(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) { + trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) { + trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) { + trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) { + trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("avoid"))) { + err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF); + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + } + else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) { + err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF); + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) { + trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) { + trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("da_rel"))) { + trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3); + } + t1 = time_get_us2(); + while ((err = test_traj_end(0xFF)) == 0) { + t2 = time_get_us2(); + if (t2 - t1 > 200000) { + dump_cs_debug("angle", &mainboard.angle.cs); + dump_cs_debug("distance", &mainboard.distance.cs); + t1 = t2; + } + } + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + printf_P(PSTR("returned %s\r\n"), get_err(err)); +} + +prog_char str_goto_arg0[] = "goto"; +parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0); +prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs"; +parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a); +parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32); + +/* 1 params */ +prog_char help_goto1[] = "Change orientation of the mainboard"; +parse_pgm_inst_t cmd_goto1 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_a, + (prog_void *)&cmd_goto_arg2, + NULL, + }, +}; + +prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#xy_abs_fow#xy_abs_back#da_rel#a_to_xy#avoid#avoid_bw#a_behind_xy"; +parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b); +parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32); + +/* 2 params */ +prog_char help_goto2[] = "Go to a (x,y) or (d,a) position"; +parse_pgm_inst_t cmd_goto2 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_b, + (prog_void *)&cmd_goto_arg2, + (prog_void *)&cmd_goto_arg3, + NULL, + }, +}; + +/**********************************************************/ +/* Position tests */ + +/* this structure is filled when cmd_position is parsed successfully */ +struct cmd_position_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; +}; + +#define AUTOPOS_SPEED_FAST 200 +static void auto_position(void) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + + interrupt_traj_reset(); + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); + + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + if (err == END_INTR) + goto intr; + wait_ms(100); + strat_reset_pos(ROBOT_LENGTH/2, 0, 0); + + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + + trajectory_a_rel(&mainboard.traj, COLOR_A(90)); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + if (err == END_INTR) + goto intr; + wait_ms(100); + strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2), + COLOR_A(90)); + + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + wait_ms(100); + + trajectory_a_rel(&mainboard.traj, COLOR_A(-40)); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + wait_ms(100); + + strat_set_speed(old_spdd, old_spda); + return; + +intr: + strat_hardstop(); + strat_set_speed(old_spdd, old_spda); +} + +/* function called when cmd_position is parsed successfully */ +static void cmd_position_parsed(void * parsed_result, void * data) +{ + struct cmd_position_result * res = parsed_result; + + /* display raw position values */ + if (!strcmp_P(res->arg1, PSTR("reset"))) { + position_set(&mainboard.pos, 0, 0, 0); + } + else if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) { + mainboard.our_color = I2C_COLOR_GREEN; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + auto_position(); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { + mainboard.our_color = I2C_COLOR_RED; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + auto_position(); + } + + /* else it's just a "show" */ + printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), + position_get_x_double(&mainboard.pos), + position_get_y_double(&mainboard.pos), + DEG(position_get_a_rad_double(&mainboard.pos))); +} + +prog_char str_position_arg0[] = "position"; +parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0); +prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red"; +parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1); + +prog_char help_position[] = "Show/reset (x,y,a) position"; +parse_pgm_inst_t cmd_position = { + .f = cmd_position_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_position, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1, + NULL, + }, +}; + + +prog_char str_position_arg1_set[] = "set"; +parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set); +parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32); +parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32); +parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32); + +prog_char help_position_set[] = "Set (x,y,a) position"; +parse_pgm_inst_t cmd_position_set = { + .f = cmd_position_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_position_set, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1_set, + (prog_void *)&cmd_position_arg2, + (prog_void *)&cmd_position_arg3, + (prog_void *)&cmd_position_arg4, + NULL, + }, +}; + + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_infos is parsed successfully */ +struct cmd_strat_infos_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_strat_infos is parsed successfully */ +static void cmd_strat_infos_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_infos_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + strat_reset_infos(); + } + strat_infos.dump_enabled = 1; + strat_dump_infos(__FUNCTION__); +} + +prog_char str_strat_infos_arg0[] = "strat_infos"; +parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0); +prog_char str_strat_infos_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1); + +prog_char help_strat_infos[] = "reset/show strat_infos"; +parse_pgm_inst_t cmd_strat_infos = { + .f = cmd_strat_infos_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_infos, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_infos_arg0, + (prog_void *)&cmd_strat_infos_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf is parsed successfully */ +struct cmd_strat_conf_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_strat_conf is parsed successfully */ +static void cmd_strat_conf_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("base"))) { + strat_infos.conf.flags = 0; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("big3"))) { + strat_infos.conf.flags = + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_BIG_3_TEMPLE; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("base_check"))) { + strat_infos.conf.flags = 0; + strat_infos.conf.scan_our_min_time = 35; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("big3_check"))) { + strat_infos.conf.flags = + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_BIG_3_TEMPLE; + strat_infos.conf.scan_our_min_time = 35; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) { + strat_infos.conf.flags = + STRAT_CONF_TAKE_ONE_LINTEL | + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_EARLY_SCAN | + STRAT_CONF_PUSH_OPP_COLS; + strat_infos.conf.scan_our_min_time = 50; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 15; + strat_infos.conf.delay_between_opp_scan = 90; + strat_infos.conf.wait_opponent = 5; + } + else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) { + strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 30; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) { + strat_infos.conf.flags = + STRAT_CONF_ONLY_ONE_ON_DISC; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0); +prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc"; +parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1); + +prog_char help_strat_conf[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf = { + .f = cmd_strat_conf_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf_arg0, + (prog_void *)&cmd_strat_conf_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf2 is parsed successfully */ +struct cmd_strat_conf2_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_strat_conf2 is parsed successfully */ +static void cmd_strat_conf2_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf2_result *res = parsed_result; + uint8_t on, bit = 0; + + if (!strcmp_P(res->arg2, PSTR("on"))) + on = 1; + else + on = 0; + + if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc"))) + bit = STRAT_CONF_ONLY_ONE_ON_DISC; + else if (!strcmp_P(res->arg1, PSTR("bypass_static2"))) + bit = STRAT_CONF_BYPASS_STATIC2; + else if (!strcmp_P(res->arg1, PSTR("take_one_lintel"))) + bit = STRAT_CONF_TAKE_ONE_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail"))) + bit = STRAT_CONF_TAKE_ONE_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("store_static2"))) + bit = STRAT_CONF_STORE_STATIC2; + else if (!strcmp_P(res->arg1, PSTR("big3_temple"))) + bit = STRAT_CONF_BIG_3_TEMPLE; + else if (!strcmp_P(res->arg1, PSTR("early_opp_scan"))) + bit = STRAT_CONF_EARLY_SCAN; + else if (!strcmp_P(res->arg1, PSTR("push_opp_cols"))) + bit = STRAT_CONF_PUSH_OPP_COLS; + + if (on) + strat_infos.conf.flags |= bit; + else + strat_infos.conf.flags &= (~bit); + + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf2_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0); +prog_char str_strat_conf2_arg1[] = "push_opp_cols#one_temple_on_disc#bypass_static2#take_one_lintel#skip_when_check_fail#store_static2#big3_temple#early_opp_scan"; +parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1); +prog_char str_strat_conf2_arg2[] = "on#off"; +parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2); + + +prog_char help_strat_conf2[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf2 = { + .f = cmd_strat_conf2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf2_arg0, + (prog_void *)&cmd_strat_conf2_arg1, + (prog_void *)&cmd_strat_conf2_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf3 is parsed successfully */ +struct cmd_strat_conf3_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t arg2; +}; + +/* function called when cmd_strat_conf3 is parsed successfully */ +static void cmd_strat_conf3_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf3_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.scan_opp_min_time = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.delay_between_opp_scan = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.scan_our_min_time = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.delay_between_our_scan = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) { + strat_infos.conf.wait_opponent = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) { + strat_infos.conf.lintel_min_time = res->arg2; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf3_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0); +prog_char str_strat_conf3_arg1[] = "lintel_min_time#scan_opponent_min_time#delay_between_opponent_scan#scan_our_min_time#delay_between_our_scan#wait_opponent"; +parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1); +parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16); + +prog_char help_strat_conf3[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf3 = { + .f = cmd_strat_conf3_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf3, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf3_arg0, + (prog_void *)&cmd_strat_conf3_arg1, + (prog_void *)&cmd_strat_conf3_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf4 is parsed successfully */ +struct cmd_strat_conf4_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; +}; + +/* function called when cmd_strat_conf4 is parsed successfully */ +static void cmd_strat_conf4_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf4_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) { + strat_infos.conf.scan_opp_angle = res->arg2; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf4_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0); +prog_char str_strat_conf4_arg1[] = "scan_opponent_angle"; +parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1); +parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16); + +prog_char help_strat_conf4[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf4 = { + .f = cmd_strat_conf4_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf4, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf4_arg0, + (prog_void *)&cmd_strat_conf4_arg1, + (prog_void *)&cmd_strat_conf4_arg2, + NULL, + }, +}; + + +/**********************************************************/ +/* Subtraj */ + +/* this structure is filled when cmd_subtraj is parsed successfully */ +struct cmd_subtraj_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; + int32_t arg5; +}; + +/* function called when cmd_subtraj is parsed successfully */ +static void cmd_subtraj_parsed(void *parsed_result, void *data) +{ + struct cmd_subtraj_result *res = parsed_result; + uint8_t err = 0; + struct column_dispenser *disp; + + if (strcmp_P(res->arg1, PSTR("static")) == 0) { + err = strat_static_columns(res->arg2); + } + else if (strcmp_P(res->arg1, PSTR("static2")) == 0) { + strat_infos.s_cols.configuration = res->arg2; + switch (res->arg2) { + case 1: + position_set(&mainboard.pos, 1398, + COLOR_Y(1297), COLOR_A(-66)); + break; + case 2: + position_set(&mainboard.pos, 1232, + COLOR_Y(1051), COLOR_A(4)); + break; + case 3: + position_set(&mainboard.pos, 1232, + COLOR_Y(1043), COLOR_A(5)); + break; + case 4: + position_set(&mainboard.pos, 1346, + COLOR_Y(852), COLOR_A(57)); + break; + default: + return; + } + if (res->arg2 == 1 && res->arg3 == 1) { + strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE; + } + if (res->arg2 == 1 && res->arg3 == 2) { + strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE; + } + err = strat_static_columns_pass2(); + } + else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) { + err = strat_goto_lintel_disp(&strat_infos.l1); + } + else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) { + err = strat_goto_lintel_disp(&strat_infos.l2); + } + else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) { + disp = &strat_infos.c1; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) { + disp = &strat_infos.c2; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) { + disp = &strat_infos.c3; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("disc")) == 0) { + if (res->arg2 == 0) { + printf_P(PSTR("bad level\r\n")); + return; + } + err = strat_goto_disc(res->arg2); + } + + printf_P(PSTR("substrat returned %s\r\n"), get_err(err)); + trajectory_hardstop(&mainboard.traj); +} + +prog_char str_subtraj_arg0[] = "subtraj"; +parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0); +prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2"; +parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1); +parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32); +parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32); +parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32); +parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32); + +prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)"; +parse_pgm_inst_t cmd_subtraj = { + .f = cmd_subtraj_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_subtraj, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_subtraj_arg0, + (prog_void *)&cmd_subtraj_arg1, + (prog_void *)&cmd_subtraj_arg2, + (prog_void *)&cmd_subtraj_arg3, + (prog_void *)&cmd_subtraj_arg4, + (prog_void *)&cmd_subtraj_arg5, + NULL, + }, +}; diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c new file mode 100644 index 0000000..ec70f9a --- /dev/null +++ b/projects/microb2010/mainboard/cs.c @@ -0,0 +1,239 @@ +/* + * 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 + * (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: cs.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "strat.h" +#include "actuator.h" + +/* called every 5 ms */ +static void do_cs(void *dummy) +{ + static uint16_t cpt = 0; + static int32_t old_a = 0, old_d = 0; + + /* read encoders */ + if (mainboard.flags & DO_ENCODERS) { + encoders_spi_manage(NULL); + } + + /* XXX there is an issue which is probably related to avr-libc + * 1.6.2 (debian): this code using fixed_point lib does not + * work with it */ + /* robot system, conversion to angle,distance */ + if (mainboard.flags & DO_RS) { + int16_t a,d; + rs_update(&mainboard.rs); /* takes about 0.5 ms */ + /* process and store current speed */ + a = rs_get_angle(&mainboard.rs); + d = rs_get_distance(&mainboard.rs); + mainboard.speed_a = a - old_a; + mainboard.speed_d = d - old_d; + old_a = a; + old_d = d; + } + + /* control system */ + if (mainboard.flags & DO_CS) { + if (mainboard.angle.on) + cs_manage(&mainboard.angle.cs); + if (mainboard.distance.on) + cs_manage(&mainboard.distance.cs); + } + if ((cpt & 1) && (mainboard.flags & DO_POS)) { + /* about 1.5ms (worst case without centrifugal force + * compensation) */ + position_manage(&mainboard.pos); + } + if (mainboard.flags & DO_BD) { + bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); + bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); + } + if (mainboard.flags & DO_TIMER) { + uint8_t second; + /* the robot should stop correctly in the strat, but + * in some cases, we must force the stop from an + * interrupt */ + second = time_get_s(); + if (second >= MATCH_TIME + 2) { + pwm_ng_set(LEFT_PWM, 0); + pwm_ng_set(RIGHT_PWM, 0); + printf_P(PSTR("END OF TIME\r\n")); + while(1); + } + } + /* brakes */ + if (mainboard.flags & DO_POWER) + BRAKE_OFF(); + else + BRAKE_ON(); + cpt++; +} + +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", + 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_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 */ + rs_init(&mainboard.rs); + rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM); + 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 * 1.0015); + rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + RIGHT_ENCODER, IMP_COEF * -1.006); + /* rs will use external encoders */ + rs_set_flags(&mainboard.rs, RS_USE_EXT); + + /* POSITION MANAGER */ + position_init(&mainboard.pos); + position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM); + position_set_related_robot_system(&mainboard.pos, &mainboard.rs); + position_set_centrifugal_coef(&mainboard.pos, 0.000016); + position_use_ext(&mainboard.pos); + + /* TRAJECTORY MANAGER */ + trajectory_init(&mainboard.traj); + 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, SPEED_DIST_FAST, SPEED_ANGLE_FAST); /* d, a */ + /* distance window, angle window, angle start */ + trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.); + + /* ---- CS angle */ + /* PID */ + pid_init(&mainboard.angle.pid); + pid_set_gains(&mainboard.angle.pid, 500, 10, 7000); + pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095); + pid_set_out_shift(&mainboard.angle.pid, 10); + pid_set_derivate_filter(&mainboard.angle.pid, 4); + + /* QUADRAMP */ + quadramp_init(&mainboard.angle.qr); + quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */ + + /* CS */ + cs_init(&mainboard.angle.cs); + cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr); + cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid); + cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs); + cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs); + cs_set_consign(&mainboard.angle.cs, 0); + + /* Blocking detection */ + bd_init(&mainboard.angle.bd); + bd_set_speed_threshold(&mainboard.angle.bd, 80); + bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50); + + /* ---- CS distance */ + /* PID */ + pid_init(&mainboard.distance.pid); + pid_set_gains(&mainboard.distance.pid, 500, 10, 7000); + pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095); + pid_set_out_shift(&mainboard.distance.pid, 10); + pid_set_derivate_filter(&mainboard.distance.pid, 6); + + /* QUADRAMP */ + quadramp_init(&mainboard.distance.qr); + quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */ + + /* CS */ + cs_init(&mainboard.distance.cs); + cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr); + cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid); + cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs); + cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs); + cs_set_consign(&mainboard.distance.cs, 0); + + /* Blocking detection */ + bd_init(&mainboard.distance.bd); + bd_set_speed_threshold(&mainboard.distance.bd, 60); + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); + + /* set them on !! */ + mainboard.angle.on = 1; + mainboard.distance.on = 1; + + + scheduler_add_periodical_event_priority(do_cs, NULL, + 5000L / SCHEDULER_UNIT, + CS_PRIO); +} diff --git a/projects/microb2010/mainboard/cs.h b/projects/microb2010/mainboard/cs.h new file mode 100644 index 0000000..d3d1fa9 --- /dev/null +++ b/projects/microb2010/mainboard/cs.h @@ -0,0 +1,26 @@ +/* + * 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 + * (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: cs.h,v 1.3 2009-03-29 18:42:41 zer0 Exp $ + * + */ + +void microb_cs_init(void); +void dump_cs(const char *name, struct cs *cs); +void dump_cs_debug(const char *name, struct cs *cs); +void dump_pid(const char *name, struct pid_filter *pid); diff --git a/projects/microb2010/mainboard/diagnostic_config.h b/projects/microb2010/mainboard/diagnostic_config.h new file mode 100644 index 0000000..9d9c3a5 --- /dev/null +++ b/projects/microb2010/mainboard/diagnostic_config.h @@ -0,0 +1,44 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: diagnostic_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $ + * + */ + +#ifndef _DEBUG_CONFIG_ +#define _DEBUG_CONFIG_ 1.0 // version + + +/** port line definition for the show_int_loop() function */ +/* undefine it to disable this functionnality */ +#define INTERRUPT_SHOW_PORT PORTA +#define INTERRUPT_SHOW_BIT 3 + + + +/** memory mark for the min_stack_space_available() function + the ram is filled with this value after a reset ! */ +#define MARK 0x55 + +/** the mark is inserted in whole RAM if this is enabled + (could lead to problems if you need to hold values through a reset...) + so it's better to disable it. + stack counting is not affected */ +//#define DIAG_FILL_ENTIRE_RAM + + +#endif //_DEBUG_CONFIG_ diff --git a/projects/microb2010/mainboard/encoders_spi_config.h b/projects/microb2010/mainboard/encoders_spi_config.h new file mode 100644 index 0000000..6528244 --- /dev/null +++ b/projects/microb2010/mainboard/encoders_spi_config.h @@ -0,0 +1,33 @@ +/* + * 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 + * (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: encoders_spi_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $ + * + */ +#ifndef _ENCODERS_SPI_CONFIG_H_ +#define _ENCODERS_SPI_CONFIG_H_ + +#define ENCODERS_SPI_NUMBER 4 +#define ENCODERS_SPI_SS_PORT SS_PORT /* PORTB on atmega2560 */ +#define ENCODERS_SPI_SS_BIT SS_BIT /* 0 on atmega2560 */ + +/* see spi configuration */ +#define ENCODERS_SPI_CLK_RATE SPI_CLK_RATE_16 +#define ENCODERS_SPI_FORMAT SPI_FORMAT_3 +#define ENCODERS_SPI_DATA_ORDER SPI_LSB_FIRST + +#endif diff --git a/projects/microb2010/mainboard/error_config.h b/projects/microb2010/mainboard/error_config.h new file mode 100644 index 0000000..7aad86a --- /dev/null +++ b/projects/microb2010/mainboard/error_config.h @@ -0,0 +1,31 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: error_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $ + * + */ + +#ifndef _ERROR_CONFIG_ +#define _ERROR_CONFIG_ + +/** enable the dump of the comment */ +#define ERROR_DUMP_TEXTLOG + +/** enable the dump of filename and line number */ +#define ERROR_DUMP_FILE_LINE + +#endif diff --git a/projects/microb2010/mainboard/i2c_config.h b/projects/microb2010/mainboard/i2c_config.h new file mode 100644 index 0000000..1617810 --- /dev/null +++ b/projects/microb2010/mainboard/i2c_config.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: i2c_config.h,v 1.2 2009-03-05 23:01:32 zer0 Exp $ + * + */ + + +#define I2C_BITRATE 1 // divider dor i2c baudrate, see TWBR in doc +#define I2C_PRESCALER 3 // prescaler config, rate = 2^(n*2) + +/* Size of transmission buffer */ +#define I2C_SEND_BUFFER_SIZE 32 + +/* Size of reception buffer */ +#define I2C_RECV_BUFFER_SIZE 32 diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c new file mode 100644 index 0000000..a503f6d --- /dev/null +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -0,0 +1,687 @@ +/* + * 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 + * (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: i2c_protocol.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "sensor.h" +#include "i2c_protocol.h" + +#define I2C_STATE_MAX 4 + +#define I2C_TIMEOUT 100 /* ms */ +#define I2C_MAX_ERRORS 40 + +static volatile uint8_t i2c_poll_num = 0; +static volatile uint8_t i2c_state = 0; +static volatile uint16_t i2c_errors = 0; + +#define OP_READY 0 /* no i2c op running */ +#define OP_POLL 1 /* a user command is running */ +#define OP_CMD 2 /* a polling (req / ans) is running */ + +static volatile uint8_t running_op = OP_READY; + +#define I2C_MAX_LOG 3 +static uint8_t error_log = 0; + +/* used for commands */ +uint8_t command_buf[I2C_SEND_BUFFER_SIZE]; +volatile int8_t command_dest=-1; +volatile uint8_t command_size=0; + +static int8_t i2c_req_mechboard_status(void); +static int8_t i2c_req_sensorboard_status(void); + +#define I2C_ERROR(args...) do { \ + if (error_log < I2C_MAX_LOG) { \ + ERROR(E_USER_I2C_PROTO, args); \ + error_log ++; \ + if (error_log == I2C_MAX_LOG) { \ + ERROR(E_USER_I2C_PROTO, \ + "i2c logs are now warnings"); \ + } \ + } \ + else \ + WARNING(E_USER_I2C_PROTO, args); \ + } while(0) + +void i2c_protocol_init(void) +{ +} + +void i2c_protocol_debug(void) +{ + printf_P(PSTR("I2C protocol debug infos:\r\n")); + printf_P(PSTR(" i2c_state=%d\r\n"), i2c_state); + printf_P(PSTR(" i2c_errors=%d\r\n"), i2c_errors); + printf_P(PSTR(" running_op=%d\r\n"), running_op); + printf_P(PSTR(" command_size=%d\r\n"), command_size); + printf_P(PSTR(" command_dest=%d\r\n"), command_dest); + printf_P(PSTR(" i2c_status=%x\r\n"), i2c_status()); +} + +static void i2cproto_next_state(uint8_t inc) +{ + i2c_state += inc; + if (i2c_state >= I2C_STATE_MAX) { + i2c_state = 0; + i2c_poll_num ++; + } +} + +void i2cproto_wait_update(void) +{ + uint8_t poll_num; + poll_num = i2c_poll_num; + WAIT_COND_OR_TIMEOUT((i2c_poll_num-poll_num) > 1, 150); +} + +/* called periodically : the goal of this 'thread' is to send requests + * and read answers on i2c slaves in the correct order. */ +void i2c_poll_slaves(void *dummy) +{ + uint8_t flags; + int8_t err; + static uint8_t a = 0; + + a++; + if (a & 0x4) + LED2_TOGGLE(); + + /* already running */ + IRQ_LOCK(flags); + if (running_op != OP_READY) { + IRQ_UNLOCK(flags); + return; + } + + /* if a command is ready to be sent, so send it */ + if (command_size) { + running_op = OP_CMD; + err = i2c_send(command_dest, command_buf, command_size, + I2C_CTRL_GENERIC); + if (err < 0) + goto error; + IRQ_UNLOCK(flags); + return; + } + + /* no command, so do the polling */ + running_op = OP_POLL; + + switch(i2c_state) { + + /* poll status of mechboard */ +#define I2C_REQ_MECHBOARD 0 + case I2C_REQ_MECHBOARD: + if ((err = i2c_req_mechboard_status())) + goto error; + break; + +#define I2C_ANS_MECHBOARD 1 + case I2C_ANS_MECHBOARD: + if ((err = i2c_recv(I2C_MECHBOARD_ADDR, + sizeof(struct i2c_ans_mechboard_status), + I2C_CTRL_GENERIC))) + goto error; + break; + + /* poll status of sensorboard */ +#define I2C_REQ_SENSORBOARD 2 + case I2C_REQ_SENSORBOARD: + if ((err = i2c_req_sensorboard_status())) + goto error; + break; + +#define I2C_ANS_SENSORBOARD 3 + case I2C_ANS_SENSORBOARD: + if ((err = i2c_recv(I2C_SENSORBOARD_ADDR, + sizeof(struct i2c_ans_sensorboard_status), + I2C_CTRL_GENERIC))) + goto error; + break; + + /* nothing, go to the first request */ + default: + i2c_state = 0; + running_op = OP_READY; + } + IRQ_UNLOCK(flags); + + return; + + error: + running_op = OP_READY; + IRQ_UNLOCK(flags); + i2c_errors++; + if (i2c_errors > I2C_MAX_ERRORS) { + I2C_ERROR("I2C send is_cmd=%d proto_state=%d " + "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status()); + i2c_reset(); + i2c_errors = 0; + } +} + +/* called when the xmit is finished */ +void i2c_sendevent(int8_t size) +{ + if (size > 0) { + if (running_op == OP_POLL) { + i2cproto_next_state(1); + } + else + command_size = 0; + } + else { + i2c_errors++; + NOTICE(E_USER_I2C_PROTO, "send error state=%d size=%d " + "op=%d", i2c_state, size, running_op); + if (i2c_errors > I2C_MAX_ERRORS) { + I2C_ERROR("I2C error, slave not ready"); + i2c_reset(); + i2c_errors = 0; + } + + if (running_op == OP_POLL) { + /* skip associated answer */ + i2cproto_next_state(2); + } + } + running_op = OP_READY; +} + +/* called rx event */ +void i2c_recvevent(uint8_t * buf, int8_t size) +{ + if (running_op == OP_POLL) + i2cproto_next_state(1); + + /* recv is only trigged after a poll */ + running_op = OP_READY; + + if (size < 0) { + goto error; + } + + switch (buf[0]) { + + case I2C_ANS_MECHBOARD_STATUS: { + struct i2c_ans_mechboard_status * ans = + (struct i2c_ans_mechboard_status *)buf; + + if (size != sizeof (*ans)) + goto error; + + /* status */ + mechboard.mode = ans->mode; + mechboard.status = ans->status; + mechboard.lintel_count = ans->lintel_count; + mechboard.column_flags = ans->column_flags; + /* pumps pwm */ + mechboard.pump_left1 = ans->pump_left1; + mechboard.pump_left2 = ans->pump_left2; + mechboard.pump_right1 = ans->pump_right1; + mechboard.pump_right2 = ans->pump_right2; + pwm_ng_set(LEFT_PUMP1_PWM, mechboard.pump_left1); + pwm_ng_set(LEFT_PUMP2_PWM, mechboard.pump_left2); + /* pumps current */ + mechboard.pump_right1_current = ans->pump_right1_current; + mechboard.pump_right2_current = ans->pump_right2_current; + /* servos */ + mechboard.servo_lintel_left = ans->servo_lintel_left; + mechboard.servo_lintel_right = ans->servo_lintel_right; + pwm_ng_set(&gen.servo2, mechboard.servo_lintel_right); + pwm_ng_set(&gen.servo3, mechboard.servo_lintel_left); + + break; + } + + case I2C_ANS_SENSORBOARD_STATUS: { + struct i2c_ans_sensorboard_status * ans = + (struct i2c_ans_sensorboard_status *)buf; + + if (size != sizeof (*ans)) + goto error; + sensorboard.status = ans->status; + sensorboard.opponent_x = ans->opponent_x; + sensorboard.opponent_y = ans->opponent_y; + sensorboard.opponent_a = ans->opponent_a; + sensorboard.opponent_d = ans->opponent_d; + + sensorboard.scan_status = ans->scan_status; + sensorboard.dropzone_h = ans->dropzone_h; + sensorboard.dropzone_x = ans->dropzone_x; + sensorboard.dropzone_y = ans->dropzone_y; + break; + } + + default: + break; + } + + return; + error: + i2c_errors++; + NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d", + i2c_state, running_op); + if (i2c_errors > I2C_MAX_ERRORS) { + I2C_ERROR("I2C error, slave not ready"); + i2c_reset(); + i2c_errors = 0; + } +} + +void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c) +{ +} + + + +/* ******** ******** ******** ******** */ +/* commands */ +/* ******** ******** ******** ******** */ + + +static int8_t +i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) +{ + uint8_t flags; + microseconds us = time_get_us2(); + + while ((time_get_us2() - us) < (I2C_TIMEOUT)*1000L) { + IRQ_LOCK(flags); + if (command_size == 0) { + memcpy(command_buf, buf, size); + command_size = size; + command_dest = addr; + IRQ_UNLOCK(flags); + return 0; + } + IRQ_UNLOCK(flags); + } + /* this should not happen... except if we are called from an + * interrupt context, but it's forbidden */ + I2C_ERROR("I2C command send failed"); + return -EBUSY; +} + +static int8_t i2c_req_mechboard_status(void) +{ + struct i2c_req_mechboard_status buf; + int8_t err; + + buf.hdr.cmd = I2C_REQ_MECHBOARD_STATUS; + buf.pump_left1_current = sensor_get_adc(ADC_CSENSE3); + buf.pump_left2_current = sensor_get_adc(ADC_CSENSE4); + err = i2c_send(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, + sizeof(buf), I2C_CTRL_GENERIC); + + return err; +} + +static int8_t i2c_req_sensorboard_status(void) +{ + struct i2c_req_sensorboard_status buf; + + buf.hdr.cmd = I2C_REQ_SENSORBOARD_STATUS; + /* robot position */ + buf.x = position_get_x_s16(&mainboard.pos); + buf.y = position_get_y_s16(&mainboard.pos); + buf.a = position_get_a_deg_s16(&mainboard.pos); + /* pickup wheels */ + buf.enable_pickup_wheels = mainboard.enable_pickup_wheels; + + return i2c_send(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, + sizeof(buf), I2C_CTRL_GENERIC); +} + +int8_t i2c_set_color(uint8_t addr, uint8_t color) +{ + struct i2c_cmd_generic_color buf; + + if (addr == I2C_SENSORBOARD_ADDR) + return 0; /* XXX disabled for now */ + buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR; + buf.color = color; + return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state) +{ + struct i2c_cmd_led_control buf; + buf.hdr.cmd = I2C_CMD_GENERIC_LED_CONTROL; + buf.led_num = led; + buf.state = state; + return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_manual(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_MANUAL; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_harvest(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_HARVEST; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_lazy_harvest(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; + buf.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; + buf.prep_pickup.side = side; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC; + buf.prep_pickup.side = side; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; + buf.prep_pickup.next_mode = next_mode; + buf.prep_pickup.side = side; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_pickup(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PICKUP; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_eject(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_EJECT; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_manivelle(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_MANIVELLE; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_push_temple(uint8_t level) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE; + buf.push_temple.level = level; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,sizeof(buf)); +} + +int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD; + buf.prep_build.level_l = level; + buf.prep_build.level_r = level; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD; + buf.prep_build.level_l = level_l; + buf.prep_build.level_r = level_r; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE; + buf.prep_inside.level_l = level; + buf.prep_inside.level_r = level; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE; + buf.prep_inside.level_l = level_l; + buf.prep_inside.level_r = level_r; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD; + buf.autobuild.level_left = level; + buf.autobuild.level_right = level; + buf.autobuild.count_left = 2; + buf.autobuild.count_right = 2; + buf.autobuild.do_lintel = 1; + buf.autobuild.distance_left = 210; + buf.autobuild.distance_right = 210; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_autobuild(uint8_t level_l, uint8_t count_l, + uint8_t dist_l, + uint8_t level_r, uint8_t count_r, + uint8_t dist_r, + uint8_t do_lintel) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD; + buf.autobuild.level_left = level_l; + buf.autobuild.level_right = level_r; + buf.autobuild.count_left = count_l; + buf.autobuild.count_right = count_r; + buf.autobuild.distance_left = dist_l; + buf.autobuild.distance_right = dist_r; + buf.autobuild.do_lintel = do_lintel; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_init(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_INIT; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_prepare_get_lintel(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_get_lintel(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_GET_LINTEL; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_put_lintel(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_PUT_LINTEL; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_clear(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_CLEAR; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_loaded(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_LOADED; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_store(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_STORE; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_mechboard_mode_lazy_pickup(void) +{ + struct i2c_cmd_mechboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE; + buf.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; + return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_sensorboard_set_beacon(uint8_t enable) +{ + struct i2c_cmd_sensorboard_start_beacon buf; + buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_BEACON; + buf.enable = enable; + return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_sensorboard_scanner_set(uint8_t mode) +{ + struct i2c_cmd_sensorboard_scanner buf; + buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_SCANNER; + buf.mode = mode; + return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_sensorboard_scanner_calib(void) +{ + struct i2c_cmd_sensorboard_calib_scanner buf; + buf.hdr.cmd = I2C_CMD_SENSORBOARD_CALIB_SCANNER; + return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone, + int16_t x, int16_t y) +{ + struct i2c_cmd_sensorboard_scanner_algo buf; + buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO; + buf.algo = I2C_SCANNER_ALGO_COLUMN_DROPZONE; + buf.drop_zone.working_zone = zone; + buf.drop_zone.center_x = x; + buf.drop_zone.center_y = y; + return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone, + int16_t x, int16_t y) +{ + struct i2c_cmd_sensorboard_scanner_algo buf; + buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO; + buf.algo = I2C_SCANNER_ALGO_TEMPLE_DROPZONE; + buf.drop_zone.working_zone = zone; + buf.drop_zone.center_x = x; + buf.drop_zone.center_y = y; + return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_sensorboard_scanner_algo_check(uint8_t level, + int16_t x, int16_t y) +{ + struct i2c_cmd_sensorboard_scanner_algo buf; + buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO; + buf.algo = I2C_SCANNER_ALGO_CHECK_TEMPLE; + buf.check_temple.level = level; + buf.check_temple.temple_x = x; + buf.check_temple.temple_y = y; + return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h new file mode 100644 index 0000000..0d40205 --- /dev/null +++ b/projects/microb2010/mainboard/i2c_protocol.h @@ -0,0 +1,92 @@ +/* + * 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 + * (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: i2c_protocol.h,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#ifndef _I2C_PROTOCOL_H_ +#define _I2C_PROTOCOL_H_ + +void i2c_protocol_init(void); +void i2c_protocol_debug(void); + +void i2cproto_wait_update(void); +void i2c_poll_slaves(void *dummy); + +void i2c_recvevent(uint8_t *buf, int8_t size); +void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c); +void i2c_sendevent(int8_t size); + +int8_t i2c_set_color(uint8_t addr, uint8_t color); +int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state); + +int8_t i2c_mechboard_mode_manual(void); +int8_t i2c_mechboard_mode_harvest(void); +int8_t i2c_mechboard_mode_lazy_harvest(void); +int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side); +int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode); +int8_t i2c_mechboard_mode_pickup(void); +int8_t i2c_mechboard_mode_eject(void); +int8_t i2c_mechboard_mode_lazy_pickup(void); + +int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level); +int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r); +int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level); +int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r); +int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level); +int8_t i2c_mechboard_mode_autobuild(uint8_t level_l, uint8_t count_l, + uint8_t dist_l, + uint8_t level_r, uint8_t count_r, + uint8_t dist_r, + uint8_t do_lintel); +int8_t i2c_mechboard_mode_init(void); +int8_t i2c_mechboard_mode_eject(void); +int8_t i2c_mechboard_mode_prepare_get_lintel(void); +int8_t i2c_mechboard_mode_get_lintel(void); +int8_t i2c_mechboard_mode_put_lintel(void); +int8_t i2c_mechboard_mode_clear(void); +int8_t i2c_mechboard_mode_loaded(void); +int8_t i2c_mechboard_mode_store(void); +int8_t i2c_mechboard_mode_manivelle(void); +int8_t i2c_mechboard_mode_push_temple(uint8_t level); +int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side); + +int8_t i2c_sensorboard_set_beacon(uint8_t enable); + +int8_t i2c_sensorboard_scanner_set(uint8_t mode); +static inline int8_t i2c_sensorboard_scanner_stop(void) { + return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_STOP); +} +static inline int8_t i2c_sensorboard_scanner_start(void) { + return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_START); +} +static inline int8_t i2c_sensorboard_scanner_prepare(void) { + return i2c_sensorboard_scanner_set(I2C_SENSORBOARD_SCANNER_PREPARE); +} + +int8_t i2c_sensorboard_scanner_calib(void); + +int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone, + int16_t x, int16_t y); +int8_t i2c_sensorboard_scanner_algo_check(uint8_t level, + int16_t x, int16_t y); +int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone, + int16_t x, int16_t y); + + +#endif diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c new file mode 100755 index 0000000..1b4d4b0 --- /dev/null +++ b/projects/microb2010/mainboard/main.c @@ -0,0 +1,295 @@ +/* + * 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 + * (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: main.c,v 1.10 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/eeprom_mapping.h" +#include "../common/i2c_commands.h" + +#include "main.h" +#include "ax12_user.h" +#include "strat.h" +#include "cmdline.h" +#include "sensor.h" +#include "actuator.h" +#include "cs.h" +#include "i2c_protocol.h" + +#if __AVR_LIBC_VERSION__ == 10602UL +#error "won't work with this version" +#endif + +/* 0 means "programmed" + * ---- with 16 Mhz quartz + * CKSEL 3-0 : 0111 + * SUT 1-0 : 10 + * CKDIV8 : 1 + * ---- bootloader + * BOOTZ 1-0 : 01 (4K bootloader) + * BOOTRST : 0 (reset on bootloader) + * ---- jtag + * jtagen : 0 + */ + +struct genboard gen; +struct mainboard mainboard; +struct mechboard mechboard; +struct sensorboard sensorboard; + +/***********************/ + +void bootloader(void) +{ +#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; + TIMSK2 = 0; + TIMSK3 = 0; + TIMSK4 = 0; + TIMSK5 = 0; + EIMSK = 0; + UCSR0B = 0; + UCSR1B = 0; + UCSR2B = 0; + UCSR3B = 0; + SPCR = 0; + TWCR = 0; + ACSR = 0; + ADCSRA = 0; + + EIND = 1; + __asm__ __volatile__ ("ldi r31,0xf8\n"); + __asm__ __volatile__ ("ldi r30,0x00\n"); + __asm__ __volatile__ ("eijmp\n"); + + /* never returns */ +} + +void do_time_monitor(void *dummy) +{ + uint16_t seconds; + seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); + seconds ++; + eeprom_write_word(EEPROM_TIME_ADDRESS, seconds); +} + +void do_led_blink(void *dummy) +{ +#if 1 /* simple blink */ + LED1_TOGGLE(); +#endif +} + +static void main_timer_interrupt(void) +{ + static uint8_t cpt = 0; + cpt++; + sei(); + if ((cpt & 0x3) == 0) + scheduler_interrupt(); +} + +int main(void) +{ + uint16_t seconds; + + /* brake */ + BRAKE_DDR(); + BRAKE_OFF(); + + /* CPLD reset on PG3 */ + DDRG |= 1<<3; + PORTG &= ~(1<<3); /* implicit */ + + /* LEDS */ + DDRJ |= 0x0c; + DDRL = 0xc0; + LED1_OFF(); + LED2_OFF(); + LED3_OFF(); + LED4_OFF(); + + memset(&gen, 0, sizeof(gen)); + memset(&mainboard, 0, sizeof(mainboard)); + mainboard.flags = DO_ENCODERS | DO_RS | + DO_POS | DO_POWER | DO_BD; + sensorboard.opponent_x = I2C_OPPONENT_NOT_THERE; + + /* UART */ + uart_init(); +#if CMDLINE_UART == 3 + fdevopen(uart3_dev_send, uart3_dev_recv); + uart_register_rx_event(3, emergency); +#elif CMDLINE_UART == 1 + fdevopen(uart1_dev_send, uart1_dev_recv); + uart_register_rx_event(1, emergency); +#else +# error not supported +#endif + + //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD); + /* check eeprom to avoid to run the bad program */ + if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != + EEPROM_MAGIC_MAINBOARD) { + sei(); + printf_P(PSTR("Bad eeprom value\r\n")); + while(1); + } + + /* LOGS */ + error_register_emerg(mylog); + error_register_error(mylog); + error_register_warning(mylog); + error_register_notice(mylog); + error_register_debug(mylog); + + /* SPI + ENCODERS */ + encoders_spi_init(); /* this will also init spi hardware */ + + /* I2C */ + i2c_init(I2C_MODE_MASTER, I2C_MAINBOARD_ADDR); + i2c_protocol_init(); + i2c_register_recv_event(i2c_recvevent); + i2c_register_send_event(i2c_sendevent); + + /* TIMER */ + timer_init(); + timer0_register_OV_intr(main_timer_interrupt); + + /* PWM */ + 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, + 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 | + PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5); + PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, + &PORTD, 6); + PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED, + &PORTD, 7); + + + /* servos */ + 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, + TIMER1_PRESCALER_DIV_256); + PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL, + NULL, 0); + PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL, + NULL, 0); + PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL, + NULL, 0); + pwm_ng_set(&gen.servo2, 290); /* right */ + pwm_ng_set(&gen.servo3, 400); /* left */ + /* 2 lintels 180, 485 */ + /* 1 lintel 155, 520 */ + + /* SCHEDULER */ + scheduler_init(); + + scheduler_add_periodical_event_priority(do_led_blink, NULL, + 100000L / SCHEDULER_UNIT, + LED_PRIO); + /* all cs management */ + microb_cs_init(); + + /* sensors, will also init hardware adc */ + sensor_init(); + + /* TIME */ + time_init(TIME_PRIO); + + /* start i2c slave polling */ + scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL, + 8000L / SCHEDULER_UNIT, I2C_POLL_PRIO); + + /* strat */ + gen.logs[0] = E_USER_STRAT; + gen.log_level = 5; + strat_reset_infos(); + + /* strat-related event */ + scheduler_add_periodical_event_priority(strat_event, NULL, + 25000L / SCHEDULER_UNIT, + STRAT_PRIO); + + /* eeprom time monitor */ + scheduler_add_periodical_event_priority(do_time_monitor, NULL, + 1000000L / SCHEDULER_UNIT, + EEPROM_TIME_PRIO); + + sei(); + + printf_P(PSTR("\r\n")); + printf_P(PSTR("Respect et robustesse.\r\n")); + seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); + printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); + cmdline_interact(); + + return 0; +} diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h new file mode 100755 index 0000000..78fccc4 --- /dev/null +++ b/projects/microb2010/mainboard/main.h @@ -0,0 +1,227 @@ +/* + * 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 + * (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: main.h,v 1.10 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#define LED_TOGGLE(port, bit) do { \ + if (port & _BV(bit)) \ + port &= ~_BV(bit); \ + else \ + port |= _BV(bit); \ + } while(0) + +#define LED1_ON() sbi(PORTJ, 2) +#define LED1_OFF() cbi(PORTJ, 2) +#define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) + +#define LED2_ON() sbi(PORTL, 7) +#define LED2_OFF() cbi(PORTL, 7) +#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) + +#define LED3_ON() sbi(PORTJ, 3) +#define LED3_OFF() cbi(PORTJ, 3) +#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) + +#define LED4_ON() sbi(PORTL, 6) +#define LED4_OFF() cbi(PORTL, 6) +#define LED4_TOGGLE() LED_TOGGLE(PORTL, 6) + +#define BRAKE_DDR() do { DDRJ |= 0xF0; } while(0) +#define BRAKE_ON() do { PORTJ |= 0xF0; } while(0) +#define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0) + +/* only 90 seconds, don't forget it :) */ +#define MATCH_TIME 89 + +/* decrease track to decrease angle */ +#define EXT_TRACK_MM 302.0188 +#define VIRTUAL_TRACK_MM EXT_TRACK_MM + +#define ROBOT_LENGTH 320 +#define ROBOT_WIDTH 320 + +/* it is a 2048 imps -> 8192 because we see 1/4 period + * and diameter: 55mm -> perimeter 173mm + * 8192/173 -> 473 */ +/* increase it to go further */ +#define IMP_ENCODERS 2048 +#define WHEEL_DIAMETER_MM 55.0 +#define WHEEL_PERIM_MM (WHEEL_DIAMETER_MM * M_PI) +#define IMP_COEF 10. +#define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF) + +#define LEFT_ENCODER ((void *)1) +#define RIGHT_ENCODER ((void *)0) + +#define LEFT_PWM ((void *)&gen.pwm1_4A) +#define RIGHT_PWM ((void *)&gen.pwm2_4B) + +#define LEFT_PUMP1_PWM ((void *)&gen.pwm3_1A) +#define LEFT_PUMP2_PWM ((void *)&gen.pwm4_1B) + +/** ERROR NUMS */ +#define E_USER_STRAT 194 +#define E_USER_I2C_PROTO 195 +#define E_USER_SENSOR 196 +#define E_USER_CS 197 + +#define LED_PRIO 170 +#define TIME_PRIO 160 +#define ADC_PRIO 120 +#define CS_PRIO 100 +#define STRAT_PRIO 30 +#define I2C_POLL_PRIO 20 +#define EEPROM_TIME_PRIO 10 + +#define CS_PERIOD 5000L + +#define NB_LOGS 4 + +/* generic to all boards */ +struct genboard { + /* command line interface */ + struct rdline rdl; + char prompt[RDLINE_PROMPT_SIZE]; + + /* motors */ + struct pwm_ng pwm1_4A; + struct pwm_ng pwm2_4B; + struct pwm_ng pwm3_1A; + struct pwm_ng pwm4_1B; + + /* servos */ + struct pwm_ng servo1; + struct pwm_ng servo2; + struct pwm_ng servo3; + struct pwm_ng servo4; + + /* ax12 interface */ + AX12 ax12; + + /* log */ + uint8_t logs[NB_LOGS+1]; + uint8_t log_level; + uint8_t debug; +}; + +struct cs_block { + uint8_t on; + struct cs cs; + struct pid_filter pid; + struct quadramp_filter qr; + struct blocking_detection bd; +}; + +/* mainboard specific */ +struct mainboard { +#define DO_ENCODERS 1 +#define DO_CS 2 +#define DO_RS 4 +#define DO_POS 8 +#define DO_BD 16 +#define DO_TIMER 32 +#define DO_POWER 64 + uint8_t flags; /* misc flags */ + + /* control systems */ + struct cs_block angle; + struct cs_block distance; + + /* x,y positionning */ + struct robot_system rs; + struct robot_position pos; + struct trajectory traj; + + /* robot status */ + uint8_t our_color; + volatile int16_t speed_a; /* current angle speed */ + volatile int16_t speed_d; /* current dist speed */ + int32_t pwm_l; /* current left pwm */ + int32_t pwm_r; /* current right pwm */ + uint8_t enable_pickup_wheels; /* these PWM are on sensorboard */ + +}; + +/* state of mechboard, synchronized through i2c */ +struct mechboard { + uint8_t mode; + uint8_t status; + int8_t lintel_count; + uint8_t column_flags; + + /* pwm */ + int16_t pump_left1; + int16_t pump_right1; + int16_t pump_left2; + int16_t pump_right2; + + /* currents (for left arm, we can just read it on adc) */ + int16_t pump_right1_current; + int16_t pump_right2_current; + + /* pwm for lintel servos */ + uint16_t servo_lintel_left; + uint16_t servo_lintel_right; +}; + +/* state of sensorboard, synchronized through i2c */ +struct sensorboard { + uint8_t status; + /* opponent pos */ + int16_t opponent_x; + int16_t opponent_y; + int16_t opponent_a; + int16_t opponent_d; + + /* scanner */ +#define I2C_SCAN_DONE 1 + uint8_t scan_status; +#define I2C_COLUMN_NO_DROPZONE -1 + int8_t dropzone_h; + int16_t dropzone_x; + int16_t dropzone_y; +}; + +extern struct genboard gen; +extern struct mainboard mainboard; +extern struct mechboard mechboard; +extern struct sensorboard sensorboard; + +/* start the bootloader */ +void bootloader(void); + +#define WAIT_COND_OR_TIMEOUT(cond, timeout) \ +({ \ + microseconds __us = time_get_us2(); \ + uint8_t __ret = 1; \ + while(! (cond)) { \ + if (time_get_us2() - __us > (timeout)*1000L) {\ + __ret = 0; \ + break; \ + } \ + } \ + if (__ret) \ + DEBUG(E_USER_STRAT, "cond is true at line %d",\ + __LINE__); \ + else \ + DEBUG(E_USER_STRAT, "timeout at line %d", \ + __LINE__); \ + \ + __ret; \ +}) diff --git a/projects/microb2010/mainboard/obstacle_avoidance_config.h b/projects/microb2010/mainboard/obstacle_avoidance_config.h new file mode 100644 index 0000000..7306668 --- /dev/null +++ b/projects/microb2010/mainboard/obstacle_avoidance_config.h @@ -0,0 +1,25 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: obstacle_avoidance_config.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#define MAX_POLY 3 +#define MAX_PTS 12 +#define MAX_RAYS 150 +#define MAX_CHKPOINTS 7 diff --git a/projects/microb2010/mainboard/pid_config.h b/projects/microb2010/mainboard/pid_config.h new file mode 100755 index 0000000..fa95f08 --- /dev/null +++ b/projects/microb2010/mainboard/pid_config.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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 + * + * + * + */ + +#ifndef PID_CONFIG_H +#define PID_CONFIG_H + +/** the derivate term can be filtered to remove the noise. This value + * is the maxium sample count to keep in memory to do this + * filtering. For an instance of pid, this count is defined o*/ +#define PID_DERIVATE_FILTER_MAX_SIZE 4 + +#endif diff --git a/projects/microb2010/mainboard/rdline_config.h b/projects/microb2010/mainboard/rdline_config.h new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/mainboard/scheduler_config.h b/projects/microb2010/mainboard/scheduler_config.h new file mode 100755 index 0000000..daf5853 --- /dev/null +++ b/projects/microb2010/mainboard/scheduler_config.h @@ -0,0 +1,47 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: scheduler_config.h,v 1.2 2009-03-15 20:18:33 zer0 Exp $ + * + */ + +#ifndef _SCHEDULER_CONFIG_H_ +#define _SCHEDULER_CONFIG_H_ + +#define _SCHEDULER_CONFIG_VERSION_ 4 + +/** maximum number of allocated events */ +#define SCHEDULER_NB_MAX_EVENT 10 + + +#define SCHEDULER_UNIT_FLOAT 512.0 +#define SCHEDULER_UNIT 512L + +/** number of allowed imbricated scheduler interrupts. The maximum + * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate + * more than once per event. If it is less, it can avoid to browse the + * event table, events are delayed (we loose precision) but it takes + * less CPU */ +#define SCHEDULER_NB_STACKING_MAX SCHEDULER_NB_MAX_EVENT + +/** define it for debug infos (not recommended, because very slow on + * an AVR, it uses printf in an interrupt). It can be useful if + * prescaler is very high, making the timer interrupt period very + * long in comparison to printf() */ +/* #define SCHEDULER_DEBUG */ + +#endif // _SCHEDULER_CONFIG_H_ diff --git a/projects/microb2010/mainboard/sensor.c b/projects/microb2010/mainboard/sensor.c new file mode 100644 index 0000000..6ecff35 --- /dev/null +++ b/projects/microb2010/mainboard/sensor.c @@ -0,0 +1,293 @@ +/* + * Copyright Droids Corporation (2009) + * 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 + * (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: sensor.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "sensor.h" + +/************ ADC */ + +struct adc_infos { + uint16_t config; + int16_t value; + int16_t prev_val; + int16_t (*filter)(struct adc_infos *, int16_t); +}; + +/* reach 90% of the value in 4 samples */ +int16_t rii_light(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + (int32_t)adc->prev_val / 2; + return adc->prev_val / 2; +} + +/* reach 90% of the value in 8 samples */ +int16_t rii_medium(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4; + return adc->prev_val / 4; +} + +/* reach 90% of the value in 16 samples */ +int16_t rii_strong(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8; + return adc->prev_val / 8; +} + + +#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x ) + +/* 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, see init below */ +static void do_adc(void *dummy) +{ + /* launch first conversion */ + adc_launch(adc_infos[0].config); +} + +static void adc_event(int16_t result) +{ + static uint8_t i = 0; + + /* filter value if needed */ + if (adc_infos[i].filter) + adc_infos[i].value = adc_infos[i].filter(&adc_infos[i], + result); + else + adc_infos[i].value = result; + + i ++; + if (i >= ADC_MAX) + i = 0; + else + adc_launch(adc_infos[i].config); +} + +int16_t sensor_get_adc(uint8_t i) +{ + int16_t tmp; + uint8_t flags; + + IRQ_LOCK(flags); + tmp = adc_infos[i].value; + 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; + uint8_t invert; +}; + +/* pullup mapping: + * CAP 1,5,6,7,8 + */ +static struct sensor_filter sensor_filter[SENSOR_MAX] = { + [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ + [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */ + [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ + [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ + [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 0 */ + [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ + [S_DISP_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 6 */ + [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ + [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ + [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ + [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */ + [S_RESERVED4] = { 1, 0, 0, 1, 0, 0 }, /* 11 */ + [S_RESERVED5] = { 1, 0, 0, 1, 0, 0 }, /* 12 */ + [S_RESERVED6] = { 1, 0, 0, 1, 0, 0 }, /* 13 */ + [S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */ + [S_RESERVED8] = { 1, 0, 0, 1, 0, 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_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); +} + +/* virtual obstacle */ + +#define DISABLE_CPT_MAX 200 +static uint8_t disable = 0; /* used to disable obstacle detection + * during some time */ + +/* called every 10 ms */ +void +sensor_obstacle_update(void) +{ + if (disable > 0) { + disable --; + if (disable == 0) + DEBUG(E_USER_STRAT, "re-enable sensor"); + } +} + +void sensor_obstacle_disable(void) +{ + DEBUG(E_USER_STRAT, "disable sensor"); + disable = DISABLE_CPT_MAX; +} + +void sensor_obstacle_enable(void) +{ + disable = 0; +} + +uint8_t sensor_obstacle_is_disabled(void) +{ + return disable; +} + + +/************ global sensor init */ + +/* called every 10 ms, see init below */ +static void do_sensors(void *dummy) +{ + do_adc(NULL); + do_boolean_sensors(NULL); + sensor_obstacle_update(); +} + +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); +} + diff --git a/projects/microb2010/mainboard/sensor.h b/projects/microb2010/mainboard/sensor.h new file mode 100644 index 0000000..0df5567 --- /dev/null +++ b/projects/microb2010/mainboard/sensor.h @@ -0,0 +1,62 @@ +/* + * Copyright Droids Corporation (2009) + * 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 + * (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: sensor.h,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +/* 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_CAP1 0 +#define S_CAP2 1 +#define S_COLUMN_RIGHT 2 +#define S_COLUMN_LEFT 3 +#define S_START_SWITCH 4 +#define S_DISP_LEFT 5 +#define S_DISP_RIGHT 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); + +/* 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); + +void sensor_obstacle_disable(void); +void sensor_obstacle_enable(void); +uint8_t sensor_obstacle_is_disabled(void); diff --git a/projects/microb2010/mainboard/spi_config.h b/projects/microb2010/mainboard/spi_config.h new file mode 100644 index 0000000..76697c3 --- /dev/null +++ b/projects/microb2010/mainboard/spi_config.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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 + * + */ + +/* + * Author : Julien LE GUEN - jlg@jleguen.info + */ + + +/* + * Configure HERE your SPI module + */ + + + +/* Number of slave devices in your system + * Each slave have a dedicated SS line that you have to register + * before using the SPI module + */ +#define SPI_MAX_SLAVES 1 + diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c new file mode 100644 index 0000000..3f54716 --- /dev/null +++ b/projects/microb2010/mainboard/strat.c @@ -0,0 +1,807 @@ +/* + * Copyright Droids, Microb Technology (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 + * (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: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" +#include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "sensor.h" +#include "actuator.h" + +#define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */ +#define COL_SCAN_PRE_MARGIN 250 + + +#ifdef TEST_BEACON + +#define BEACON_MAX_SAMPLES 100 +struct beacon_sample { + int16_t posx; + int16_t posy; + int16_t posa; + int16_t oppx; + int16_t oppy; + uint8_t time; +}; + +static struct beacon_sample beacon_sample[BEACON_MAX_SAMPLES]; +static uint8_t beacon_prev_time = 0; +static uint8_t beacon_cur_idx = 0; + +static void beacon_update_samples(void) +{ + int16_t opp_a, opp_d, opp_x, opp_y; + int8_t err; + uint8_t time; + + time = time_get_s(); + + /* one sample per second max */ + if (time <= beacon_prev_time) + return; + /* limit max number of samples */ + if (beacon_cur_idx >= BEACON_MAX_SAMPLES) + return; + + memset(&beacon_sample[beacon_cur_idx], 0, sizeof(beacon_sample[beacon_cur_idx])); + beacon_prev_time = time; + beacon_sample[beacon_cur_idx].time = time; + + /* get opponent pos; if not found, just set struct to 0 */ + err = get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a); + if (err == -1) + return; + + beacon_sample[beacon_cur_idx].posx = position_get_x_s16(&mainboard.pos); + beacon_sample[beacon_cur_idx].posy = position_get_y_s16(&mainboard.pos); + beacon_sample[beacon_cur_idx].posa = position_get_a_deg_s16(&mainboard.pos); + beacon_sample[beacon_cur_idx].oppx = opp_x; + beacon_sample[beacon_cur_idx].oppy = opp_y; + beacon_cur_idx++; +} + +void beacon_dump_samples(void) +{ + uint16_t i; + + for (i=0; izone->name); + + if (temple->flags & TEMPLE_F_MONOCOL) + printf_P(PSTR("MONOCOL ")); + else + printf_P(PSTR("BICOL ")); + + if (temple->flags & TEMPLE_F_ON_DISC) + printf_P(PSTR("ON_DISC ")); + else + printf_P(PSTR("ON_ZONE_0_1 ")); + + if (temple->flags & TEMPLE_F_OPPONENT) + printf_P(PSTR("OPPONENT ")); + else + printf_P(PSTR("OURS ")); + + if (temple->flags & TEMPLE_F_LINTEL) + printf_P(PSTR("LIN_ON_TOP ")); + else + printf_P(PSTR("COL_ON_TOP ")); + + printf_P(PSTR("\r\n")); + + printf_P(PSTR(" pos=(%d,%d,%d) ckpt=(%d,%d) ltime=%d\r\n"), + temple->x, temple->y, temple->a, + temple->checkpoint_x, temple->checkpoint_y, + temple->last_try_time); + printf_P(PSTR(" L: lev=%d da=%d,%d\r\n"), + temple->level_l, temple->dist_l, temple->angle_l); + printf_P(PSTR(" R: lev=%d da=%d,%d\r\n"), + temple->level_l, temple->dist_l, temple->angle_l); +} + +void strat_dump_zone(struct build_zone *zone) +{ + if (!strat_infos.dump_enabled) + return; + + printf_P(PSTR(" zone %s: "), zone->name); + + if (zone->flags & ZONE_F_DISC) + printf_P(PSTR("DISC ")); + else if (zone->flags & ZONE_F_ZONE1) + printf_P(PSTR("ZONE1 ")); + else if (zone->flags & ZONE_F_ZONE0) + printf_P(PSTR("ZONE0 ")); + + if (zone->flags & ZONE_F_BUSY) + printf_P(PSTR("BUSY ")); + else + printf_P(PSTR("FREE ")); + + printf_P(PSTR("\r\n")); + + printf_P(PSTR(" lev=%d ckpt=(%d,%d) ltime=%d\r\n"), + zone->level, + zone->checkpoint_x, zone->checkpoint_y, + zone->last_try_time); +} + +void strat_dump_static_cols(void) +{ + if (!strat_infos.dump_enabled) + return; + + printf_P(PSTR(" static cols: l0=%d l1=%d l2=%d\r\n"), + strat_infos.s_cols.flags & STATIC_COL_LINE0_DONE, + strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE, + strat_infos.s_cols.flags & STATIC_COL_LINE2_DONE); +} + +void strat_dump_col_disp(void) +{ + if (!strat_infos.dump_enabled) + return; + + printf_P(PSTR(" c1 cnt=%d ltt=%d\r\n"), + strat_infos.c1.count, strat_infos.c1.last_try_time); + printf_P(PSTR(" c2 cnt=%d ltt=%d\r\n"), + strat_infos.c2.count, strat_infos.c2.last_try_time); + printf_P(PSTR(" c3 cnt=%d ltt=%d\r\n"), + strat_infos.c3.count, strat_infos.c3.last_try_time); +} + +void strat_dump_lin_disp(void) +{ + if (!strat_infos.dump_enabled) + return; + printf_P(PSTR(" l1 cnt=%d ltt=%d\r\n"), + strat_infos.l1.count, strat_infos.l1.last_try_time); + printf_P(PSTR(" l2 cnt=%d ltt=%d\r\n"), + strat_infos.l2.count, strat_infos.l2.last_try_time); + +} + +void strat_dump_all_temples(void) +{ + struct temple *temple; + uint8_t i; + + if (!strat_infos.dump_enabled) + return; + + for (i=0; iflags & TEMPLE_F_VALID)) + continue; + strat_dump_temple(temple); + } +} + +void strat_dump_all_zones(void) +{ + struct build_zone *zone; + uint8_t i; + + if (!strat_infos.dump_enabled) + return; + + for (i=0; iflags & ZONE_F_VALID)) + continue; + strat_dump_zone(zone); + } +} + +/* display current information about the state of the game */ +void strat_dump_infos(const char *caller) +{ + if (!strat_infos.dump_enabled) + return; + + printf_P(PSTR("%s() dump strat infos:\r\n"), caller); + strat_dump_static_cols(); + strat_dump_col_disp(); + strat_dump_lin_disp(); + strat_dump_all_temples(); + strat_dump_all_zones(); +} + +/* init current area state before a match. Dump update user conf + * here */ +void strat_reset_infos(void) +{ + uint8_t i; + + /* /!\ don't do a big memset() as there is static data */ + strat_infos.s_cols.flags = 0; + strat_infos.c1.count = 5; + strat_infos.c1.last_try_time = 0; + strat_infos.c2.count = 5; + strat_infos.c2.last_try_time = 0; + strat_infos.c3.count = 5; + strat_infos.c3.last_try_time = 0; + strat_infos.l1.count = 1; + strat_infos.l1.last_try_time = 0; + strat_infos.l2.count = 1; + strat_infos.l2.last_try_time = 0; + + strat_infos.taken_lintel = 0; + strat_infos.col_in_boobs = 0; + strat_infos.lazy_pickup_done = 0; + strat_infos.i2c_loaded_skipped = 0; + + memset(strat_infos.temple_list, 0, sizeof(strat_infos.temple_list)); + + for (i=0; i= 1) && + (get_column_count_right() >= 1)) + return 0; + } + else { + if (get_column_count()) + return 0; + } + return 1; +} + +/* dump state (every 5 s max) */ +#define DUMP_RATE_LIMIT(dump, last_print) \ + do { \ + if (time_get_s() - last_print > 5) { \ + dump(); \ + last_print = time_get_s(); \ + } \ + } while (0) + + +uint8_t strat_main(void) +{ + uint8_t err; + struct temple *temple = NULL; + struct build_zone *zone = NULL; + + uint8_t last_print_cols = 0; + uint8_t last_print_lin = 0; + uint8_t last_print_temple = 0; + uint8_t last_print_zone = 0; + + /* do static cols + first temple */ + err = strat_beginning(); + + /* skip error code */ + + while (1) { + + if (err == END_TIMER) { + DEBUG(E_USER_STRAT, "End of time"); + strat_exit(); + break; + } + + /* we have at least one col on each arm, build now */ + if (need_more_elements() == 0) { + + /* try to build on opponent, will return + * END_TRAJ without doing anything if + * disabled */ + err = strat_build_on_opponent_temple(); + if (!TRAJ_SUCCESS(err)) + continue; + if (need_more_elements()) + continue; + + /* try to scan and build on our temple, will + * return END_TRAJ without doing anything if + * disabled */ + err = strat_check_temple_and_build(); + if (!TRAJ_SUCCESS(err)) + continue; + if (need_more_elements()) + continue; + + /* Else, do a simple build, as before */ + + temple = strat_get_best_temple(); + + /* one valid temple found */ + if (temple) { + DUMP_RATE_LIMIT(strat_dump_all_temples, last_print_temple); + + err = strat_goto_temple(temple); + if (!TRAJ_SUCCESS(err)) + continue; + + /* can return END_ERROR or END_TIMER, + * should not happen here */ + err = strat_grow_temple(temple); + if (!TRAJ_SUCCESS(err)) + continue; + + err = strat_escape(temple->zone, TRAJ_FLAGS_STD); + if (!TRAJ_SUCCESS(err)) + continue; + + continue; + } + + zone = strat_get_best_zone(); + if (zone) { + DUMP_RATE_LIMIT(strat_dump_all_zones, last_print_zone); + + DEBUG(E_USER_STRAT, "goto zone %s", zone->name); + err = strat_goto_build_zone(zone, zone->level); + if (!TRAJ_SUCCESS(err)) + continue; + DEBUG(E_USER_STRAT, "zone reached"); + + /* no error code except END_ERROR, should not happen */ + err = strat_build_new_temple(zone); + + err = strat_escape(zone, TRAJ_FLAGS_STD); + if (!TRAJ_SUCCESS(err)) + continue; + + continue; + } + + /* XXX hey what can we do here... :'( */ + DEBUG(E_USER_STRAT, "panic :)"); + time_wait_ms(1000); + continue; + } + + /* else we need some elements (lintels, then columns) */ + else { + if (strat_infos.l1.count != 0 && strat_infos.l2.count != 0) + DUMP_RATE_LIMIT(strat_dump_lin_disp, last_print_lin); + + err = strat_pickup_lintels(); + /* can return an error code, but we have + * nothing to do because pickup_column() + * starts with a goto_and_avoid() */ + if (!TRAJ_SUCCESS(err)) + nop(); + + DUMP_RATE_LIMIT(strat_dump_col_disp, last_print_cols); + + err = strat_pickup_columns(); + if (!TRAJ_SUCCESS(err)) + nop(); /* nothing to do */ + + /* XXX check here that we have elements, or do + * something else */ + /* if we cannot take elements, try to build */ + } + } + return END_TRAJ; +} diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h new file mode 100644 index 0000000..2b2b96a --- /dev/null +++ b/projects/microb2010/mainboard/strat.h @@ -0,0 +1,302 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2008) + * + * 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: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#ifndef _STRAT_H_ +#define _STRAT_H_ + +/* convert coords according to our color */ +#define COLOR_Y(y) ((mainboard.our_color==I2C_COLOR_RED)? (y) : (AREA_Y-(y))) +#define COLOR_A(a) ((mainboard.our_color==I2C_COLOR_RED)? (a) : (-a)) +#define COLOR_SIGN(x) ((mainboard.our_color==I2C_COLOR_RED)? (x) : (-x)) +#define COLOR_INVERT(x) ((mainboard.our_color==I2C_COLOR_RED)? (x) : (!x)) + +/* area */ +#define AREA_X 3000 +#define AREA_Y 2100 + +#define START_X 200 +#define START_Y COLOR_Y(200) +#define START_A COLOR_A(45) + +#define CENTER_X 1500 +#define CENTER_Y 1050 + +#define CORNER_X 3000 +#define CORNER_Y COLOR_Y(2100) + +/* + * /- line 0 + * | /- line 1 + * | | /- line 2 + * | | | + * +---C1--------------------------C1---+ + * | z0a z1 z0b | + * | . | + * | . | + * C3 0 1 2 ^ C3 + * y | 3 4 5 < > | + * C2 6 7 8 v C2 + * |------ 9 10 11 . ------| + * | | . | | + * | red | . |green| + * +-----+--L1--L2-------L2--L1---+-----+ + * x + */ + +/* static columns */ +#define LINE0_X 600 +#define LINE1_X 850 +#define LINE2_X 1100 + +#define COL0_X 600 +#define COL0_Y COLOR_Y(1175) +#define COL1_X 850 +#define COL1_Y COLOR_Y(1175) +#define COL2_X 1100 +#define COL2_Y COLOR_Y(1175) + +#define COL3_X 600 +#define COL3_Y COLOR_Y(975) +#define COL4_X 850 +#define COL4_Y COLOR_Y(975) +#define COL5_X 1100 +#define COL5_Y COLOR_Y(975) + +#define COL6_X 600 +#define COL6_Y COLOR_Y(775) +#define COL7_X 850 +#define COL7_Y COLOR_Y(775) +#define COL8_X 1100 +#define COL8_Y COLOR_Y(775) + +#define COL9_X 600 +#define COL9_Y COLOR_Y(575) +#define COL10_X 850 +#define COL10_Y COLOR_Y(575) +#define COL11_X 1100 +#define COL11_Y COLOR_Y(575) + +/* distance to go backward before pickup in dispenser */ +#define DIST_BACK_DISPENSER 35 + +/* diag of the pentagon (pentacle ?) */ +#define DISC_PENTA_DIAG 530 + +#define COL_DISP_MAX_TRIES 5 +#define LIN_DISP_MAX_TRIES 3 + +/* useful traj flags */ +#define TRAJ_SUCCESS(f) (f & (END_TRAJ|END_NEAR)) +#define TRAJ_FLAGS_STD (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR|END_TIMER) +#define TRAJ_FLAGS_NO_TIMER (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR) +#define TRAJ_FLAGS_NO_NEAR (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR|END_TIMER) +#define TRAJ_FLAGS_NO_NEAR_NO_TIMER (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR) +#define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR) + +/* default speeds */ +#define SPEED_DIST_FAST 2500 +#define SPEED_ANGLE_FAST 2000 +#define SPEED_DIST_SLOW 1000 +#define SPEED_ANGLE_SLOW 1000 +#define SPEED_DIST_VERY_SLOW 400 +#define SPEED_ANGLE_VERY_SLOW 400 + +/* strat infos structures */ + +struct bbox { + int32_t x1; + int32_t y1; + int32_t x2; + int32_t y2; +}; + +struct conf { +#define STRAT_CONF_ONLY_ONE_ON_DISC 0x01 +#define STRAT_CONF_BYPASS_STATIC2 0x02 +#define STRAT_CONF_TAKE_ONE_LINTEL 0x04 +#define STRAT_CONF_SKIP_WHEN_CHECK_FAILS 0x08 +#define STRAT_CONF_STORE_STATIC2 0x10 +#define STRAT_CONF_BIG_3_TEMPLE 0x20 +#define STRAT_CONF_EARLY_SCAN 0x40 +#define STRAT_CONF_PUSH_OPP_COLS 0x80 + uint8_t flags; + uint8_t scan_opp_min_time; + uint8_t delay_between_opp_scan; + uint8_t scan_our_min_time; + uint8_t delay_between_our_scan; + uint8_t wait_opponent; + uint8_t lintel_min_time; + int16_t scan_opp_angle; +}; + +struct static_columns { +#define STATIC_COL_LINE0_DONE 0x01 +#define STATIC_COL_LINE1_DONE 0x02 +#define STATIC_COL_LINE2_DONE 0x04 + uint8_t flags; + uint8_t configuration; +}; + +struct column_dispenser { + int8_t count; + uint8_t last_try_time; + uint8_t scan_left; + int16_t checkpoint_x; + int16_t checkpoint_y; + int16_t scan_a; + int16_t eject_a; + int16_t pickup_a; + int16_t recalib_x; + int16_t recalib_y; + char *name; +}; + +struct lintel_dispenser { + int8_t count; + uint8_t last_try_time; + int16_t x; + char *name; +}; + +struct temple { +#define TEMPLE_F_VALID 0x01 /* structure is valid */ +#define TEMPLE_F_MONOCOL 0x02 /* temple has only one col */ +#define TEMPLE_F_ON_DISC 0x04 /* temple is on disc (else it's on other zone) */ +#define TEMPLE_F_OPPONENT 0x08 /* temple was originally built by opponent */ +#define TEMPLE_F_LINTEL 0x10 /* lintel on top (don't put another lintel) */ + + uint8_t flags; + /* position of the robot when we built it */ + int16_t x; + int16_t y; + int16_t a; + + /* position of the robot checkpoint */ + int16_t checkpoint_x; + int16_t checkpoint_y; + + /* position and level of each col */ + uint8_t level_l; + uint8_t dist_l; + uint8_t angle_l; + + uint8_t level_r; + uint8_t dist_r; + uint8_t angle_r; + +#define TEMPLE_DISABLE_TIME 5 + uint8_t last_try_time; + + struct build_zone *zone; +}; + +struct build_zone { +#define ZONE_F_VALID 0x01 /* zone is valid */ +#define ZONE_F_DISC 0x02 /* specific disc zone */ +#define ZONE_F_ZONE1 0x04 /* specific zone 1 */ +#define ZONE_F_ZONE0 0x08 /* specific zone 0 */ +#define ZONE_F_BUSY 0x10 /* this zone is busy */ + uint8_t flags; + uint8_t level; + int16_t checkpoint_x; + int16_t checkpoint_y; + +#define ZONE_DISABLE_TIME 5 + uint8_t last_try_time; + char *name; +}; + +#define MAX_TEMPLE 5 +#define MAX_ZONE 5 + +/* all infos related to strat */ +struct strat_infos { + uint8_t dump_enabled; + struct conf conf; + struct bbox area_bbox; + uint8_t taken_lintel; + uint8_t col_in_boobs; + uint8_t lazy_pickup_done; + uint8_t i2c_loaded_skipped; + struct static_columns s_cols; + struct column_dispenser c1; + struct column_dispenser c2; + struct column_dispenser c3; + struct lintel_dispenser l1; + struct lintel_dispenser l2; + struct build_zone zone_list[MAX_ZONE]; + struct temple temple_list[MAX_TEMPLE]; +}; +extern struct strat_infos strat_infos; + +/* in strat.c */ +void strat_dump_infos(const char *caller); /* show current known state + of area */ +void strat_dump_temple(struct temple *temple); +void strat_dump_conf(void); +void strat_reset_infos(void); /* reset current known state */ +void strat_preinit(void); +void strat_init(void); +void strat_exit(void); +void strat_dump_flags(void); +void strat_goto_near(int16_t x, int16_t y, uint16_t dist); +uint8_t strat_main(void); +void strat_event(void *dummy); + +/* in strat_static_columns.c */ +uint8_t strat_static_columns(uint8_t configuration); +uint8_t strat_static_columns_pass2(void); + +/* in strat_lintel.c */ +uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp); +uint8_t strat_pickup_lintels(void); + +/* in strat_column_disp.c */ +uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a); +uint8_t strat_pickup_columns(void); +uint8_t strat_goto_col_disp(struct column_dispenser **disp); + +/* in strat_building.c */ +uint8_t strat_goto_disc(int8_t level); +uint8_t strat_goto_build_zone(struct build_zone *build_zone, uint8_t level); +uint8_t strat_build_new_temple(struct build_zone *build_zone); +uint8_t strat_goto_temple(struct temple *temple); +uint8_t strat_grow_temple(struct temple *temple); +uint8_t strat_grow_temple_column(struct temple *temple); +struct temple *strat_get_best_temple(void); +struct temple *strat_get_our_temple_on_disc(uint8_t valid); +struct build_zone *strat_get_best_zone(void); +struct temple *strat_get_free_temple(void); + +/* in strat_scan.c */ +struct scan_disc_result; +void scanner_dump_state(void); +int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x, + int16_t *ckpt_rel_y, int16_t *back_mm); +uint8_t strat_scan_disc(int16_t angle, uint8_t mode, + struct scan_disc_result *result); +uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level); +int16_t strat_get_temple_angle(struct temple *temple); +int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle); +uint8_t strat_build_on_opponent_temple(void); +uint8_t strat_check_temple_and_build(void); + +#endif diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c new file mode 100644 index 0000000..6e75dd0 --- /dev/null +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -0,0 +1,535 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_avoid.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "sensor.h" + +#define EDGE_NUMBER 5 +void set_rotated_pentagon(poly_t *pol, const point_t *robot_pt, + int16_t radius, int16_t x, int16_t y) +{ + + double c_a, s_a; + uint8_t i; + double px1, py1, px2, py2; + double a_rad; + + a_rad = atan2(y - robot_pt->y, x - robot_pt->x); + + /* generate pentagon */ + c_a = cos(-2*M_PI/EDGE_NUMBER); + s_a = sin(-2*M_PI/EDGE_NUMBER); + + /* + px1 = radius; + py1 = 0; + */ + px1 = radius * cos(a_rad + 2*M_PI/(2*EDGE_NUMBER)); + py1 = radius * sin(a_rad + 2*M_PI/(2*EDGE_NUMBER)); + + + for (i = 0; i < EDGE_NUMBER; i++){ + oa_poly_set_point(pol, x + px1, y + py1, i); + + px2 = px1*c_a + py1*s_a; + py2 = -px1*s_a + py1*c_a; + + px1 = px2; + py1 = py2; + } +} + +void set_rotated_poly(poly_t *pol, const point_t *robot_pt, + int16_t w, int16_t l, int16_t x, int16_t y) +{ + double tmp_x, tmp_y; + double a_rad; + + a_rad = atan2(y - robot_pt->y, x - robot_pt->x); + + DEBUG(E_USER_STRAT, "%s() x,y=%d,%d a_rad=%2.2f", + __FUNCTION__, x, y, a_rad); + + /* point 1 */ + tmp_x = w; + tmp_y = l; + rotate(&tmp_x, &tmp_y, a_rad); + tmp_x += x; + tmp_y += y; + oa_poly_set_point(pol, tmp_x, tmp_y, 0); + + /* point 2 */ + tmp_x = -w; + tmp_y = l; + rotate(&tmp_x, &tmp_y, a_rad); + tmp_x += x; + tmp_y += y; + oa_poly_set_point(pol, tmp_x, tmp_y, 1); + + /* point 3 */ + tmp_x = -w; + tmp_y = -l; + rotate(&tmp_x, &tmp_y, a_rad); + tmp_x += x; + tmp_y += y; + oa_poly_set_point(pol, tmp_x, tmp_y, 2); + + /* point 4 */ + tmp_x = w; + tmp_y = -l; + rotate(&tmp_x, &tmp_y, a_rad); + tmp_x += x; + tmp_y += y; + oa_poly_set_point(pol, tmp_x, tmp_y, 3); +} + +#define DISC_X CENTER_X +#define DISC_Y CENTER_Y + +void set_central_disc_poly(poly_t *pol, const point_t *robot_pt) +{ + set_rotated_pentagon(pol, robot_pt, DISC_PENTA_DIAG, + DISC_X, DISC_Y); +} + +#ifdef HOMOLOGATION +/* /!\ half size */ +#define O_WIDTH 400 +#define O_LENGTH 550 +#else +/* /!\ half size */ +#define O_WIDTH 360 +#define O_LENGTH 500 +#endif + +void set_opponent_poly(poly_t *pol, const point_t *robot_pt, int16_t w, int16_t l) +{ + int16_t x, y; + get_opponent_xy(&x, &y); + DEBUG(E_USER_STRAT, "oponent at: %d %d", x, y); + + /* place poly even if invalid, because it's -100 */ + set_rotated_poly(pol, robot_pt, w, l, x, y); +} + +/* don't care about polygons further than this distance for escape */ +#define ESCAPE_POLY_THRES 1000 + +/* don't reduce opp if opp is too far */ +#define REDUCE_POLY_THRES 600 + +/* has to be longer than any poly */ +#define ESCAPE_VECT_LEN 3000 + +/* + * Go in playground, loop until out of poly. The argument robot_pt is + * the pointer to the current position of the robot. + * Return 0 if there was nothing to do. + * Return 1 if we had to move. In this case, update the theorical + * position of the robot in robot_pt. + */ +static int8_t go_in_area(point_t *robot_pt) +{ + point_t poly_pts_area[4]; + poly_t poly_area; + point_t disc_pt, dst_pt; + + disc_pt.x = DISC_X; + disc_pt.y = DISC_Y; + + /* Go in playground */ + if (!is_in_boundingbox(robot_pt)){ + NOTICE(E_USER_STRAT, "not in playground %"PRIi32", %"PRIi32"", + robot_pt->x, robot_pt->y); + + poly_area.l = 4; + poly_area.pts = poly_pts_area; + poly_pts_area[0].x = strat_infos.area_bbox.x1; + poly_pts_area[0].y = strat_infos.area_bbox.y1; + + poly_pts_area[1].x = strat_infos.area_bbox.x2; + poly_pts_area[1].y = strat_infos.area_bbox.y1; + + poly_pts_area[2].x = strat_infos.area_bbox.x2; + poly_pts_area[2].y = strat_infos.area_bbox.y2; + + poly_pts_area[3].x = strat_infos.area_bbox.x1; + poly_pts_area[3].y = strat_infos.area_bbox.y2; + + is_crossing_poly(*robot_pt, disc_pt, &dst_pt, &poly_area); + NOTICE(E_USER_STRAT, "pt dst %"PRIi32", %"PRIi32"", dst_pt.x, dst_pt.y); + + strat_goto_xy_force(dst_pt.x, dst_pt.y); + + robot_pt->x = dst_pt.x; + robot_pt->y = dst_pt.y; + + NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", + dst_pt.x, dst_pt.y); + + return 1; + } + + return 0; +} + + +/* + * Escape from polygons if needed. + * robot_pt is the current position of the robot, it will be + * updated. + */ +static int8_t escape_from_poly(point_t *robot_pt, + poly_t *pol_disc, + int16_t opp_x, int16_t opp_y, + int16_t opp_w, int16_t opp_l, + poly_t *pol_opp) +{ + uint8_t in_disc = 0, in_opp = 0; + double escape_dx = 0, escape_dy = 0; + double disc_dx = 0, disc_dy = 0; + double opp_dx = 0, opp_dy = 0; + double len; + point_t opp_pt, disc_pt, dst_pt; + point_t intersect_disc_pt, intersect_opp_pt; + + opp_pt.x = opp_x; + opp_pt.y = opp_y; + disc_pt.x = DISC_X; + disc_pt.y = DISC_Y; + + /* escape from other poly if necessary */ + if (is_in_poly(robot_pt, pol_disc) == 1) + in_disc = 1; + if (is_in_poly(robot_pt, pol_opp) == 1) + in_opp = 1; + + if (in_disc == 0 && in_opp == 0) { + NOTICE(E_USER_STRAT, "no need to escape"); + return 0; + } + + NOTICE(E_USER_STRAT, "in_disc=%d, in_opp=%d", in_disc, in_opp); + + /* process escape vector */ + + if (distance_between(robot_pt->x, robot_pt->y, DISC_X, DISC_Y) < ESCAPE_POLY_THRES) { + disc_dx = robot_pt->x - DISC_X; + disc_dy = robot_pt->y - DISC_Y; + NOTICE(E_USER_STRAT, " robot is near disc: vect=%2.2f,%2.2f", + disc_dx, disc_dy); + len = norm(disc_dx, disc_dy); + if (len != 0) { + disc_dx /= len; + disc_dy /= len; + } + else { + disc_dx = 1.0; + disc_dy = 0.0; + } + escape_dx += disc_dx; + escape_dy += disc_dy; + } + + if (distance_between(robot_pt->x, robot_pt->y, opp_x, opp_y) < ESCAPE_POLY_THRES) { + opp_dx = robot_pt->x - opp_x; + opp_dy = robot_pt->y - opp_y; + NOTICE(E_USER_STRAT, " robot is near opp: vect=%2.2f,%2.2f", + opp_dx, opp_dy); + len = norm(opp_dx, opp_dy); + if (len != 0) { + opp_dx /= len; + opp_dy /= len; + } + else { + opp_dx = 1.0; + opp_dy = 0.0; + } + escape_dx += opp_dx; + escape_dy += opp_dy; + } + + /* normalize escape vector */ + len = norm(escape_dx, escape_dy); + if (len != 0) { + escape_dx /= len; + escape_dy /= len; + } + else { + if (pol_disc != NULL) { + /* rotate 90° */ + escape_dx = disc_dy; + escape_dy = disc_dx; + } + else if (pol_opp != NULL) { + /* rotate 90° */ + escape_dx = opp_dy; + escape_dy = opp_dx; + } + else { /* should not happen */ + opp_dx = 1.0; + opp_dy = 0.0; + } + } + + NOTICE(E_USER_STRAT, " escape vect = %2.2f,%2.2f", + escape_dx, escape_dy); + + /* process the correct len of escape vector */ + + dst_pt.x = robot_pt->x + escape_dx * ESCAPE_VECT_LEN; + dst_pt.y = robot_pt->y + escape_dy * ESCAPE_VECT_LEN; + + NOTICE(E_USER_STRAT, "robot pt %"PRIi32" %"PRIi32, + robot_pt->x, robot_pt->y); + NOTICE(E_USER_STRAT, "dst point %"PRIi32",%"PRIi32, + dst_pt.x, dst_pt.y); + + if (in_disc) { + if (is_crossing_poly(*robot_pt, dst_pt, &intersect_disc_pt, + pol_disc) == 1) { + /* we add 2 mm to be sure we are out of th polygon */ + dst_pt.x = intersect_disc_pt.x + escape_dx * 2; + dst_pt.y = intersect_disc_pt.y + escape_dy * 2; + if (is_point_in_poly(pol_opp, dst_pt.x, dst_pt.y) != 1) { + + if (!is_in_boundingbox(&dst_pt)) + return -1; + + NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", + dst_pt.x, dst_pt.y); + + strat_goto_xy_force(dst_pt.x, dst_pt.y); + + robot_pt->x = dst_pt.x; + robot_pt->y = dst_pt.y; + + return 0; + } + } + } + + if (in_opp) { + if (is_crossing_poly(*robot_pt, dst_pt, &intersect_opp_pt, + pol_opp) == 1) { + /* we add 2 cm to be sure we are out of th polygon */ + dst_pt.x = intersect_opp_pt.x + escape_dx * 2; + dst_pt.y = intersect_opp_pt.y + escape_dy * 2; + + if (is_point_in_poly(pol_disc, dst_pt.x, dst_pt.y) != 1) { + + if (!is_in_boundingbox(&dst_pt)) + return -1; + + NOTICE(E_USER_STRAT, "GOTO %"PRIi32",%"PRIi32"", + dst_pt.x, dst_pt.y); + + strat_goto_xy_force(dst_pt.x, dst_pt.y); + + robot_pt->x = dst_pt.x; + robot_pt->y = dst_pt.y; + + return 0; + } + } + } + + /* should not happen */ + return -1; +} + + +static int8_t __goto_and_avoid(int16_t x, int16_t y, + uint8_t flags_intermediate, + uint8_t flags_final, + uint8_t forward) +{ + int8_t len = -1, i; + point_t *p; + poly_t *pol_disc, *pol_opp; + int8_t ret; + int16_t opp_w, opp_l, opp_x, opp_y; + point_t p_dst, robot_pt; + + DEBUG(E_USER_STRAT, "%s(%d,%d) flags_i=%x flags_f=%x forw=%d", + __FUNCTION__, x, y, flags_intermediate, flags_final, forward); + + retry: + get_opponent_xy(&opp_x, &opp_y); + opp_w = O_WIDTH; + opp_l = O_LENGTH; + + robot_pt.x = position_get_x_s16(&mainboard.pos); + robot_pt.y = position_get_y_s16(&mainboard.pos); + + oa_init(); + pol_disc = oa_new_poly(5); + set_central_disc_poly(pol_disc, &robot_pt); + pol_opp = oa_new_poly(4); + set_opponent_poly(pol_opp, &robot_pt, O_WIDTH, O_LENGTH); + + /* If we are not in the limited area, try to go in it. */ + ret = go_in_area(&robot_pt); + + /* check that destination is valid */ + p_dst.x = x; + p_dst.y = y; + if (!is_in_boundingbox(&p_dst)) { + NOTICE(E_USER_STRAT, " dst is not in playground"); + return END_ERROR; + } + if (is_point_in_poly(pol_disc, x, y)) { + NOTICE(E_USER_STRAT, " dst is in disc"); + return END_ERROR; + } + if (is_point_in_poly(pol_opp, x, y)) { + NOTICE(E_USER_STRAT, " dst is in opp"); + return END_ERROR; + } + + /* now start to avoid */ + while (opp_w && opp_l) { + + /* robot_pt is not updated if it fails */ + ret = escape_from_poly(&robot_pt, + pol_disc, opp_x, opp_y, + opp_w, opp_l, pol_opp); + if (ret == 0) { + oa_reset(); + oa_start_end_points(robot_pt.x, robot_pt.y, x, y); + /* oa_dump(); */ + + len = oa_process(); + if (len >= 0) + break; + } + if (distance_between(robot_pt.x, robot_pt.y, opp_x, opp_y) < REDUCE_POLY_THRES ) { + if (opp_w == 0) + opp_l /= 2; + opp_w /= 2; + } + else { + NOTICE(E_USER_STRAT, "oa_process() returned %d", len); + return END_ERROR; + } + + NOTICE(E_USER_STRAT, "reducing opponent %d %d", opp_w, opp_l); + set_opponent_poly(pol_opp, &robot_pt, opp_w, opp_l); + } + + p = oa_get_path(); + for (i=0 ; ix, p->y); + + if (forward) + trajectory_goto_forward_xy_abs(&mainboard.traj, p->x, p->y); + else + trajectory_goto_backward_xy_abs(&mainboard.traj, p->x, p->y); + + /* no END_NEAR for the last point */ + if (i == len - 1) + ret = wait_traj_end(flags_final); + else + ret = wait_traj_end(flags_intermediate); + + if (ret == END_BLOCKING) { + DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)", + __FUNCTION__, x, y); + goto retry; + } + else if (ret == END_OBSTACLE) { + /* brake and wait the speed to be slow */ + DEBUG(E_USER_STRAT, "Retry avoidance %s(%d,%d)", + __FUNCTION__, x, y); + goto retry; + } + /* else if it is not END_TRAJ or END_NEAR, return */ + else if (!TRAJ_SUCCESS(ret)) { + return ret; + } + p++; + } + + return END_TRAJ; +} + +/* go forward to a x,y point. use current speed for that */ +uint8_t goto_and_avoid_forward(int16_t x, int16_t y, uint8_t flags_intermediate, + uint8_t flags_final) +{ + return __goto_and_avoid(x, y, flags_intermediate, flags_final, 1); +} + +/* go backward to a x,y point. use current speed for that */ +uint8_t goto_and_avoid_backward(int16_t x, int16_t y, uint8_t flags_intermediate, + uint8_t flags_final) +{ + return __goto_and_avoid(x, y, flags_intermediate, flags_final, 0); +} + +/* go to a x,y point. prefer backward but go forward if the point is + * near and in front of us */ +uint8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate, + uint8_t flags_final) +{ + double d,a; + abs_xy_to_rel_da(x, y, &d, &a); + + if (d < 300 && a < RAD(90) && a > RAD(-90)) + return __goto_and_avoid(x, y, flags_intermediate, + flags_final, 1); + else + return __goto_and_avoid(x, y, flags_intermediate, + flags_final, 0); +} diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h new file mode 100644 index 0000000..0978fd7 --- /dev/null +++ b/projects/microb2010/mainboard/strat_avoid.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_avoid.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +void set_opponent_poly(poly_t *pol, int16_t w, int16_t l); +int8_t goto_and_avoid(int16_t x, int16_t y, uint8_t flags_intermediate, + uint8_t flags_final); +int8_t goto_and_avoid_backward(int16_t x, int16_t y, + uint8_t flags_intermediate, + uint8_t flags_final); +int8_t goto_and_avoid_forward(int16_t x, int16_t y, + uint8_t flags_intermediate, + uint8_t flags_final); diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c new file mode 100644 index 0000000..21d526a --- /dev/null +++ b/projects/microb2010/mainboard/strat_base.c @@ -0,0 +1,512 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_base.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" + +#include "main.h" +#include "cmdline.h" +#include "strat_utils.h" +#include "strat_base.h" +#include "strat.h" +#include "sensor.h" +#include "i2c_protocol.h" + +/* true if we want to interrupt a trajectory */ +static uint8_t traj_intr=0; + +/* filled when a END_OBSTACLE is returned */ +struct opponent_obstacle opponent_obstacle; + +/* asked speed */ +static volatile int16_t strat_speed_a = SPEED_DIST_FAST; +static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST; +static volatile uint16_t strat_limit_speed_a = 0; /* no limit */ +static volatile uint16_t strat_limit_speed_d = 0; + +static volatile uint8_t strat_limit_speed_enabled = 1; + + +/* Strings that match the end traj cause */ +/* /!\ keep it sync with stat_base.h */ +const char *err_tab []= { + "END_TRAJ", + "END_BLOCKING", + "END_NEAR", + "END_OBSTACLE", + "END_ERROR", + "END_INTR", + "END_TIMER", + "END_RESERVED", +}; + +/* return string from end traj type num */ +const char *get_err(uint8_t err) +{ + uint8_t i; + if (err == 0) + return "SUCCESS"; + for (i=0 ; i<8; i++) { + if (err & (1 < 200) || + (ABS(mainboard.speed_a) > 200)) + + trajectory_hardstop(&mainboard.traj); + pid_reset(&mainboard.angle.pid); + pid_reset(&mainboard.distance.pid); + bd_reset(&mainboard.angle.bd); + bd_reset(&mainboard.distance.bd); +} + +/* go to an x,y point without checking for obstacle or blocking. It + * should be used for very small dist only. Return END_TRAJ if we + * reach destination, or END_BLOCKING if the robot blocked more than 3 + * times. */ +uint8_t strat_goto_xy_force(int16_t x, int16_t y) +{ + uint8_t i, err; + +#ifdef HOMOLOGATION + int8_t serr; + uint8_t hardstop = 0; + microseconds us = time_get_us2(); + int16_t opp_a, opp_d, opp_x, opp_y; + + while (1) { + serr = get_opponent_xyda(&opp_x, &opp_y, + &opp_d, &opp_a); + if (serr == -1) + break; + if (opp_d < 600) + break; + if (hardstop == 0) { + strat_hardstop(); + hardstop = 1; + } + if ((time_get_us2() - us) > 3000000L) + break; + } +#endif + for (i=0; i<3; i++) { + trajectory_goto_xy_abs(&mainboard.traj, x, y); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (TRAJ_SUCCESS(err)) + return END_TRAJ; + if (err == END_BLOCKING) { + time_wait_ms(500); + strat_hardstop(); + } + } + return END_BLOCKING; +} + +/* reset position */ +void strat_reset_pos(int16_t x, int16_t y, int16_t a) +{ + int16_t posx = position_get_x_s16(&mainboard.pos); + int16_t posy = position_get_y_s16(&mainboard.pos); + int16_t posa = position_get_a_deg_s16(&mainboard.pos); + + if (x == DO_NOT_SET_POS) + x = posx; + if (y == DO_NOT_SET_POS) + y = posy; + if (a == DO_NOT_SET_POS) + a = posa; + + DEBUG(E_USER_STRAT, "reset pos (%s%s%s)", + x == DO_NOT_SET_POS ? "" : "x", + y == DO_NOT_SET_POS ? "" : "y", + a == DO_NOT_SET_POS ? "" : "a"); + position_set(&mainboard.pos, x, y, a); + DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__); +} + +/* + * decrease gain on angle PID, and go forward until we reach the + * border. + */ +uint8_t strat_calib(int16_t dist, uint8_t flags) +{ + int32_t p = pid_get_gain_P(&mainboard.angle.pid); + int32_t i = pid_get_gain_I(&mainboard.angle.pid); + int32_t d = pid_get_gain_D(&mainboard.angle.pid); + uint8_t err; + + pid_set_gains(&mainboard.angle.pid, 150, 0, 2000); + trajectory_d_rel(&mainboard.traj, dist); + err = wait_traj_end(flags); + pid_set_gains(&mainboard.angle.pid, p, i, d); + return err; +} + +/* escape from zone, and don't brake, so we can continue with another + * traj */ +uint8_t strat_escape(struct build_zone *zone, uint8_t flags) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + + strat_get_speed(&old_spdd, &old_spda); + + err = WAIT_COND_OR_TIMEOUT(!opponent_is_behind(), 1000); + if (err == 0) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -150); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + strat_set_speed(old_spdd, old_spda); + return err; + } + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + if (zone->flags & ZONE_F_DISC) { + trajectory_d_rel(&mainboard.traj, -350); + err = WAIT_COND_OR_TRAJ_END(!robot_is_near_disc(), flags); + } + else { + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(flags); + } + + strat_set_speed(old_spdd, old_spda); + if (err == 0) + return END_NEAR; + return err; +} + +static void strat_update_traj_speed(void) +{ + uint16_t d, a; + + d = strat_speed_d; + if (strat_limit_speed_d && d > strat_limit_speed_d) + d = strat_limit_speed_d; + a = strat_speed_a; + if (strat_limit_speed_a && a > strat_limit_speed_a) + a = strat_limit_speed_a; + + trajectory_set_speed(&mainboard.traj, d, a); +} + +void strat_set_speed(uint16_t d, uint16_t a) +{ + uint8_t flags; + IRQ_LOCK(flags); + strat_speed_d = d; + strat_speed_a = a; + strat_update_traj_speed(); + IRQ_UNLOCK(flags); +} + +void strat_get_speed(uint16_t *d, uint16_t *a) +{ + uint8_t flags; + IRQ_LOCK(flags); + *d = strat_speed_d; + *a = strat_speed_a; + IRQ_UNLOCK(flags); +} + +void strat_limit_speed_enable(void) +{ + strat_limit_speed_enabled = 1; +} + +void strat_limit_speed_disable(void) +{ + strat_limit_speed_enabled = 0; +} + +/* called periodically */ +void strat_limit_speed(void) +{ + uint16_t lim_d = 0, lim_a = 0; + int16_t opp_d, opp_a; + + if (strat_limit_speed_enabled == 0) + goto update; + + if (get_opponent_da(&opp_d, &opp_a) != 0) + goto update; + +#ifdef HOMOLOGATION + if (opp_d < 600) { + lim_d = 150; + lim_a = 200; + } +#else + if (opp_d < 500) { + if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) { + lim_d = SPEED_DIST_VERY_SLOW; + lim_a = SPEED_ANGLE_VERY_SLOW; + } + else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) { + lim_d = SPEED_DIST_VERY_SLOW; + lim_a = SPEED_ANGLE_VERY_SLOW; + } + else { + lim_d = SPEED_DIST_SLOW; + lim_a = SPEED_ANGLE_VERY_SLOW; + } + } +#endif + else if (opp_d < 800) { + if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) { + lim_d = SPEED_DIST_SLOW; + lim_a = SPEED_ANGLE_SLOW; + } + else if (mainboard.speed_d < 0 && (opp_a < 250 && opp_a > 110)) { + lim_d = SPEED_DIST_SLOW; + lim_a = SPEED_ANGLE_SLOW; + } + } + + update: + if (lim_d != strat_limit_speed_d || + lim_a != strat_limit_speed_a) { + strat_limit_speed_d = lim_d; + strat_limit_speed_a = lim_a; + DEBUG(E_USER_STRAT, "new speed limit da=%d,%d", lim_d, lim_a); + strat_update_traj_speed(); + } +} + +/* start the strat */ +void strat_start(void) +{ + int8_t i; + uint8_t err; + + strat_preinit(); + + /* if start sw not plugged */ + 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_get(S_START_SWITCH)) { + if (! cmdline_keypressed()) + continue; + + for (i=3; i>0; i--) { + printf_P(PSTR("%d\r\n"), i); + time_wait_ms(1000); + } + break; + } + } + + /* if start sw plugged */ + if (!sensor_get(S_START_SWITCH)) { + printf_P(PSTR("Ready, unplug start switch to start\r\n")); + /* while start sw plugged */ + while (!sensor_get(S_START_SWITCH)); + } + + strat_init(); + err = strat_main(); + NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err)); + strat_exit(); +} + +/* return true if we have to brake due to an obstacle */ +uint8_t strat_obstacle(void) +{ + int16_t x_rel, y_rel; + int16_t opp_x, opp_y, opp_d, opp_a; + + /* too slow */ + if (ABS(mainboard.speed_d) < 150) + return 0; + + /* no opponent detected */ + if (get_opponent_xyda(&opp_x, &opp_y, + &opp_d, &opp_a)) + return 0; + + /* save obstacle position */ + opponent_obstacle.x = opp_x; + opponent_obstacle.y = opp_y; + opponent_obstacle.d = opp_d; + opponent_obstacle.a = opp_a; + + /* sensor are temporarily disabled */ + if (sensor_obstacle_is_disabled()) + return 0; + + /* relative position */ + x_rel = cos(RAD(opp_a)) * (double)opp_d; + y_rel = sin(RAD(opp_a)) * (double)opp_d; + + /* opponent too far */ + if (opp_d > 600) + return 0; + + /* opponent is in front of us */ + if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) { + DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d " + "xrel=%d yrel=%d (speed_d=%d)", + opp_d, opp_a, x_rel, y_rel, mainboard.speed_d); + sensor_obstacle_disable(); + return 1; + } + /* opponent is behind us */ + if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) { + DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", + opp_d, opp_a, x_rel, y_rel); + sensor_obstacle_disable(); + return 1; + } + + return 0; +} + +void interrupt_traj(void) +{ + traj_intr = 1; +} + +void interrupt_traj_reset(void) +{ + traj_intr = 0; +} + +uint8_t test_traj_end(uint8_t why) +{ + uint16_t cur_timer; + point_t robot_pt; + + robot_pt.x = position_get_x_s16(&mainboard.pos); + robot_pt.y = position_get_y_s16(&mainboard.pos); + + /* trigger an event at 3 sec before the end of the match if we + * have balls in the barrel */ + cur_timer = time_get_s(); + + if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) { + /* end of match */ + if (cur_timer >= MATCH_TIME) + return END_TIMER; + } + + if ((why & END_INTR) && traj_intr) { + interrupt_traj_reset(); + return END_INTR; + } + + if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj)) + return END_TRAJ; + + /* we are near the destination point (depends on current + * speed) AND the robot is in the area bounding box. */ + if (why & END_NEAR) { + int16_t d_near = 100; + + if (mainboard.speed_d > 2000) + d_near = 150; + + if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && + is_in_boundingbox(&robot_pt)) + return END_NEAR; + } + + if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) { + strat_hardstop(); + return END_BLOCKING; + } + + if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) { + strat_hardstop(); + return END_BLOCKING; + } + + if ((why & END_OBSTACLE) && strat_obstacle()) { + strat_hardstop(); + return END_OBSTACLE; + } + + return 0; +} + +uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line) +{ + uint8_t ret = 0; + int16_t opp_x, opp_y, opp_d, opp_a; + + while (ret == 0) + ret = test_traj_end(why); + + if (ret == END_OBSTACLE) { + if (get_opponent_xyda(&opp_x, &opp_y, + &opp_d, &opp_a) == 0) + DEBUG(E_USER_STRAT, "Got %s at line %d" + " xy=(%d,%d) da=(%d,%d)", get_err(ret), + line, opp_x, opp_y, opp_d, opp_a); + } + else { + DEBUG(E_USER_STRAT, "Got %s at line %d", + get_err(ret), line); + } + return ret; +} diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h new file mode 100644 index 0000000..27e1497 --- /dev/null +++ b/projects/microb2010/mainboard/strat_base.h @@ -0,0 +1,90 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_base.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +/* return values for strats and sub trajs */ +#define END_TRAJ 1 /* traj successful */ +#define END_BLOCKING 2 /* blocking during traj */ +#define END_NEAR 4 /* we are near destination */ +#define END_OBSTACLE 8 /* There is an obstacle in front of us */ +#define END_ERROR 16 /* Cannot do the command */ +#define END_INTR 32 /* interrupted by user */ +#define END_TIMER 64 /* we don't a lot of time */ +#define END_RESERVED 128 /* reserved */ + +/* only valid after a END_OBSTACLE */ +struct opponent_obstacle { + int16_t x; + int16_t y; + int16_t a; + int16_t d; +}; +extern struct opponent_obstacle opponent_obstacle; + +/* stop as fast as possible, without ramp */ +void strat_hardstop(void); + +#define DO_NOT_SET_POS -1000 +/* Reset position. If arg == DO_NOT_SET_POS, don't update value for + * it. */ +void strat_reset_pos(int16_t x, int16_t y, int16_t a); + +/* decrease gain on angle PID, and go forward until we reach the + * border. */ +uint8_t strat_calib(int16_t d, uint8_t flags); + +/* launch start procedure */ +void strat_start(void); + +/* go to an x,y point without checking for obstacle or blocking. It + * should be used for very small dist only. Return END_TRAJ if we + * reach destination, or END_BLOCKING if the robot blocked more than 3 + * times. */ +uint8_t strat_goto_xy_force(int16_t x, int16_t y); + +/* escape from disc polygon or another zone */ +struct build_zone; +uint8_t strat_escape(struct build_zone *zone, uint8_t flags); + +/* return true if we have to brake due to an obstacle */ +uint8_t strat_obstacle(void); + +/* set/get user strat speed */ +void strat_set_speed(uint16_t d, uint16_t a); +void strat_get_speed(uint16_t *d, uint16_t *a); + +/* when user type ctrl-c we can interrupt traj */ +void interrupt_traj(void); +void interrupt_traj_reset(void); + +/* limit speed when we are close of opponent */ +void strat_limit_speed_enable(void); +void strat_limit_speed_disable(void); +void strat_limit_speed(void); + +/* get name of traj error with its number */ +const char *get_err(uint8_t err); + +/* test trajectory end condition */ +uint8_t test_traj_end(uint8_t why); + +/* loop until test_traj_end() is true */ +#define wait_traj_end(why) __wait_traj_end_debug(why, __LINE__) +uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line); diff --git a/projects/microb2010/mainboard/strat_building.c b/projects/microb2010/mainboard/strat_building.c new file mode 100644 index 0000000..9abdb5a --- /dev/null +++ b/projects/microb2010/mainboard/strat_building.c @@ -0,0 +1,907 @@ +/* + * Copyright Droids, Microb Technology (2008) + * + * 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: strat_building.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + +#include "../common/i2c_commands.h" +#include "main.h" +#include "cmdline.h" +#include "i2c_protocol.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "strat_avoid.h" +#include "sensor.h" + + +#define DISC_DIST_NEED_GOTO_AVOID 1000 +#define DISC_DIST_PREPARE_BUILD 700 +#define DISC_DIST_SLOW 500 + +#define ERROUT(e) do { \ + err = e; \ + goto end; \ + } while(0) + +static uint8_t is_ready_for_prepare_build(void) +{ + double d, a; + if (distance_from_robot(CENTER_X, CENTER_Y) > + DISC_DIST_PREPARE_BUILD) + return 0; + abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a); + if (a < RAD(-30)) + return 0; + if (a > RAD(30)) + return 0; + return 1; +} + +/* go to the nearest place on the disc. Also prepare the arms for + * building at the correct level. If level==-1, don't move the + * arms. */ +uint8_t strat_goto_disc(int8_t level) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + double d, a, x, y; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + /* workaround for some static cols configurations */ + if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) { + if (time_get_s() > 15) + i2c_mechboard_mode_loaded(); + } + + /* if we are far from the disc, goto backward faster */ + abs_xy_to_rel_da(CENTER_X, CENTER_Y, &d, &a); + if (d > DISC_DIST_NEED_GOTO_AVOID) { + rel_da_to_abs_xy(d - DISC_DIST_PREPARE_BUILD, a, &x, &y); + err = goto_and_avoid(x, y, TRAJ_FLAGS_STD, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + } + +#ifdef HOMOLOGATION + { + int16_t opp_d, opp_a; + trajectory_turnto_xy(&mainboard.traj, + CENTER_X, CENTER_Y); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + + time_wait_ms(500); + + err = get_opponent_da(&opp_d, &opp_a); + if (err == 0 && opp_d < 600 && + (opp_a > 325 || opp_a < 35)) + return END_ERROR; + } +#endif + + trajectory_goto_forward_xy_abs(&mainboard.traj, + CENTER_X, CENTER_Y); + err = WAIT_COND_OR_TRAJ_END(is_ready_for_prepare_build(), + TRAJ_FLAGS_NO_NEAR); + + if (err == END_BLOCKING) + ERROUT(END_BLOCKING); + if (TRAJ_SUCCESS(err)) /* should not reach dest */ + ERROUT(END_ERROR); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + if (level != -1) + i2c_mechboard_mode_prepare_build_both(level); + + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(CENTER_X, + CENTER_Y) < DISC_DIST_SLOW, + TRAJ_FLAGS_NO_NEAR); + + if (err == END_BLOCKING) + ERROUT(END_BLOCKING); + if (TRAJ_SUCCESS(err)) /* should not reach dest */ + ERROUT(END_ERROR); + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + if (err == END_BLOCKING) + ERROUT(END_TRAJ); + if (TRAJ_SUCCESS(err)) /* should not reach dest */ + ERROUT(END_ERROR); + + ERROUT(err); + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* must be called from the checkpoint before zone 1. */ +static uint8_t strat_goto_build_zone1_near(uint8_t level) +{ + uint8_t err; + + /* turn to build zone */ + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + return err; + + /* move forward to reach the build zone */ + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + i2c_mechboard_mode_prepare_build_both(level); + err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); + if (err == END_BLOCKING) { + err = END_TRAJ; + } + + DEBUG(E_USER_STRAT, "build zone reached"); + return err; +} + +/* must be called from the checkpoint before zone 0 */ +static uint8_t strat_goto_build_zone0_near(uint8_t level) +{ + uint8_t err; +#ifdef OLD_STYLE + int16_t cur_y, diff_y, dst_y; +#endif + + /* turn to build zone */ + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_a_abs(&mainboard.traj, COLOR_A(90)); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + return err; + +#ifdef OLD_STYLE + cur_y = position_get_y_s16(&mainboard.pos); + dst_y = COLOR_Y(AREA_Y - (ROBOT_LENGTH/2) - 100); + diff_y = ABS(cur_y - dst_y); + + /* move forward to reach the build zone */ + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + i2c_mechboard_mode_prepare_build_both(level); + trajectory_d_rel(&mainboard.traj, diff_y); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (err == END_BLOCKING) { /* not very good for z0 but... */ + err = END_TRAJ; + } +#else + /* move forward to reach the build zone */ + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW); + i2c_mechboard_mode_prepare_build_both(level); + err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); + if (err == END_BLOCKING) { + err = END_TRAJ; + } +#endif + + DEBUG(E_USER_STRAT, "build zone reached"); + return err; +} + +/* Go to any build zone: disc, 1a or 1b. Doesn't work with zone 0 for + * now... */ +uint8_t strat_goto_build_zone(struct build_zone *zone, uint8_t level) +{ + uint8_t err = END_TRAJ; + uint16_t old_spdd, old_spda; + int16_t checkpoint_x, checkpoint_y; + int16_t errx; + + zone->last_try_time = time_get_s(); + + if (zone->flags & ZONE_F_DISC) + return strat_goto_disc(level); + + DEBUG(E_USER_STRAT, "goto build zone x=%d", zone->checkpoint_x); + + /* workaround for some static cols configurations */ + if (time_get_s() > 15) + i2c_mechboard_mode_loaded(); + + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + checkpoint_x = zone->checkpoint_x; + checkpoint_y = COLOR_Y(zone->checkpoint_y); + errx = position_get_x_s16(&mainboard.pos) - checkpoint_x; + + /* goto checkpoint if we are too far from it, or if error on x + * is too big. */ + if (distance_from_robot(checkpoint_x, checkpoint_y) > 300 || + ABS(errx) > 15) { + err = goto_and_avoid(checkpoint_x, checkpoint_y, + TRAJ_FLAGS_STD, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + } + + if (zone->flags & ZONE_F_ZONE1) + err = strat_goto_build_zone1_near(level); + else if (zone->flags & ZONE_F_ZONE0) + err = strat_goto_build_zone0_near(level); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* return a free temple structure */ +struct temple *strat_get_free_temple(void) +{ + uint8_t i; + + for (i=0; i col_r ? col_l : col_r); + lintel = (get_lintel_count() > 0); + + if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) { + if ((temple->level_r > 5) && + (temple->flags & TEMPLE_F_ON_DISC)) + return 0; + } + + /* return symetric temples only */ + if (temple->level_l != temple->level_r) + return 0; + + if ((time_get_s() - temple->last_try_time) < TEMPLE_DISABLE_TIME) + return 0; + + /* we could do better to work on non-symetric temples */ + if (temple->level_l + max_col + lintel > 9) + return 0; + + if (temple->flags & TEMPLE_F_MONOCOL) + return 0; + + /* XXX don't allow to build on opponent temple. For that we + * must support the little back_mm. */ + if (temple->flags & TEMPLE_F_OPPONENT) + return 0; + + return 1; +} + + +/* return the best existing temple for building */ +struct temple *strat_get_best_temple(void) +{ + uint8_t i; + struct temple *best = NULL; + struct temple *temple = NULL; + + for (i=0; iflags & TEMPLE_F_VALID)) + continue; + + if (strat_can_build_on_temple(temple) == 0) + continue; + + if (best == NULL) { + best = temple; + continue; + } + + /* take the higher temple between 'best' and 'temple' */ + if (best->level_l < temple->level_l) + best = temple; + } + + DEBUG(E_USER_STRAT, "%s() return %p", __FUNCTION__, best); + return best; +} + +/* return the temple we built on the disc if any. If valid == 1, the + * temple must be buildable. */ +struct temple *strat_get_our_temple_on_disc(uint8_t valid) +{ + uint8_t i; + struct temple *temple = NULL; + + if (strat_infos.conf.flags & STRAT_CONF_ONLY_ONE_ON_DISC) { + return NULL; + } + + for (i=0; iflags & TEMPLE_F_VALID)) + continue; + + if (valid == 1 && strat_can_build_on_temple(temple) == 0) + continue; + + if (temple->flags & TEMPLE_F_ON_DISC) + return temple; + } + return NULL; +} + +#define COL_MAX(x,y) (((x)>(y)) ? (x) : (y)) + +#define TIME_FOR_LINTEL 3000L +#define TIME_FOR_BUILD 0L +#define TIME_FOR_COL 800L +#define TIME_MARGIN 2000L + +#define CHECKPOINT_DISC_DIST 380 +#define CHECKPOINT_OTHER_DIST 200 +/* Grow a temple. It will update temple list. */ +uint8_t strat_grow_temple(struct temple *temple) +{ + double checkpoint_x, checkpoint_y; + uint8_t add_level = 0; + uint8_t do_lintel = 1; + uint8_t col_l, col_r, col_max; + uint8_t err; + uint16_t timeout; + + /* XXX temple must be symetric */ + uint8_t level = temple->level_l; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + if (temple->level_l >= 9) + return END_ERROR; + + if ( (temple->zone->flags & ZONE_F_ZONE1) || + (temple->zone->flags & ZONE_F_ZONE0) ) { + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, -17); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + col_l = get_column_count_left(); + col_r = get_column_count_right(); + + if (time_get_s() < 75) { + /* make temple symetric: if we have 1 col on left and 2 cols + * on right, only build one on both sides. */ + if (col_l > col_r) { + col_r = col_l; + do_lintel = 0; + } + if (col_r > col_l) { + col_r = col_l; + do_lintel = 0; + } + if (get_lintel_count() == 0) + do_lintel = 0; + } + else if (col_l != col_r) + do_lintel = 0; + + if (col_l == 0 || col_r == 0) { + if (temple->flags & TEMPLE_F_LINTEL) + do_lintel = 0; + } + + if (col_l == 0 && col_r == 0 && do_lintel == 0) { + DEBUG(E_USER_STRAT, "nothing to do"); + return END_ERROR; + } + + add_level = do_lintel + col_l; + while (level + add_level > 9) { + if (do_lintel) { + do_lintel = 0; + add_level = do_lintel + col_l; + continue; + } + /* we know col_r and col_l are > 0 */ + col_r--; + col_l--; + } + + col_max = COL_MAX(col_r, col_l); + + /* Reduce nb elts if we don't have time */ + timeout = (!!col_max) * TIME_FOR_BUILD; + timeout += col_max * TIME_FOR_COL; + timeout += do_lintel * TIME_FOR_LINTEL; + if ((timeout / 1000L) + time_get_s() > 89 && do_lintel) { + do_lintel = 0; + timeout -= TIME_FOR_LINTEL; + } + if ((timeout / 1000L) + time_get_s() > 89 && col_max) { + if (col_r > 0) + col_r--; + if (col_l > 0) + col_l--; + col_max--; + timeout -= TIME_FOR_COL; + } + + /* take a margin for timeout */ + timeout += (!!col_max) * TIME_MARGIN; + + if (col_l == 0 && col_r == 0 && do_lintel == 0) { + DEBUG(E_USER_STRAT, "nothing to do (2)"); + return END_ERROR; + } + + DEBUG(E_USER_STRAT, "Autobuild: left=%d,%d right=%d,%d lintel=%d", + level, col_l, level, col_r, do_lintel); + + i2c_mechboard_mode_autobuild(level, col_l, I2C_AUTOBUILD_DEFAULT_DIST, + level, col_r, I2C_AUTOBUILD_DEFAULT_DIST, + do_lintel); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == + I2C_MECHBOARD_MODE_AUTOBUILD, 100); + err = WAIT_COND_OR_TIMEOUT(get_mechboard_mode() != + I2C_MECHBOARD_MODE_AUTOBUILD, timeout); + if (err == 0) { + DEBUG(E_USER_STRAT, "timeout building temple (timeout was %d)", timeout); + temple->flags = 0; /* remove temple from list */ + return END_TRAJ; + } + else + DEBUG(E_USER_STRAT, "temple built"); + + /* position of the robot when build the new temple */ + temple->x = position_get_x_s16(&mainboard.pos); + temple->y = position_get_y_s16(&mainboard.pos); + temple->a = position_get_a_deg_s16(&mainboard.pos); + + /* checkpoint is a bit behind us */ + if (temple->zone->flags & ZONE_F_DISC) { + rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI, + &checkpoint_x, &checkpoint_y); + } + else { + rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI, + &checkpoint_x, &checkpoint_y); + } + temple->checkpoint_x = checkpoint_x; + temple->checkpoint_y = checkpoint_y; + + temple->level_l = level + add_level; + temple->dist_l = 0; + temple->angle_l = 0; + + temple->level_r = level + add_level; + temple->dist_r = 0; + temple->angle_r = 0; + + temple->flags = TEMPLE_F_VALID; + + if (distance_from_robot(CENTER_X, CENTER_Y) < 400) + temple->flags |= TEMPLE_F_ON_DISC; + + if (do_lintel) + temple->flags |= TEMPLE_F_LINTEL; + + /* we must push the temple */ + if ( ((temple->zone->flags & ZONE_F_ZONE1) || + (temple->zone->flags & ZONE_F_ZONE0)) && + level <= 1) { + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(level); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + /* Special case for big 3 */ + if (strat_infos.col_in_boobs) { + uint16_t old_spdd, old_spda; + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_SLOW); + DEBUG(E_USER_STRAT, "%s() big 3", __FUNCTION__); + strat_infos.col_in_boobs = 0; + trajectory_d_rel(&mainboard.traj, -120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE, + I2C_MECHBOARD_MODE_CLEAR); + WAIT_COND_OR_TIMEOUT(get_column_count() >= 2, 4000L); + i2c_mechboard_mode_prepare_build_both(level + add_level); + time_wait_ms(800); + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + err = strat_grow_temple(temple); + strat_set_speed(old_spdd, old_spda); + return err; + } + + return END_TRAJ; +} + +#define COL_BACK_DIST 70 +#define COL_ANGLE 20 +#define COL_ARM_DIST 220 + +#define COL_BACK_DIST_ZONE1 35 +#define COL_ARM_DIST_ZONE1 230 +#define COL_ANGLE_ZONE1 19 + +static uint8_t try_build_col(uint8_t l, uint8_t r, + uint8_t lp, uint8_t rp, + uint8_t lvl) +{ + uint8_t max_lvl = lvl + r + l; + + if (l == 0 && r == 0) + return 0; + if (lp - l == 2 && rp - r == 0) + return 0; + if (lp - l == 0 && rp - r == 2) + return 0; + if (max_lvl > 9) + return 0; + if (max_lvl == 9 && rp == 2 && r == 1) + return 0; + return max_lvl; +} + +/* Grow a temple by building a column on it. It will update temple + * list. */ +uint8_t strat_grow_temple_column(struct temple *temple) +{ + uint16_t old_spdd, old_spda; + double checkpoint_x, checkpoint_y; + uint8_t add_level = 0; + uint8_t col_l, col_r; + uint8_t col_l_before, col_r_before; + uint8_t err; + int16_t a_abs, a; + uint8_t level = temple->level_l; + uint8_t lvl_ok = 0, col_l_ok = 0, col_r_ok = 0; + uint8_t tmp_lvl; + int16_t col_arm_dist = COL_ARM_DIST; + int16_t col_back_dist = COL_BACK_DIST; + int16_t col_angle = COL_ANGLE; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + if (level >= 9) + return END_ERROR; + + strat_get_speed(&old_spdd, &old_spda); + + if ( (temple->zone->flags & ZONE_F_ZONE1) || + (temple->zone->flags & ZONE_F_ZONE0) ) { + if (level == 1) + col_arm_dist = COL_ARM_DIST_ZONE1; + col_back_dist = COL_BACK_DIST_ZONE1; + col_angle = COL_ANGLE_ZONE1; + } + + a_abs = position_get_a_deg_s16(&mainboard.pos); + + col_l_before = get_column_count_left(); + col_r_before = get_column_count_right(); + col_l = col_l_before; + col_r = col_r_before; + + /* check number of cols */ + for (col_l = 0; col_l < col_l_before + 1; col_l++) { + for (col_r = 0; col_r < col_r_before + 1; col_r++) { + tmp_lvl = try_build_col(col_l, col_r, + col_l_before, + col_r_before, level); + if (tmp_lvl > lvl_ok) { + lvl_ok = tmp_lvl; + col_l_ok = col_l; + col_r_ok = col_r; + } + } + } + + col_l = col_l_ok; + col_r = col_r_ok; + add_level = col_l + col_r; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + + if (col_l == 0 && col_r == 0) + ERROUT(END_ERROR); + + DEBUG(E_USER_STRAT, "Build col: left=%d right=%d", + col_l, col_r); + + i2c_mechboard_mode_prepare_inside_both(level); + trajectory_d_rel(&mainboard.traj, -col_back_dist); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* build with left arm */ + if (col_l) { + a = a_abs - col_angle; + if (a < -180) + a += 360; + trajectory_a_abs(&mainboard.traj, a); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (time_get_s() > 88) + return END_TIMER; + + if (level >= 7 && get_column_count_left() == 2) + i2c_mechboard_mode_prepare_build_select(level+1, -1); + else + i2c_mechboard_mode_prepare_build_select(level, -1); + time_wait_ms(200); + i2c_mechboard_mode_autobuild(level, col_l, col_arm_dist, + 0, 0, col_arm_dist, 0); + while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) && + (col_r == 0) && + (temple->flags & TEMPLE_F_OPPONENT)) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple_disc(I2C_LEFT_SIDE); + time_wait_ms(500); + trajectory_d_rel(&mainboard.traj, 100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + else if ((level == 1 || level == 0) && (col_r == 0)) { + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(level); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + i2c_mechboard_mode_prepare_inside_select(level+col_l, -1); + } + + /* build with right arm */ + if (col_r) { + a = a_abs + col_angle; + if (a > 180) + a -= 360; + trajectory_a_abs(&mainboard.traj, a); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (time_get_s() > 88) + return END_TIMER; + + if ((level+col_l) >= 7 && get_column_count_right() == 2) + i2c_mechboard_mode_prepare_build_select(-1, level + col_l + 1); + else + i2c_mechboard_mode_prepare_build_select(-1, level + col_l); + time_wait_ms(200); + i2c_mechboard_mode_autobuild(0, 0, col_arm_dist, + level + col_l, col_r, + col_arm_dist, 0); + while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD); + while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD); + + if ((strat_infos.conf.flags & STRAT_CONF_PUSH_OPP_COLS) && + (temple->flags & TEMPLE_F_OPPONENT)) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE); + time_wait_ms(500); + trajectory_d_rel(&mainboard.traj, 100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + else if (level == 1 || level == 0) { + trajectory_d_rel(&mainboard.traj, -100); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + i2c_mechboard_mode_push_temple(level); + time_wait_ms(400); + strat_set_speed(200, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 120); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + i2c_mechboard_mode_prepare_inside_select(-1, level + col_l + col_r); + + } + + trajectory_a_abs(&mainboard.traj, a_abs); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* position of the robot when build the new temple */ + temple->x = position_get_x_s16(&mainboard.pos); + temple->y = position_get_y_s16(&mainboard.pos); + temple->a = position_get_a_deg_s16(&mainboard.pos); + + /* checkpoint is a bit behind us */ + if (temple->zone->flags | ZONE_F_DISC) { + rel_da_to_abs_xy(CHECKPOINT_DISC_DIST, M_PI, + &checkpoint_x, &checkpoint_y); + } + else { + rel_da_to_abs_xy(CHECKPOINT_OTHER_DIST, M_PI, + &checkpoint_x, &checkpoint_y); + } + temple->checkpoint_x = checkpoint_x; + temple->checkpoint_y = checkpoint_y; + + temple->level_l = level + add_level; + temple->dist_l = 0; /* XXX */ + temple->angle_l = 0; + + temple->level_r = level + add_level; + temple->dist_r = 0; + temple->angle_r = 0; + + temple->flags = TEMPLE_F_VALID | TEMPLE_F_MONOCOL; + + if (distance_from_robot(CENTER_X, CENTER_Y) < 400) + temple->flags |= TEMPLE_F_ON_DISC; + + if ( (temple->zone->flags & ZONE_F_ZONE1) || + (temple->zone->flags & ZONE_F_ZONE0) ) { + + } + return END_TRAJ; + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +uint8_t strat_build_new_temple(struct build_zone *zone) +{ + struct temple *temple; + uint8_t level = zone->level; + uint8_t err; + + /* create a dummy temple */ + temple = strat_get_free_temple(); + if (!temple) + return END_ERROR; + + memset(temple, 0, sizeof(*temple)); + temple->level_l = level; + temple->level_r = level; + temple->flags = TEMPLE_F_VALID | TEMPLE_F_LINTEL; + temple->zone = zone; + + zone->flags |= ZONE_F_BUSY; + + if (time_get_s() > 50 && time_get_s() < 85 && + get_lintel_count() == 0) + err = strat_grow_temple_column(temple); + else + err = strat_grow_temple(temple); + + if (!TRAJ_SUCCESS(err)) + temple->flags = 0; + return err; +} + +uint8_t strat_goto_temple(struct temple *temple) +{ + uint16_t old_spdd, old_spda; + uint8_t err; + + DEBUG(E_USER_STRAT, "goto temple %p checkpoint=%d,%d", + temple, temple->checkpoint_x, temple->checkpoint_y); + + temple->last_try_time = time_get_s(); + + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + i2c_mechboard_mode_loaded(); + + err = goto_and_avoid(temple->checkpoint_x, + temple->checkpoint_y, + TRAJ_FLAGS_STD, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + err = strat_goto_build_zone(temple->zone, temple->level_r); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + DEBUG(E_USER_STRAT, "zone reached"); + ERROUT(END_TRAJ); + + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* return the best existing temple for building */ +struct build_zone *strat_get_best_zone(void) +{ + uint8_t i; + struct build_zone *zone = NULL; + + for (i=0; iflags & ZONE_F_BUSY) + continue; + if ((time_get_s() - zone->last_try_time) < ZONE_DISABLE_TIME) + continue; + + return zone; + } + return NULL; +} diff --git a/projects/microb2010/mainboard/strat_column_disp.c b/projects/microb2010/mainboard/strat_column_disp.c new file mode 100644 index 0000000..e0b481e --- /dev/null +++ b/projects/microb2010/mainboard/strat_column_disp.c @@ -0,0 +1,485 @@ +/* + * Copyright Droids, Microb Technology (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 + * (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: strat_column_disp.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "actuator.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_avoid.h" +#include "strat_utils.h" +#include "sensor.h" +#include "i2c_protocol.h" + +#define ERROUT(e) do { \ + err = e; \ + goto end; \ + } while(0) + +/* distance between the wheel axis and the IR sensor */ +#define IR_SHIFT_DISTANCE_RIGHT 85 +#define IR_SHIFT_DISTANCE_LEFT 95 + +/* return red or green sensor */ +#define COLOR_IR_SENSOR(left) \ + ({ \ + uint8_t __ret = 0; \ + if (left) \ + __ret = sensor_get(S_DISP_LEFT); \ + else \ + __ret = sensor_get(S_DISP_RIGHT); \ + \ + __ret; \ + }) \ + +/* eject one col, some error codes are ignored here: we want to be + * sure that the column is correctly ejected. */ +uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a) +{ + uint8_t err; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + i2c_mechboard_mode_eject(); + time_wait_ms(600); + trajectory_a_abs(&mainboard.traj, eject_a); + + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); + i2c_mechboard_mode_clear(); + time_wait_ms(1000); + trajectory_a_abs(&mainboard.traj, pickup_a); + + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + return err; +} + +/* get columns from dispenser. Must be called when the robot is in + * front of the dispenser. */ +static uint8_t strat_pickup_col_disp(struct column_dispenser *disp) +{ + uint16_t old_spdd, old_spda; + int16_t recalib_x, recalib_y; + int16_t eject_a, pickup_a; + uint8_t err, timeout = 0; + int8_t cols_count_before, cols_count_after, cols; + microseconds us; + uint8_t first_try = 1; + uint8_t pickup_mode = I2C_MECHBOARD_MODE_PICKUP; + + /* XXX set lazy pickup mode */ + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + strat_get_speed(&old_spdd, &old_spda); + + cols_count_before = get_column_count(); + pickup_a = COLOR_A(disp->pickup_a); + eject_a = COLOR_A(disp->eject_a); + recalib_x = disp->recalib_x; + recalib_y = COLOR_Y(disp->recalib_y); + + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + + /* turn to dispenser */ + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + trajectory_a_abs(&mainboard.traj, pickup_a); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* go forward until blocking, then go back ~30mm */ + + pickup_wheels_on(); + retry: + if (time_get_s() > 86) { + DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__); + return END_TIMER; + } + + if ((strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE) && + strat_infos.col_in_boobs == 0 && + strat_infos.lazy_pickup_done == 0) { + DEBUG(E_USER_STRAT, "%s() mode lazy", __FUNCTION__); + pickup_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; + strat_infos.col_in_boobs = 1; + strat_infos.lazy_pickup_done = 1; + } + else { + pickup_mode = I2C_MECHBOARD_MODE_PICKUP; + strat_infos.col_in_boobs = 0; + } + + if (first_try) + i2c_mechboard_mode_lazy_harvest(); + else + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + first_try = 0; + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + + err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST); + + trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { + strat_eject_col(eject_a, pickup_a); + goto retry; + } + + /* start to pickup with finger / arms */ + + DEBUG(E_USER_STRAT, "%s pickup now", __FUNCTION__); + + if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP) + i2c_mechboard_mode_pickup(); + else + i2c_mechboard_mode_lazy_pickup(); + WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == pickup_mode, 100); + us = time_get_us2(); + cols = get_column_count(); + while (get_mechboard_mode() == pickup_mode) { + if (get_column_count() != cols) { + cols = get_column_count(); + us = time_get_us2(); + } + if ((get_column_count() - cols_count_before) >= disp->count) { + DEBUG(E_USER_STRAT, "%s no more cols in disp", __FUNCTION__); + break; + } + /* 1 second timeout */ + if (time_get_us2() - us > 1000000L) { + DEBUG(E_USER_STRAT, "%s timeout", __FUNCTION__); + timeout = 1; + break; + } + } + + /* eject if we found a bad color column */ + + if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) { + strat_eject_col(eject_a, pickup_a); + goto retry; + } + + /* only recalib if it was not a timeout or if we got at least + * 2 cols. */ + if (timeout == 0 || (get_column_count() - cols_count_before >= 2)) + strat_reset_pos(recalib_x, recalib_y, pickup_a); + else { + /* else just update x or y depending on disp */ + if (disp == &strat_infos.c1) + strat_reset_pos(recalib_x, DO_NOT_SET_POS, + DO_NOT_SET_POS); + else + strat_reset_pos(recalib_x, DO_NOT_SET_POS, + DO_NOT_SET_POS); + } + + /* go back */ + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -300); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR); + + /* update dispenser count */ + + cols_count_after = get_column_count(); + cols = cols_count_after - cols_count_before; + if (cols > 0) { + DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols); + disp->count -= cols; + if (disp->count < 0) + disp->count = 0; + } + + pickup_wheels_off(); + if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP) + i2c_mechboard_mode_clear(); + else + disp->count -= 2; + + ERROUT(END_TRAJ); + +end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* + * Go in front of a dispenser. It will update the dispenser if it is + * c2 or c3 if we detect that this dispenser does not exist. + */ +uint8_t strat_goto_col_disp(struct column_dispenser **pdisp) +{ + uint8_t err; + int16_t checkpoint_x, checkpoint_y; + int16_t scan_a; + uint16_t old_spdd, old_spda, scan_left; + int16_t pos1x, pos1y, pos2x, pos2y, pos, dist; + int16_t margin_col2, margin_col3; + struct column_dispenser *disp = *pdisp; + + if (disp->count <= 0) + return END_ERROR; + + if (disp->last_try_time >= time_get_s()) + return END_ERROR; + + disp->last_try_time = time_get_s(); + + strat_get_speed(&old_spdd, &old_spda); + + i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE, + I2C_MECHBOARD_MODE_CLEAR); + + /* set some useful variables */ + checkpoint_x = disp->checkpoint_x; + checkpoint_y = COLOR_Y(disp->checkpoint_y); + scan_a = COLOR_A(disp->scan_a); + scan_left = COLOR_INVERT(disp->scan_left); + + /* goto checkpoint */ + DEBUG(E_USER_STRAT, "%s(): goto %s (%d,%d) scan_left=%d", + __FUNCTION__, disp->name, checkpoint_x, + checkpoint_y, scan_left); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + +#if 0 + /* we have an intermediate checkpoint if we are on our + * side. If goto_and_avoid() returns END_ERROR, skip + * this checkpoint. */ + if (position_get_x_s16(&mainboard.pos) < 1500) { + err = goto_and_avoid(1000, COLOR_Y(1500), + TRAJ_FLAGS_STD, + TRAJ_FLAGS_STD); + if (!TRAJ_SUCCESS(err) && err != END_ERROR) + ERROUT(err); + } +#endif + /* go to checkpoint near the dispenser */ + err = goto_and_avoid(checkpoint_x, checkpoint_y, + TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* turn to correct angle to prepare scanning */ + + trajectory_a_abs(&mainboard.traj, scan_a); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* scan now */ + + DEBUG(E_USER_STRAT, "%s(): scanning dispenser", __FUNCTION__); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -1000); + err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(scan_left), + TRAJ_FLAGS_NO_NEAR); + if (err) /* we should not reach end */ + ERROUT(END_ERROR); + pos1x = position_get_x_s16(&mainboard.pos); + pos1y = position_get_y_s16(&mainboard.pos); + + err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(scan_left), + TRAJ_FLAGS_NO_NEAR); + if (err) + ERROUT(END_ERROR); + pos2x = position_get_x_s16(&mainboard.pos); + pos2y = position_get_y_s16(&mainboard.pos); + + dist = distance_between(pos1x, pos1y, pos2x, pos2y); + DEBUG(E_USER_STRAT, "%s(): scan done dist=%d", __FUNCTION__, dist); + + if (scan_left) + trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_LEFT + dist/2); + else + trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_RIGHT + dist/2); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + if (disp == &strat_infos.c1) + ERROUT(END_TRAJ); + + /* mark c2 or c3 as empty... */ + if (strat_infos.c2.count == 0 || strat_infos.c3.count == 0) + ERROUT(END_TRAJ); + + pos = (pos2y + pos1y) / 2; + if (scan_a == 90) /* y is decreasing when scanning */ + pos -= 80; + else if (scan_a == -90) /* y is increasing when scanning */ + pos += 80; + + margin_col2 = ABS(pos - COLOR_Y(strat_infos.c2.recalib_y)); + margin_col3 = ABS(pos - COLOR_Y(strat_infos.c3.recalib_y)); + + if (margin_col3 > margin_col2) { + DEBUG(E_USER_STRAT, "%s(): delete disp c3 (scan_pos=%d)", __FUNCTION__, pos); + strat_infos.c3.count = 0; + *pdisp = &strat_infos.c2; + if (strat_infos.c3.last_try_time > strat_infos.c2.last_try_time) + strat_infos.c2.last_try_time = strat_infos.c3.last_try_time; + } + else { + DEBUG(E_USER_STRAT, "%s(): delete disp c2 (scan_pos=%d)", __FUNCTION__, pos); + strat_infos.c2.count = 0; + *pdisp = &strat_infos.c3; + if (strat_infos.c2.last_try_time > strat_infos.c3.last_try_time) + strat_infos.c3.last_try_time = strat_infos.c2.last_try_time; + } + ERROUT(END_TRAJ); + +end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* return the best dispenser between the 2 */ +static struct column_dispenser * +strat_disp_compare(struct column_dispenser *a, + struct column_dispenser *b) +{ + uint8_t want_cols = 4 - get_column_count(); + + DEBUG(E_USER_STRAT, "%s() want_cols=%d", __FUNCTION__, want_cols); + + /* an empty dispenser is not valid */ + if (a->count == 0) + return b; + if (b->count == 0) + return a; + + /* try to do a round robin: this is not optimal, but at least + * we will try another dispenser when one fails. */ + if (a->last_try_time < b->last_try_time) { + return a; + } + if (b->last_try_time < a->last_try_time) { + return b; + } + + /* take the one with the most columns */ + if (a->count >= want_cols && b->count < want_cols) + return a; + + /* take the one with the most columns */ + if (b->count >= want_cols && a->count < want_cols) + return b; + + /* the closer is the better */ + if (distance_from_robot(a->recalib_x, COLOR_Y(a->recalib_y)) < + distance_from_robot(b->recalib_x, COLOR_Y(b->recalib_y))) { + return a; + } + return b; +} + +/* choose the best dispenser */ +static struct column_dispenser *strat_get_best_col_disp(void) +{ + struct column_dispenser *disp; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + /* for the first call, use c3 */ + if (strat_infos.c1.last_try_time == 0 && + strat_infos.c2.last_try_time == 0 && + strat_infos.c3.last_try_time == 0) + return &strat_infos.c2; // XXX c3 + + DEBUG(E_USER_STRAT, "%s(): compare values", __FUNCTION__); + + /* else compare with standard conditions */ + disp = strat_disp_compare(&strat_infos.c1, &strat_infos.c2); + disp = strat_disp_compare(disp, &strat_infos.c3); + + if (disp->count == 0) + return NULL; + + return disp; +} + +/* choose the best dispenser, depending on disp count, distance, + * tries, ... and go pickup on it. */ +uint8_t strat_pickup_columns(void) +{ + struct column_dispenser *disp; + uint8_t err; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + disp = strat_get_best_col_disp(); + + if (disp == NULL) { + DEBUG(E_USER_STRAT, "%s(): no col disp found", __FUNCTION__); + return END_ERROR; + } + + err = strat_goto_col_disp(&disp); + if (!TRAJ_SUCCESS(err)) + return err; + + err = strat_pickup_col_disp(disp); + if (!TRAJ_SUCCESS(err)) + return err; + + return END_TRAJ; +} diff --git a/projects/microb2010/mainboard/strat_lintel.c b/projects/microb2010/mainboard/strat_lintel.c new file mode 100644 index 0000000..72ab299 --- /dev/null +++ b/projects/microb2010/mainboard/strat_lintel.c @@ -0,0 +1,247 @@ +/* + * Copyright Droids, Microb Technology (2008) + * + * 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: strat_lintel.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_avoid.h" +#include "strat_utils.h" +#include "sensor.h" +#include "i2c_protocol.h" + +#define ERROUT(e) do { \ + err = e; \ + goto end; \ + } while(0) + +#define X_PRE_MARGIN 20 +#define X_POST_MARGIN 10 + +/* + * goto lintel disp. Return END_TRAJ if success or if there is nothing + * to do. Return END_ERROR if dest cannot be reached, else, it may + * return END_OBSTACLE or END_BLOCKING. + */ +uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp) +{ + uint8_t err, first_try = 1, right_ok, left_ok; + uint16_t old_spdd, old_spda; + int16_t left_cur, right_cur, a; + + if (disp->count == 0) + return END_ERROR; + + if (get_lintel_count() == 2) + return END_ERROR; + + if (disp->last_try_time >= time_get_s()) + return END_ERROR; + + disp->last_try_time = time_get_s(); + + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + DEBUG(E_USER_STRAT, "%s(): goto %s", __FUNCTION__, disp->name); + i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE); + + err = goto_and_avoid_backward(disp->x, COLOR_Y(400), + TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + trajectory_a_abs(&mainboard.traj, COLOR_A(-90)); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (time_get_s() > 86) { + DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__); + return END_TIMER; + } + + i2c_mechboard_mode_prepare_get_lintel(); + retry: + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST); + err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST); + if (err == END_BLOCKING) { + a = position_get_a_deg_s16(&mainboard.pos); + /* only reset pos if angle is not too different */ + if (ABS(a - COLOR_A(-90)) < 5) + strat_reset_pos(DO_NOT_SET_POS, + COLOR_Y(ROBOT_LENGTH/2), + COLOR_A(-90)); + } + else if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + i2c_mechboard_mode_get_lintel(); + time_wait_ms(500); + + left_cur = sensor_get_adc(ADC_CSENSE3); + left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN); + right_cur = mechboard.pump_right1_current; + right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN); + + DEBUG(E_USER_STRAT, "%s left_ok=%d (%d), right_ok=%d (%d)", __FUNCTION__, + left_ok, left_cur, right_ok, right_cur); + if (first_try) { + if (!right_ok && !left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + } + /* XXX recalib x ? */ + else if (right_ok && !left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + strat_set_speed(500, 500); + trajectory_d_a_rel(&mainboard.traj, -200, 30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_d_a_rel(&mainboard.traj, 190, -30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + first_try = 0; + goto retry; + } + else if (!right_ok && left_ok) { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + strat_set_speed(500, 500); + trajectory_d_a_rel(&mainboard.traj, -200, -30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_d_a_rel(&mainboard.traj, 190, 30); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + first_try = 0; + goto retry; + } + /* else, lintel is ok */ + else { + strat_infos.taken_lintel ++; + i2c_mechboard_mode_put_lintel(); + } + } + else { + if (right_ok && left_ok) { + /* lintel is ok */ + strat_infos.taken_lintel ++; + i2c_mechboard_mode_put_lintel(); + } + else { + i2c_mechboard_mode_prepare_get_lintel(); + time_wait_ms(300); + } + } + disp->count--; + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + trajectory_d_rel(&mainboard.traj, -250); + err = wait_traj_end(TRAJ_FLAGS_STD); + + ERROUT(err); + +end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* go pickup lintels on dispensers. Return END_TRAJ on success or if + + * there is nothing to do, else return the error status. */ +uint8_t strat_pickup_lintels(void) +{ + uint8_t err; + + if (get_column_count() != 0) + return END_ERROR; + + if (strat_infos.l1.count == 0 && strat_infos.l2.count == 0) + return END_TRAJ; + + /* skip if it's too early */ + if (time_get_s() < strat_infos.conf.lintel_min_time) + return END_TRAJ; + + /* skip next lintel if we want only one */ + if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) { + if (strat_infos.taken_lintel) + return END_TRAJ; + } + + /* don't take lintel now if we already have one and if there + * is not much time */ + if (get_lintel_count() && time_get_s() > 75) + return END_TRAJ; + + /* take lintel 1 */ + err = strat_goto_lintel_disp(&strat_infos.l1); + if (!TRAJ_SUCCESS(err) && err != END_ERROR) + return err; + + /* skip next lintel if we want only one */ + if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) { + if (strat_infos.taken_lintel) + return END_TRAJ; + } + + /* don't take lintel now if we already have one and if there + * is not much time */ + if (get_lintel_count() && time_get_s() > 75) + return END_TRAJ; + + /* take lintel 2 */ + err = strat_goto_lintel_disp(&strat_infos.l2); + if (!TRAJ_SUCCESS(err) && err != END_ERROR) + return err; + + return END_TRAJ; +} diff --git a/projects/microb2010/mainboard/strat_scan.c b/projects/microb2010/mainboard/strat_scan.c new file mode 100644 index 0000000..4140226 --- /dev/null +++ b/projects/microb2010/mainboard/strat_scan.c @@ -0,0 +1,689 @@ +/* + * Copyright Droids, Microb Technology (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 + * (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: strat_scan.c,v 1.2 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + +#include "../common/i2c_commands.h" +#include "main.h" +#include "cmdline.h" +#include "i2c_protocol.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "strat_avoid.h" +#include "sensor.h" + +#define ERROUT(e) do { \ + err = e; \ + goto end; \ + } while(0) + + +void scanner_dump_state(void) +{ + uint8_t status; + + printf_P(PSTR("scanner state:\r\n")); + status = sensorboard.scan_status; + + printf_P(PSTR(" status=%x: "), sensorboard.scan_status); + + if (status & I2C_SCAN_DONE) + printf_P(PSTR("DONE ")); + else + printf_P(PSTR("RUNNING ")); + if (status & I2C_SCAN_MAX_COLUMN) + printf_P(PSTR("OBSTACLE ")); + + printf_P(PSTR("\r\n")); + + if (sensorboard.dropzone_h == -1) { + printf_P(PSTR("No zone found\r\n")); + return; + } + + printf_P(PSTR(" column_h=%d\r\n"), sensorboard.dropzone_h); + printf_P(PSTR(" column_x=%d\r\n"), sensorboard.dropzone_x); + printf_P(PSTR(" column_y=%d\r\n"), sensorboard.dropzone_y); +} + +/* must be larger than the disc poly */ +#define CHECKPOINT_DIST 600 + +/* go to a specific angle on disc, if level == -1, don't move arms */ +uint8_t strat_goto_disc_angle(int16_t a_deg, int8_t level) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + double x, y; + uint8_t need_clear = 0; + + DEBUG(E_USER_STRAT, "%s(a_deg=%d, level=%d)", __FUNCTION__, + a_deg, level); + + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + /* workaround for some static cols configurations */ + if ((strat_infos.conf.flags & STRAT_CONF_EARLY_SCAN) == 0) { + if (time_get_s() > 15) + i2c_mechboard_mode_loaded(); + } + /* another workaround for offensive configuration */ + else { + if (strat_infos.i2c_loaded_skipped == 0) { + DEBUG(E_USER_STRAT, "%s() need clear"); + strat_infos.i2c_loaded_skipped = 1; + i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE, + I2C_MECHBOARD_MODE_CLEAR); + need_clear = 1; + } + else + i2c_mechboard_mode_loaded(); + } + + + /* calculate the checkpoint */ + x = CHECKPOINT_DIST; + y = 0; + rotate(&x, &y, RAD(a_deg)); + x += CENTER_X; + y += CENTER_Y; + + err = goto_and_avoid(x, y, TRAJ_FLAGS_STD, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* early offensive conf only */ + if (need_clear) { + err = WAIT_COND_OR_TIMEOUT(get_column_count() == 2, + 3000L); + DEBUG(E_USER_STRAT, "%s() offensive: err=%d", err); + if (err == 0) /* timeout */ + return END_ERROR; + } + err = strat_goto_disc(level); + + end: + strat_set_speed(old_spdd, old_spda); + return err; + +} + +/* only valid for temple on disc */ +int16_t strat_get_temple_angle(struct temple *temple) +{ + int16_t x, y; + double a; + + x = temple->x; + y = temple->y; + x -= CENTER_X; + y -= CENTER_Y; + a = atan2(y, x); + return DEG(a); +} + +#define SCAN_ANGLE_OFFSET (-40) +int16_t strat_temple_angle_to_scan_angle(int16_t temple_angle) +{ + return temple_angle + SCAN_ANGLE_OFFSET; +} + +/* start to scan after this distance */ +#define DIST_START_SCAN 50 + +/* scan during this distance (includes DIST_START_SCAN) */ +#define DIST_SCAN 430 + +/* speed of the scan */ +#define SPEED_SCAN 450 + +/* from scanner point of view */ +#define DISC_CENTER_X 15 +#define DISC_CENTER_Y 13 + +/* distance of the checkpoint */ +#define CKPT_DST 550. + +/* to convert in robot coordinates */ +#define SIDE_OFFSET (ROBOT_WIDTH/2) +#define DIST_OFFSET (DIST_SCAN - DIST_START_SCAN) + +/* center of the disc in robot coordinates */ +#define CENTER_X_SCANNER 166 +#define CENTER_Y_SCANNER 174 + +/* center of the disc in scanner millimeters coordinates */ +#define CENTER_X_SCANNER2 120 +#define CENTER_Y_SCANNER2 155 + +/* structure filled by strat_scan_disc() */ +struct scan_disc_result { +#define SCAN_FAILED 0 +#define SCAN_VALID 1 + uint8_t status; + +#define SCAN_ACTION_BUILD_TEMPLE 0 +#define SCAN_ACTION_BUILD_COL 1 + uint8_t action; + + uint8_t level; +}; + +#define SCAN_MODE_CHECK_TEMPLE 0 +#define SCAN_MODE_SCAN_COL 1 +#define SCAN_MODE_SCAN_TEMPLE 2 + +int8_t strat_scan_get_checkpoint(uint8_t mode, int16_t *ckpt_rel_x, + int16_t *ckpt_rel_y, int16_t *back_mm) +{ + int16_t center_rel_x, center_rel_y; + int16_t col_rel_x, col_rel_y; + int16_t col_vect_x, col_vect_y; + double col_vect_norm; + int16_t ckpt_vect_x, ckpt_vect_y; + + /* do some filtering */ + if (mode == SCAN_MODE_SCAN_TEMPLE && + sensorboard.dropzone_x > CENTER_X_SCANNER) { + DEBUG(E_USER_STRAT, "x too big"); + return -1; + } + + /* process relative pos from robot point of view */ + center_rel_x = DIST_OFFSET - CENTER_Y_SCANNER; + center_rel_y = -(SIDE_OFFSET + CENTER_X_SCANNER); + + col_rel_x = DIST_OFFSET - sensorboard.dropzone_y; + col_rel_y = -(SIDE_OFFSET + sensorboard.dropzone_x); + DEBUG(E_USER_STRAT, "col_rel = %d,%d", col_rel_x, col_rel_y); + + /* vector from center to column */ + col_vect_x = col_rel_x - center_rel_x; + col_vect_y = col_rel_y - center_rel_y; + col_vect_norm = norm(col_vect_x, col_vect_y); + + /* vector from center to ckpt */ + ckpt_vect_x = (double)(col_vect_x) * CKPT_DST / col_vect_norm; + ckpt_vect_y = (double)(col_vect_y) * CKPT_DST / col_vect_norm; + + /* rel pos of ckpt */ + *ckpt_rel_x = center_rel_x + ckpt_vect_x; + *ckpt_rel_y = center_rel_y + ckpt_vect_y; + + /* do some filtering */ + if (col_vect_norm > 150 || col_vect_norm < 30) { + DEBUG(E_USER_STRAT, "bad norm"); + return -1; + } + + if (mode == SCAN_MODE_SCAN_TEMPLE) { + if (col_vect_norm > 50) { + *back_mm = ABS(col_vect_norm-50); + } + } + return 0; +} + +/* + * scan the disc: return END_TRAJ on success (and status in result is + * set to SCAN_VALID). In this case, all the scan_disc_result + * structure is filled with appropriate parameters. mode can be + * 'check' or 'scan_col'. Note that if we do a check_temple, the level + * field in structure must be filled first by the caller. + */ +uint8_t strat_scan_disc(int16_t angle, uint8_t mode, + struct scan_disc_result *result) +{ + uint16_t old_spdd, old_spda; + uint8_t err, stop_scanner = 0; + uint8_t original_mode = mode; + int16_t pos1x, pos1y, dist; + int16_t back_mm = 0; + + int16_t ckpt_rel_x = 0, ckpt_rel_y = 0; + + double center_abs_x, center_abs_y; + double ckpt_rel_d, ckpt_rel_a; + double ckpt_abs_x, ckpt_abs_y; + + /* mark status as failed for now */ + result->status = SCAN_FAILED; + + DEBUG(E_USER_STRAT, "%s(angle=%d)", __FUNCTION__, angle); + + strat_get_speed(&old_spdd, &old_spda); + + /* go on disc */ + err = strat_goto_disc_angle(angle, -1); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* wait opponent before scanning */ + if (strat_infos.conf.wait_opponent > 0) { + int16_t opp_x, opp_y, opp_d, opp_a; + int8_t err; + microseconds us; + + us = time_get_us2(); + while ((err = get_opponent_xyda(&opp_x, &opp_y, + &opp_d, &opp_a)) == 0) { + if (opp_d > 600) + break; + if (opp_a < 180) + break; + + if (time_get_us2() - us >= (uint32_t)strat_infos.conf.wait_opponent * 1000000L) + return END_ERROR; + } + } + + /* save absolute position of disc */ + rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y); + + strat_limit_speed_disable(); + + /* go back and prepare to scan */ + strat_set_speed(1000, 1000); + trajectory_d_a_rel(&mainboard.traj, -140, 130); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + /* XXX check that opp is not behind us */ + + /* prepare scanner */ + + stop_scanner = 1; + i2c_sensorboard_scanner_prepare(); + time_wait_ms(250); /* XXX to remove ? */ + + strat_set_speed(SPEED_SCAN, 1000); + + pos1x = position_get_x_s16(&mainboard.pos); + pos1y = position_get_y_s16(&mainboard.pos); + trajectory_d_rel(&mainboard.traj, -DIST_SCAN); + + while (1) { + err = test_traj_end(TRAJ_FLAGS_SMALL_DIST); + if (err != 0) + break; + + dist = distance_from_robot(pos1x, pos1y); + + if (dist > DIST_START_SCAN) + break; + + if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) { + err = END_ERROR; + break; + } + } + + if (err) { + if (TRAJ_SUCCESS(err)) + err = END_ERROR; /* should not reach end */ + strat_hardstop(); + trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + ERROUT(err); + } + + /* start the scanner */ + + i2c_sensorboard_scanner_start(); + + err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN, + TRAJ_FLAGS_NO_NEAR); + if (err == 0) + err = END_ERROR; + if (!TRAJ_SUCCESS(err)) { + strat_hardstop(); + trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y); + wait_traj_end(TRAJ_FLAGS_NO_NEAR); + ERROUT(err); + } + + wait_scan_done(1000); + + i2c_sensorboard_scanner_stop(); + stop_scanner = 0; + + if (mode == SCAN_MODE_CHECK_TEMPLE) { + i2c_sensorboard_scanner_algo_check(result->level, + CENTER_X_SCANNER2, + CENTER_Y_SCANNER2); + i2cproto_wait_update(); + wait_scan_done(1000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1 && + !(strat_infos.conf.flags & STRAT_CONF_SKIP_WHEN_CHECK_FAILS)) { + DEBUG(E_USER_STRAT, "-- try to build a temple"); + mode = SCAN_MODE_SCAN_TEMPLE; + } + else { + result->action = SCAN_ACTION_BUILD_TEMPLE; + /* level is already set by caller */ + } + } + + if (mode == SCAN_MODE_SCAN_TEMPLE) { + i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC, + DISC_CENTER_X, + DISC_CENTER_Y); + i2cproto_wait_update(); + wait_scan_done(1000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1 || + strat_scan_get_checkpoint(mode, &ckpt_rel_x, + &ckpt_rel_y, &back_mm)) { + if (original_mode != SCAN_MODE_CHECK_TEMPLE) { + DEBUG(E_USER_STRAT, "-- try to build a column"); + mode = SCAN_MODE_SCAN_COL; + } + else { + DEBUG(E_USER_STRAT, "-- check failed"); + } + } + else { + result->action = SCAN_ACTION_BUILD_TEMPLE; + result->level = sensorboard.dropzone_h; + } + } + + if (mode == SCAN_MODE_SCAN_COL) { + i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC, + DISC_CENTER_X, + DISC_CENTER_Y); + i2cproto_wait_update(); + wait_scan_done(1000); + scanner_dump_state(); + + if (sensorboard.dropzone_h == -1 || + strat_scan_get_checkpoint(mode, &ckpt_rel_x, + &ckpt_rel_y, &back_mm)) { + ERROUT(END_ERROR); + } + else { + result->action = SCAN_ACTION_BUILD_COL; + result->level = sensorboard.dropzone_h; + } + } + + if (sensorboard.dropzone_h == -1) { + ERROUT(END_ERROR); + } + + if (mode == SCAN_MODE_CHECK_TEMPLE) { + ckpt_rel_x = 220; + ckpt_rel_y = 100; + } + + DEBUG(E_USER_STRAT, "rel xy for ckpt is %d,%d", ckpt_rel_x, ckpt_rel_y); + + rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y); + abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a); + + DEBUG(E_USER_STRAT, "abs ckpt is %2.2f,%2.2f", ckpt_abs_x, ckpt_abs_y); + + strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST); + + /* intermediate checkpoint for some positions */ + if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) { + trajectory_goto_xy_rel(&mainboard.traj, 200, 100); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + } + + trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + if (result->action == SCAN_ACTION_BUILD_TEMPLE) { + i2c_mechboard_mode_prepare_build_both(result->level); + } + + trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + pos1x = position_get_x_s16(&mainboard.pos); + pos1y = position_get_y_s16(&mainboard.pos); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); + trajectory_d_rel(&mainboard.traj, 400); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200, + TRAJ_FLAGS_SMALL_DIST); + if (err == 0) { + strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW); + trajectory_d_rel(&mainboard.traj, 400); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + if (TRAJ_SUCCESS(err)) + err = END_ERROR; /* should not reach end */ + if (err != END_BLOCKING && !TRAJ_SUCCESS(err)) + ERROUT(err); + + if (back_mm) { + trajectory_d_rel(&mainboard.traj, -back_mm); + wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + } + + result->status = SCAN_VALID; + + strat_limit_speed_enable(); + return END_TRAJ; + + end: + if (stop_scanner) + i2c_sensorboard_scanner_stop(); + strat_limit_speed_enable(); + strat_set_speed(old_spdd, old_spda); + return err; + +} + +/* do action according to scanner result. temple argument can be NULL + * if it's a new one (from opponent) or it can be our previous + * temple. */ +uint8_t strat_scan_do_action(struct scan_disc_result *scan_result, + struct temple *temple, struct build_zone *zone) +{ + uint8_t err; + + /* remove the temple from the list */ + if (scan_result->status != SCAN_VALID) + return END_ERROR; + + if (temple) { + /* we were scanning a temple, remove it */ + if (scan_result->level != temple->level_l) { + temple->flags = 0; + temple = NULL; + } + } + + if (temple == NULL) { + temple = strat_get_free_temple(); + if (temple == NULL) + return END_ERROR; + memset(temple, 0, sizeof(*temple)); + temple->level_l = scan_result->level; + temple->level_r = scan_result->level; + temple->flags = TEMPLE_F_OPPONENT | + TEMPLE_F_VALID | TEMPLE_F_LINTEL; + temple->zone = zone; + } + zone->flags |= ZONE_F_BUSY; + + switch (scan_result->action) { + + case SCAN_ACTION_BUILD_COL: + err = strat_grow_temple_column(temple); + break; + + case SCAN_ACTION_BUILD_TEMPLE: + err = strat_grow_temple(temple); + break; + default: + err = END_TRAJ; + break; + } + if (!TRAJ_SUCCESS(err)) + temple->flags = 0; + return err; +} + +uint8_t strat_build_on_opponent_temple(void) +{ + struct temple *temple; + uint8_t err; + struct scan_disc_result scan_result; + int16_t temple_angle; + + if (time_get_s() < strat_infos.conf.scan_opp_min_time) + return END_TRAJ; + + strat_infos.conf.scan_opp_min_time = + time_get_s() + strat_infos.conf.delay_between_opp_scan; + + /* scan on disc only */ + if (strat_infos.conf.scan_opp_angle == -1) { + temple = strat_get_our_temple_on_disc(0); + + /* scan the opposite of our temple if we found + * one on disc */ + if (temple) { + temple_angle = strat_get_temple_angle(temple); + temple_angle += 180; + if (temple_angle > 180) + temple_angle -= 360; + } + /* else scan at 0 deg (opponent side) */ + else { + temple_angle = 0; + } + } + else { + /* user specified scan position */ + temple_angle = strat_infos.conf.delay_between_opp_scan; + if (temple_angle > 180) + temple_angle -= 360; + } + temple_angle = strat_temple_angle_to_scan_angle(temple_angle); + + + err = strat_scan_disc(temple_angle, SCAN_MODE_SCAN_TEMPLE, + &scan_result); + if (!TRAJ_SUCCESS(err)) + return err; + + /* XXX on disc only */ + err = strat_scan_do_action(&scan_result, NULL, + &strat_infos.zone_list[0]); + + if (!TRAJ_SUCCESS(err)) + return err; + + err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD); + return err; +} + +uint8_t strat_check_temple_and_build(void) +{ + struct temple *temple; + uint8_t err; + struct scan_disc_result scan_result; + int16_t temple_angle; + + if (time_get_s() < strat_infos.conf.scan_our_min_time) + return END_TRAJ; + strat_infos.conf.scan_our_min_time = + time_get_s() + strat_infos.conf.delay_between_our_scan; + + /* on disc only, symetric only */ + temple = strat_get_our_temple_on_disc(1); + if (temple == NULL) + return END_TRAJ; + + temple_angle = strat_get_temple_angle(temple); + temple_angle = strat_temple_angle_to_scan_angle(temple_angle); + + scan_result.level = temple->level_l; + err = strat_scan_disc(temple_angle, SCAN_MODE_CHECK_TEMPLE, + &scan_result); + if (scan_result.status != SCAN_VALID) { + temple->flags = 0; + temple = NULL; + } + /* no column after a temple check */ + else if (scan_result.action == SCAN_ACTION_BUILD_COL && + time_get_s() < 70) + err = END_ERROR; + if (!TRAJ_SUCCESS(err)) + return err; + + err = strat_scan_do_action(&scan_result, temple, + temple->zone); + if (!TRAJ_SUCCESS(err)) + return err; + + err = strat_escape(&strat_infos.zone_list[0], TRAJ_FLAGS_STD); + return err; +} diff --git a/projects/microb2010/mainboard/strat_static_columns.c b/projects/microb2010/mainboard/strat_static_columns.c new file mode 100644 index 0000000..7af6043 --- /dev/null +++ b/projects/microb2010/mainboard/strat_static_columns.c @@ -0,0 +1,416 @@ +/* + * Copyright Droids, Microb Technology (2008) + * + * 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: strat_static_columns.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "strat.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "strat_avoid.h" +#include "sensor.h" +#include "i2c_protocol.h" + +#define ERROUT(e) do { \ + err = e; \ + goto end; \ + } while(0) + +#define BIG_DIST 5000 + +/* + * must be called from start area. + * get 4 static columns and build a temple on the disc + */ +uint8_t strat_static_columns(uint8_t configuration) +{ + uint8_t err; + uint8_t col1_present = 0, col4_present = 0; + uint16_t old_spdd, old_spda; + + DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration); + + strat_get_speed(&old_spdd, &old_spda); + + /* calibrate scanner */ + i2c_sensorboard_scanner_calib(); + + i2c_mechboard_mode_harvest(); + + /* go straight. total distance is less than 5 meters */ + strat_set_speed(1000, 1000); + trajectory_d_rel(&mainboard.traj, BIG_DIST); + + /* when y > 50, break */ + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + + /* turn to 90° abs while going forward */ + DEBUG(E_USER_STRAT, "turn now"); + strat_set_speed(1000, 350); + trajectory_only_a_abs(&mainboard.traj, COLOR_A(90)); + + /* when y > 100, check the presence of column 4 */ + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT)) + col4_present = 1; + if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT)) + col4_present = 1; + + /* when y > 120, check the presence of column 1 */ + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT)) + col1_present = 1; + if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT)) + col1_present = 1; + + /* when y > 130, break */ + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + + strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE; + + DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present); + DEBUG(E_USER_STRAT, "have %d cols", get_column_count()); + + if (configuration == 0) { + if (get_column_count() > 2) { + configuration = 1; + if (col4_present || col1_present) { + strat_infos.s_cols.flags |= + STATIC_COL_LINE2_DONE; + } + else { + strat_infos.s_cols.flags |= + STATIC_COL_LINE1_DONE; + } + } + + /* only 2 colums on the first line */ + else { + /* all other colums are on line 1 */ + if (col4_present && col1_present) { + configuration = 2; + strat_infos.s_cols.flags |= + STATIC_COL_LINE2_DONE; + } + + /* only 2 columns on line 1, so there are also + * 2 on line 2 */ + else if (col4_present || col1_present) { + configuration = 4; + strat_infos.s_cols.flags |= + STATIC_COL_LINE2_DONE; + } + + /* all other columns are on line 2 */ + else { + configuration = 3; + strat_infos.s_cols.flags |= + STATIC_COL_LINE1_DONE; + } + } + } + + strat_infos.s_cols.configuration = configuration; + DEBUG(E_USER_STRAT, "use configuration %d", configuration); + + if (configuration == 1) { + /* we already got 4 columns, go to the disc directly */ + + strat_set_speed(1500, 900); + trajectory_only_a_abs(&mainboard.traj, COLOR_A(0)); + err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD); + + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + } + else if (configuration == 2 /* go from line 0 to line 1 */) { + strat_set_speed(800, 1000); + /* relative is needed here */ + trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); + err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + strat_set_speed(1000, 600); + err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100), + TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + } + else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns + on line 2*/) { + strat_set_speed(1000, 600); + /* relative is needed here */ + trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); + err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + } + else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns + on line 2 */) { + strat_set_speed(1000, 600); + /* relative is needed here */ + trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); + err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + } + else { + trajectory_stop(&mainboard.traj); + } + + ERROUT(END_TRAJ); + + end: + strat_set_speed(old_spdd, old_spda); + return err; +} + +/* + * get last 2 columns + * must be called after the first temple building + */ +uint8_t strat_static_columns_pass2(void) +{ + uint16_t old_spdd, old_spda; + uint8_t side, err, next_mode; + + DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); + + strat_get_speed(&old_spdd, &old_spda); + + if (get_color() == I2C_COLOR_RED) + side = I2C_RIGHT_SIDE; + else + side = I2C_LEFT_SIDE; + + if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2) + next_mode = I2C_MECHBOARD_MODE_STORE; + else + next_mode = I2C_MECHBOARD_MODE_HARVEST; + + switch (strat_infos.s_cols.configuration) { + + /* configuration 1: 4 cols on line 0 */ + case 1: + if (strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE) { + /* go on line 2 */ + + strat_set_speed(2000, 700); + trajectory_d_a_rel(&mainboard.traj, -450, COLOR_A(35)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + i2c_mechboard_mode_prepare_pickup_next(side, + next_mode); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + trajectory_goto_forward_xy_abs(&mainboard.traj, + LINE2_X, + COLOR_Y(400)); + err = WAIT_COND_OR_TRAJ_END(get_column_count() == 2, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + } + else { + /* go on line 1 */ + strat_set_speed(2000, 700); + trajectory_d_a_rel(&mainboard.traj, -650, COLOR_A(55)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + i2c_mechboard_mode_prepare_pickup_next(side, + next_mode); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + + err = goto_and_avoid_forward(LINE1_X, + COLOR_Y(400), + TRAJ_FLAGS_NO_NEAR, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + } + + ERROUT(END_TRAJ); + break; + + /* configuration 2: 2 cols on line 0, + all other colums are on line 1 */ + case 2: + /* go on line 1 */ + strat_set_speed(2000, 700); + trajectory_d_a_rel(&mainboard.traj, -410, COLOR_A(-20)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + i2c_mechboard_mode_prepare_pickup_next(side, + next_mode); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + + err = goto_and_avoid_forward(COL10_X, COLOR_Y(400), + TRAJ_FLAGS_NO_NEAR, + TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + ERROUT(END_TRAJ); + break; + + /* configuration 3: 2 cols on line 0, + all other colums are on line 2 */ + case 3: + /* go on line 2 */ + strat_set_speed(2000, 700); + trajectory_d_a_rel(&mainboard.traj, -150, COLOR_A(-30)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + i2c_mechboard_mode_prepare_pickup_next(side, + next_mode); + + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + + trajectory_goto_forward_xy_abs(&mainboard.traj, + LINE2_X, + COLOR_Y(400)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + ERROUT(END_TRAJ); + break; + + /* configuration 4: 2 cols on line 0, + 2 on line 1, 2 on line 2 */ + case 4: + /* go on line 1 */ + strat_set_speed(600, 2000); + trajectory_d_a_rel(&mainboard.traj, -BIG_DIST, + COLOR_A(-135)); + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(900), + TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + i2c_mechboard_mode_prepare_pickup_next(side, + next_mode); + + strat_set_speed(2000, 2000); + trajectory_d_rel(&mainboard.traj, -BIG_DIST); + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1100), + TRAJ_FLAGS_STD); + if (TRAJ_SUCCESS(err)) /* we should not reach end */ + ERROUT(END_ERROR); + else if (err) + ERROUT(err); + + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + trajectory_d_a_rel(&mainboard.traj, -600, COLOR_A(40)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + ERROUT(err); + + DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); + err = goto_and_avoid_forward(LINE1_X, + COLOR_Y(400), + TRAJ_FLAGS_NO_NEAR, + TRAJ_FLAGS_NO_NEAR); + ERROUT(END_TRAJ); + break; + + default: + break; + } + + /* should not reach this point */ + ERROUT(END_ERROR); + + end: + strat_set_speed(old_spdd, old_spda); + return err; +} diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c new file mode 100644 index 0000000..81a12d6 --- /dev/null +++ b/projects/microb2010/mainboard/strat_utils.c @@ -0,0 +1,413 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_utils.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $ + * + */ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" + +#include "main.h" +#include "strat_utils.h" +#include "strat.h" +#include "sensor.h" +#include "i2c_protocol.h" + +/* return the distance between two points */ +int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2) +{ + int32_t x,y; + x = (x2-x1); + x = x*x; + y = (y2-y1); + y = y*y; + return sqrt(x+y); +} + +/* return the distance to a point in the area */ +int16_t distance_from_robot(int16_t x, int16_t y) +{ + return distance_between(position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), x, y); +} + +/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */ +int16_t simple_modulo_360(int16_t a) +{ + if (a < -180) { + a += 360; + } + else if (a > 180) { + a -= 360; + } + return a; +} + +/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ +double simple_modulo_2pi(double a) +{ + if (a < -M_PI) { + a += M_2PI; + } + else if (a > M_PI) { + a -= M_2PI; + } + return a; +} + +/* return the distance to a point in the area */ +int16_t angle_abs_to_rel(int16_t a_abs) +{ + return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos)); +} + +void rel_da_to_abs_xy(double d_rel, double a_rel_rad, + double *x_abs, double *y_abs) +{ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + *x_abs = x + d_rel*cos(a+a_rel_rad); + *y_abs = y + d_rel*sin(a+a_rel_rad); +} + +double norm(double x, double y) +{ + return sqrt(x*x + y*y); +} + +void rel_xy_to_abs_xy(double x_rel, double y_rel, + double *x_abs, double *y_abs) +{ + double d_rel, a_rel; + d_rel = norm(x_rel, y_rel); + a_rel = atan2(y_rel, x_rel); + rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs); +} + +/* return an angle between -pi and pi */ +void abs_xy_to_rel_da(double x_abs, double y_abs, + double *d_rel, double *a_rel_rad) +{ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + *a_rel_rad = atan2(y_abs - y, x_abs - x) - a; + if (*a_rel_rad < -M_PI) { + *a_rel_rad += M_2PI; + } + else if (*a_rel_rad > M_PI) { + *a_rel_rad -= M_2PI; + } + *d_rel = norm(x_abs-x, y_abs-y); +} + +void rotate(double *x, double *y, double rot) +{ + double l, a; + + l = norm(*x, *y); + a = atan2(*y, *x); + + a += rot; + *x = l * cos(a); + *y = l * sin(a); +} + +/* return true if the point is in area */ +uint8_t is_in_area(int16_t x, int16_t y, int16_t margin) +{ + if (x < margin) + return 0; + if (x > (AREA_X - margin)) + return 0; + if (y < margin) + return 0; + if (y > (AREA_Y - margin)) + return 0; + return 1; +} + + +/* return true if the point is in area */ +uint8_t robot_is_in_area(int16_t margin) +{ + return is_in_area(position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + margin); +} + +/* return true if we are near the disc */ +uint8_t robot_is_near_disc(void) +{ + if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG) + return 1; + return 0; +} + +/* return 1 or 0 depending on which side of a line (y=cste) is the + * robot. works in red or green color. */ +uint8_t y_is_more_than(int16_t y) +{ + int16_t posy; + + posy = position_get_y_s16(&mainboard.pos); + if (mainboard.our_color == I2C_COLOR_RED) { + if (posy > y) + return 1; + else + return 0; + } + else { + if (posy < (AREA_Y-y)) + return 1; + else + return 0; + } +} + +/* return 1 or 0 depending on which side of a line (x=cste) is the + * robot. works in red or green color. */ +uint8_t x_is_more_than(int16_t x) +{ + int16_t posx; + + posx = position_get_x_s16(&mainboard.pos); + if (posx > x) + return 1; + else + return 0; +} + +int16_t sin_table[] = { + 0, + 3211, + 6392, + 9512, + 12539, + 15446, + 18204, + 20787, + 23170, + 25330, + 27245, + 28898, + 30273, + 31357, + 32138, + 32610, + 32767, +}; + +int16_t fast_sin(int16_t deg) +{ + deg %= 360; + + if (deg < 0) + deg += 360; + + if (deg < 90) + return sin_table[(deg*16)/90]; + else if (deg < 180) + return sin_table[((180-deg)*16)/90]; + else if (deg < 270) + return -sin_table[((deg-180)*16)/90]; + else + return -sin_table[((360-deg)*16)/90]; +} + +int16_t fast_cos(int16_t deg) +{ + return fast_sin(90+deg); +} + + +/* get the color of our robot */ +uint8_t get_color(void) +{ + return mainboard.our_color; +} + +/* get the color of the opponent robot */ +uint8_t get_opponent_color(void) +{ + if (mainboard.our_color == I2C_COLOR_RED) + return I2C_COLOR_GREEN; + else + return I2C_COLOR_RED; +} + +/* get the xy pos of the opponent robot */ +int8_t get_opponent_xy(int16_t *x, int16_t *y) +{ + uint8_t flags; + IRQ_LOCK(flags); + *x = sensorboard.opponent_x; + *y = sensorboard.opponent_y; + IRQ_UNLOCK(flags); + if (*x == I2C_OPPONENT_NOT_THERE) + return -1; + return 0; +} + +/* get the da pos of the opponent robot */ +int8_t get_opponent_da(int16_t *d, int16_t *a) +{ + uint8_t flags; + int16_t x_tmp; + IRQ_LOCK(flags); + x_tmp = sensorboard.opponent_x; + *d = sensorboard.opponent_d; + *a = sensorboard.opponent_a; + IRQ_UNLOCK(flags); + if (x_tmp == I2C_OPPONENT_NOT_THERE) + return -1; + return 0; +} + +/* get the da pos of the opponent robot */ +int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a) +{ + uint8_t flags; + IRQ_LOCK(flags); + *x = sensorboard.opponent_x; + *y = sensorboard.opponent_y; + *d = sensorboard.opponent_d; + *a = sensorboard.opponent_a; + IRQ_UNLOCK(flags); + if (*x == I2C_OPPONENT_NOT_THERE) + return -1; + return 0; +} + +uint8_t pump_left1_is_full(void) +{ + return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) && + (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN)); +} + +uint8_t pump_left2_is_full(void) +{ + return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) && + (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN)); +} + +uint8_t pump_right1_is_full(void) +{ + return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) && + (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN)); +} + +uint8_t pump_right2_is_full(void) +{ + return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) && + (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN)); +} + +/* number of column owned by the robot */ +uint8_t get_column_count_left(void) +{ + uint8_t ret = 0; + ret += pump_left1_is_full(); + ret += pump_left2_is_full(); + return ret; +} + +/* number of column owned by the robot */ +uint8_t get_column_count_right(void) +{ + uint8_t ret = 0; + ret += pump_right1_is_full(); + ret += pump_right2_is_full(); + return ret; +} + +/* number of column owned by the robot */ +uint8_t get_column_count(void) +{ + uint8_t ret = 0; + ret += pump_left1_is_full(); + ret += pump_left2_is_full(); + ret += pump_right1_is_full(); + ret += pump_right2_is_full(); + return ret; +} + +uint8_t get_lintel_count(void) +{ + return mechboard.lintel_count; +} + +uint8_t get_mechboard_mode(void) +{ + return mechboard.mode; +} + +uint8_t get_scanner_status(void) +{ + return sensorboard.scan_status; +} + +/* return 0 if timeout, or 1 if cond is true */ +uint8_t wait_scan_done(uint16_t timeout) +{ + uint8_t err; + err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); + return err; +} + +uint8_t opponent_is_behind(void) +{ + int8_t opp_there; + int16_t opp_d, opp_a; + + opp_there = get_opponent_da(&opp_d, &opp_a); + if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) + return 1; + return 0; +} diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h new file mode 100644 index 0000000..986b601 --- /dev/null +++ b/projects/microb2010/mainboard/strat_utils.h @@ -0,0 +1,76 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_utils.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + + +#define DEG(x) (((double)(x)) * (180.0 / M_PI)) +#define RAD(x) (((double)(x)) * (M_PI / 180.0)) +#define M_2PI (2*M_PI) + +struct xy_point { + int16_t x; + int16_t y; +}; + +/* wait traj end flag or cond. return 0 if cond become true, else + * return the traj flag */ +#define WAIT_COND_OR_TRAJ_END(cond, mask) \ + ({ \ + uint8_t __err = 0; \ + while ( (! (cond)) && (__err == 0)) { \ + __err = test_traj_end(TRAJ_FLAGS_NO_NEAR); \ + } \ + __err; \ + }) \ + +int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2); +int16_t distance_from_robot(int16_t x, int16_t y); +int16_t simple_modulo_360(int16_t a); +double simple_modulo_2pi(double a); +int16_t angle_abs_to_rel(int16_t a_abs); +void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs); +double norm(double x, double y); +void rel_xy_to_abs_xy(double x_rel, double y_rel, double *x_abs, double *y_abs); +void abs_xy_to_rel_da(double x_abs, double y_abs, double *d_rel, double *a_rel_rad); +void rotate(double *x, double *y, double rot); +uint8_t is_in_area(int16_t x, int16_t y, int16_t margin); +uint8_t robot_is_in_area(int16_t margin); +uint8_t robot_is_near_disc(void); +uint8_t y_is_more_than(int16_t y); +uint8_t x_is_more_than(int16_t x); +int16_t fast_sin(int16_t deg); +int16_t fast_cos(int16_t deg); +uint8_t get_color(void); +uint8_t get_opponent_color(void); +int8_t get_opponent_xy(int16_t *x, int16_t *y); +int8_t get_opponent_da(int16_t *d, int16_t *a); +int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a); +uint8_t get_column_count(void); +uint8_t get_column_count_right(void); +uint8_t get_column_count_left(void); +uint8_t get_lintel_count(void); +uint8_t get_mechboard_mode(void); +uint8_t pump_left1_is_full(void); +uint8_t pump_left2_is_full(void); +uint8_t pump_right1_is_full(void); +uint8_t pump_right2_is_full(void); +uint8_t get_scanner_status(void); +uint8_t wait_scan_done(uint16_t timeout); +uint8_t opponent_is_behind(void); diff --git a/projects/microb2010/mainboard/time_config.h b/projects/microb2010/mainboard/time_config.h new file mode 100755 index 0000000..14db608 --- /dev/null +++ b/projects/microb2010/mainboard/time_config.h @@ -0,0 +1,23 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: time_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $ + * + */ + +/** precision of the time processor, in us */ +#define TIME_PRECISION 25000l diff --git a/projects/microb2010/mainboard/timer_config.h b/projects/microb2010/mainboard/timer_config.h new file mode 100755 index 0000000..47d9f18 --- /dev/null +++ b/projects/microb2010/mainboard/timer_config.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2006) + * + * 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: timer_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $ + * + */ + +#define TIMER0_ENABLED + +/* #define TIMER1_ENABLED */ +/* #define TIMER1A_ENABLED */ +/* #define TIMER1B_ENABLED */ +/* #define TIMER1C_ENABLED */ + +/* #define TIMER2_ENABLED */ + +/* #define TIMER3_ENABLED */ +/* #define TIMER3A_ENABLED */ +/* #define TIMER3B_ENABLED */ +/* #define TIMER3C_ENABLED */ + +#define TIMER0_PRESCALER_DIV 8 diff --git a/projects/microb2010/mainboard/uart_config.h b/projects/microb2010/mainboard/uart_config.h new file mode 100644 index 0000000..514d1e9 --- /dev/null +++ b/projects/microb2010/mainboard/uart_config.h @@ -0,0 +1,94 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: uart_config.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* + * UART1 definitions + */ + +/* compile uart1 fonctions, undefine it to pass compilation */ +#define UART1_COMPILE + +/* enable uart1 if == 1, disable if == 0 */ +#define UART1_ENABLED 1 + +/* enable uart1 interrupts if == 1, disable if == 0 */ +#define UART1_INTERRUPT_ENABLED 1 + +#define UART1_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART1_USE_DOUBLE_SPEED 1 + +#define UART1_RX_FIFO_SIZE 64 +#define UART1_TX_FIFO_SIZE 127 +#define UART1_NBITS 8 + +#define UART1_PARITY UART_PARTITY_NONE + +#define UART1_STOP_BIT UART_STOP_BITS_1 + + +/* + * UART3 definitions + */ + +/* compile uart3 fonctions, undefine it to pass compilation */ +#define UART3_COMPILE + +/* enable uart3 if == 1, disable if == 0 */ +#define UART3_ENABLED 1 + +/* enable uart3 interrupts if == 1, disable if == 0 */ +#define UART3_INTERRUPT_ENABLED 1 + +#define UART3_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART3_USE_DOUBLE_SPEED 1 + +#define UART3_RX_FIFO_SIZE 64 +#define UART3_TX_FIFO_SIZE 64 +#define UART3_NBITS 8 + +#define UART3_PARITY UART_PARTITY_NONE + +#define UART3_STOP_BIT UART_STOP_BITS_1 + + + + +/* .... same for uart 1, 2, 3 ... */ + +#endif + diff --git a/projects/microb2010/mechboard/.config b/projects/microb2010/mechboard/.config new file mode 100644 index 0000000..fd14974 --- /dev/null +++ b/projects/microb2010/mechboard/.config @@ -0,0 +1,279 @@ +# +# Automatically generated make config: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +# CONFIG_MCU_ATMEGA8 is not set +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +# CONFIG_MCU_ATMEGA128 is not set +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +CONFIG_MCU_ATMEGA2560=y +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +CONFIG_MATH_LIB=y +# CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set +# CONFIG_MINIMAL_PRINTF is not set +# CONFIG_STANDARD_PRINTF is not set +CONFIG_ADVANCED_PRINTF=y +# CONFIG_FORMAT_IHEX is not set +# CONFIG_FORMAT_SREC is not set +CONFIG_FORMAT_BINARY=y + +# +# Base modules +# + +# +# Enable math library in generation options to see all modules +# +CONFIG_MODULE_CIRBUF=y +# CONFIG_MODULE_CIRBUF_LARGE is not set +# CONFIG_MODULE_FIXED_POINT is not set +# CONFIG_MODULE_VECT2 is not set +# CONFIG_MODULE_GEOMETRY is not set +# CONFIG_MODULE_GEOMETRY_CREATE_CONFIG is not set +CONFIG_MODULE_SCHEDULER=y +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +# CONFIG_MODULE_SCHEDULER_TIMER0 is not set +CONFIG_MODULE_SCHEDULER_MANUAL=y +CONFIG_MODULE_TIME=y +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# + +# +# uart needs circular buffer, mf2 client may need scheduler +# +CONFIG_MODULE_UART=y +# CONFIG_MODULE_UART_9BITS is not set +CONFIG_MODULE_UART_CREATE_CONFIG=y +CONFIG_MODULE_SPI=y +CONFIG_MODULE_SPI_CREATE_CONFIG=y +CONFIG_MODULE_I2C=y +CONFIG_MODULE_I2C_MASTER=y +# CONFIG_MODULE_I2C_MULTIMASTER is not set +CONFIG_MODULE_I2C_CREATE_CONFIG=y +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +CONFIG_MODULE_TIMER=y +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +CONFIG_MODULE_PWM_NG=y +CONFIG_MODULE_ADC=y +CONFIG_MODULE_ADC_CREATE_CONFIG=y + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +CONFIG_MODULE_VT100=y +CONFIG_MODULE_RDLINE=y +CONFIG_MODULE_RDLINE_CREATE_CONFIG=y +CONFIG_MODULE_RDLINE_KILL_BUF=y +CONFIG_MODULE_RDLINE_HISTORY=y +CONFIG_MODULE_PARSE=y +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +CONFIG_MODULE_AX12=y +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +# CONFIG_MODULE_ENCODERS_MICROB is not set +# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +CONFIG_MODULE_ENCODERS_SPI=y +CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y + +# +# Robot specific modules +# +# CONFIG_MODULE_ROBOT_SYSTEM is not set +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +# CONFIG_MODULE_POSITION_MANAGER is not set +# CONFIG_MODULE_TRAJECTORY_MANAGER is not set +CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y +# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set + +# +# Control system modules +# +CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y + +# +# Filters +# +CONFIG_MODULE_PID=y +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +CONFIG_MODULE_QUADRAMP=y +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# + +# +# Some radio devices require SPI to be activated +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# + +# +# Crypto modules depend on utils module +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# + +# +# Encoding modules depend on utils module +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# + +# +# Debug modules depend on utils module +# +CONFIG_MODULE_DIAGNOSTIC=y +CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y +CONFIG_MODULE_ERROR=y +CONFIG_MODULE_ERROR_CREATE_CONFIG=y + +# +# Programmer options +# +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/mechboard/Makefile b/projects/microb2010/mechboard/Makefile new file mode 100644 index 0000000..34efb9f --- /dev/null +++ b/projects/microb2010/mechboard/Makefile @@ -0,0 +1,35 @@ +TARGET = main + +# repertoire des modules +AVERSIVE_DIR = ../../.. +# VALUE, absolute or relative path : example ../.. # + +CFLAGS += -Werror -Wextra +LDFLAGS = -T ../common/avr6.x + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c +SRC += commands_cs.c commands_mechboard.c commands.c +SRC += i2c_protocol.c sensor.c actuator.c cs.c +SRC += arm_xy.c state.c ax12_user.c arm_highlevel.c + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + +######################################## + +-include .aversive_conf +include $(AVERSIVE_DIR)/mk/aversive_project.mk + +AVRDUDE_DELAY=50 + +program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep + echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + diff --git a/projects/microb2010/mechboard/actuator.c b/projects/microb2010/mechboard/actuator.c new file mode 100644 index 0000000..a36a70c --- /dev/null +++ b/projects/microb2010/mechboard/actuator.c @@ -0,0 +1,163 @@ +/* + * 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 + * (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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "../common/i2c_commands.h" +#include "actuator.h" +#include "ax12_user.h" +#include "main.h" + +#define FINGER_DEBUG(args...) DEBUG(E_USER_FINGER, args) +#define FINGER_NOTICE(args...) NOTICE(E_USER_FINGER, args) +#define FINGER_ERROR(args...) ERROR(E_USER_FINGER, args) + +struct finger { + int8_t event; + uint16_t destination; +}; + +struct finger finger; + +static void finger_goto_cb(void *); + +/* schedule a single event for the finger */ +static void finger_schedule_event(struct finger *finger) +{ + uint8_t flags; + int8_t ret; + + IRQ_LOCK(flags); + ret = scheduler_add_event(SCHEDULER_SINGLE, + (void *)finger_goto_cb, + finger, 1, ARM_PRIO); + if (ret == -1) { + IRQ_UNLOCK(flags); + FINGER_ERROR("Cannot load finger event"); + return; + } + finger->event = ret; + IRQ_UNLOCK(flags); +} + +static void finger_goto_cb(void *data) +{ + uint8_t flags; + struct finger *finger = data; + uint16_t position; + + IRQ_LOCK(flags); + finger->event = -1; + position = finger->destination; + IRQ_UNLOCK(flags); + FINGER_DEBUG("goto_cb %d", position); + ax12_user_write_int(&gen.ax12, FINGER_AX12, + AA_GOAL_POSITION_L, position); +} + +/* load an event that will move the ax12 for us */ +void finger_goto(uint16_t position) +{ + uint8_t flags; + FINGER_NOTICE("goto %d", position); + + IRQ_LOCK(flags); + finger.destination = position; + if (finger.event != -1) { + IRQ_UNLOCK(flags); + return; /* nothing to do, event already scheduled */ + } + IRQ_UNLOCK(flags); + finger_schedule_event(&finger); +} + +static void finger_init(void) +{ + finger.event = -1; + finger.destination = 0; + /* XXX set pos ? */ +} + +uint16_t finger_get_goal_pos(void) +{ + return finger.destination; +} + +uint8_t finger_get_side(void) +{ + if (finger.destination <= FINGER_CENTER) + return I2C_LEFT_SIDE; + return I2C_RIGHT_SIDE; +} + +/**********/ + +#define SERVO_LEFT_OUT 400 +#define SERVO_LEFT_1LIN 520 +#define SERVO_LEFT_2LIN 485 +#define SERVO_RIGHT_OUT 290 +#define SERVO_RIGHT_1LIN 155 +#define SERVO_RIGHT_2LIN 180 + +void servo_lintel_out(void) +{ + mechboard.servo_lintel_left = SERVO_LEFT_OUT; + mechboard.servo_lintel_right = SERVO_RIGHT_OUT; +} + +void servo_lintel_1lin(void) +{ + mechboard.servo_lintel_left = SERVO_LEFT_1LIN; + mechboard.servo_lintel_right = SERVO_RIGHT_1LIN; +} + +void servo_lintel_2lin(void) +{ + mechboard.servo_lintel_left = SERVO_LEFT_2LIN; + mechboard.servo_lintel_right = SERVO_RIGHT_2LIN; +} + +/**********/ + +void actuator_init(void) +{ + finger_init(); + servo_lintel_out(); +} diff --git a/projects/microb2010/mechboard/actuator.h b/projects/microb2010/mechboard/actuator.h new file mode 100644 index 0000000..c55f564 --- /dev/null +++ b/projects/microb2010/mechboard/actuator.h @@ -0,0 +1,39 @@ +/* + * 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 + * (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.h,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#define FINGER_RIGHT 666 +#define FINGER_RIGHT_RELAX 621 +#define FINGER_CENTER_RIGHT 491 +#define FINGER_CENTER 490 +#define FINGER_CENTER_LEFT 489 +#define FINGER_LEFT 340 +#define FINGER_LEFT_RELAX 385 + +void finger_goto(uint16_t position); +uint16_t finger_get_goal_pos(void); +uint8_t finger_get_side(void); + +void servo_lintel_out(void); +void servo_lintel_1lin(void); +void servo_lintel_2lin(void); + +void actuator_init(void); + diff --git a/projects/microb2010/mechboard/adc_config.h b/projects/microb2010/mechboard/adc_config.h new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/mechboard/arm_highlevel.c b/projects/microb2010/mechboard/arm_highlevel.c new file mode 100644 index 0000000..abda4d4 --- /dev/null +++ b/projects/microb2010/mechboard/arm_highlevel.c @@ -0,0 +1,644 @@ +/* + * 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 + * (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: arm_highlevel.c,v 1.4 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "arm_xy.h" +#include "arm_highlevel.h" + +#define WRIST_ANGLE_PUMP1 -2 /* in degree */ +#define WRIST_ANGLE_PUMP2 103 /* in degree */ + +struct arm *arm_num2ptr(uint8_t arm_num) +{ + switch (arm_num) { + case ARM_LEFT_NUM: + return &left_arm; + case ARM_RIGHT_NUM: + return &right_arm; + default: + return NULL; + } +} + +#define ARM_MAX_H 110 +#define ARM_STRAIGHT_D 254 +#define ARM_STRAIGHT_H 0 + +void arm_goto_straight(uint8_t arm_num, uint8_t pump_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + arm_do_xy(arm, ARM_STRAIGHT_D, ARM_STRAIGHT_H, angle); +} + +/* position to get a column */ +#define ARM_GET_D 60 +#define ARM_GET_H -140 + +void arm_goto_get_column(uint8_t arm_num, uint8_t pump_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + arm_do_xy(arm, ARM_GET_D, ARM_GET_H, angle); +} + +/* position to get a column */ +#define ARM_PREPARE_D 62 +#define ARM_PREPARE_H -133 + +void arm_goto_prepare_get(uint8_t arm_num, uint8_t pump_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + arm_do_xy(arm, ARM_PREPARE_D, ARM_PREPARE_H, angle); +} + +#define ARM_INTERMEDIATE_D (65) +#define ARM_INTERMEDIATE_H (-115) + +void arm_goto_intermediate_get(uint8_t arm_num, uint8_t pump_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + arm_do_xy(arm, ARM_INTERMEDIATE_D, ARM_INTERMEDIATE_H, angle); +} + +/* used in prepare pickup */ +#define ARM_INTERMEDIATE_FRONT_D (90) +#define ARM_INTERMEDIATE_FRONT_H (-105) + +void arm_goto_intermediate_front_get(uint8_t arm_num, uint8_t pump_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + arm_do_xy(arm, ARM_INTERMEDIATE_FRONT_D, + ARM_INTERMEDIATE_FRONT_H, angle); +} + +/* ****** */ + +#define ARM_PREPARE_EJECT_D (70) +#define ARM_PREPARE_EJECT_H (-50) + +void arm_goto_prepare_eject(uint8_t arm_num, uint8_t pump_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + arm_do_xy(arm, ARM_PREPARE_EJECT_D, ARM_PREPARE_EJECT_H, angle); +} + +#define ARM_EJECT_D (200) +#define ARM_EJECT_H (30) + +void arm_goto_eject(uint8_t arm_num, uint8_t pump_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + arm_do_xy(arm, ARM_EJECT_D, ARM_EJECT_H, angle); +} + +/* ****** */ + +#define ARM_PREPARE_GET_LINTEL_INSIDE1_D 90 +#define ARM_PREPARE_GET_LINTEL_INSIDE1_H -75 +#define ARM_PREPARE_GET_LINTEL_INSIDE1_A -30 +void arm_goto_prepare_get_lintel_inside1(void) +{ + arm_do_xy(&left_arm, ARM_PREPARE_GET_LINTEL_INSIDE1_D, + ARM_PREPARE_GET_LINTEL_INSIDE1_H, + ARM_PREPARE_GET_LINTEL_INSIDE1_A); + arm_do_xy(&right_arm, ARM_PREPARE_GET_LINTEL_INSIDE1_D, + ARM_PREPARE_GET_LINTEL_INSIDE1_H, + ARM_PREPARE_GET_LINTEL_INSIDE1_A); +} + +#define ARM_PREPARE_GET_LINTEL_INSIDE2_D 30 +#define ARM_PREPARE_GET_LINTEL_INSIDE2_H -75 +#define ARM_PREPARE_GET_LINTEL_INSIDE2_A -30 +void arm_goto_prepare_get_lintel_inside2(uint8_t lintel_count) +{ + uint16_t d; + d = ARM_PREPARE_GET_LINTEL_INSIDE2_D; + if (lintel_count == 2) + d += 34; + arm_do_xy(&left_arm, d, + ARM_PREPARE_GET_LINTEL_INSIDE2_H, + ARM_PREPARE_GET_LINTEL_INSIDE2_A); + arm_do_xy(&right_arm, d, + ARM_PREPARE_GET_LINTEL_INSIDE2_H, + ARM_PREPARE_GET_LINTEL_INSIDE2_A); +} + +#define ARM_GET_LINTEL_INSIDE_D 10 +#define ARM_GET_LINTEL_INSIDE_H -75 +#define ARM_GET_LINTEL_INSIDE_A -30 +void arm_goto_get_lintel_inside(uint8_t lintel_count) +{ + uint16_t d; + d = ARM_GET_LINTEL_INSIDE_D; + if (lintel_count == 2) + d += 34; + arm_do_xy(&left_arm, d, + ARM_GET_LINTEL_INSIDE_H, + ARM_GET_LINTEL_INSIDE_A); + arm_do_xy(&right_arm, d, + ARM_GET_LINTEL_INSIDE_H, + ARM_GET_LINTEL_INSIDE_A); +} + +#define ARM_PREPARE_BUILD_LINTEL1_D 30 +#define ARM_PREPARE_BUILD_LINTEL1_H -50 +#define ARM_PREPARE_BUILD_LINTEL1_A -30 +void arm_goto_prepare_build_lintel1(void) +{ + arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL1_D, + ARM_PREPARE_BUILD_LINTEL1_H, + ARM_PREPARE_BUILD_LINTEL1_A); + arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL1_D, + ARM_PREPARE_BUILD_LINTEL1_H, + ARM_PREPARE_BUILD_LINTEL1_A); +} + +#define ARM_PREPARE_BUILD_LINTEL2_D 80 +#define ARM_PREPARE_BUILD_LINTEL2_H -110 +#define ARM_PREPARE_BUILD_LINTEL2_A 60 +void arm_goto_prepare_build_lintel2(uint8_t level) +{ + int16_t h; + if (level < 3) + level = 3; + h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL2_H; + if (h > ARM_MAX_H) + h = ARM_MAX_H; + arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL2_D, + h, ARM_PREPARE_BUILD_LINTEL2_A); + arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL2_D, + h, ARM_PREPARE_BUILD_LINTEL2_A); +} + +#define ARM_PREPARE_BUILD_LINTEL3_D 205 +#define ARM_PREPARE_BUILD_LINTEL3_H -100 +#define ARM_PREPARE_BUILD_LINTEL3_A 50 +void arm_goto_prepare_build_lintel3(uint8_t level) +{ + int16_t h; + if (level < 2) + level = 2; + h = (int16_t)level * 30 + ARM_PREPARE_BUILD_LINTEL3_H; + if (h > ARM_MAX_H) + h = ARM_MAX_H; + arm_do_xy(&left_arm, ARM_PREPARE_BUILD_LINTEL3_D, + h, ARM_PREPARE_BUILD_LINTEL3_A); + arm_do_xy(&right_arm, ARM_PREPARE_BUILD_LINTEL3_D, + h, ARM_PREPARE_BUILD_LINTEL3_A); +} + +#define ARM_BUILD_LINTEL_D 205 +#define ARM_BUILD_LINTEL_H -128 +#define ARM_BUILD_LINTEL_A 50 +void arm_goto_build_lintel(uint8_t level) +{ + int16_t h; + h = (int16_t)level * 30 + ARM_BUILD_LINTEL_H; + if (h > ARM_MAX_H) + h = ARM_MAX_H; + arm_do_xy(&left_arm, ARM_BUILD_LINTEL_D, + h, ARM_BUILD_LINTEL_A); + arm_do_xy(&right_arm, ARM_BUILD_LINTEL_D, + h, ARM_BUILD_LINTEL_A); +} + +/* ****** */ + +#define ARM_PREPARE_GET_LINTEL_DISP_D 190 +#define ARM_PREPARE_GET_LINTEL_DISP_H -40 +#define ARM_PREPARE_GET_LINTEL_DISP_A 50 +void arm_goto_prepare_get_lintel_disp(void) +{ + arm_do_xy(&left_arm, ARM_PREPARE_GET_LINTEL_DISP_D, + ARM_PREPARE_GET_LINTEL_DISP_H, + ARM_PREPARE_GET_LINTEL_DISP_A); + arm_do_xy(&right_arm, ARM_PREPARE_GET_LINTEL_DISP_D, + ARM_PREPARE_GET_LINTEL_DISP_H, + ARM_PREPARE_GET_LINTEL_DISP_A); +} + +#define ARM_GET_LINTEL_DISP_D 190 +#define ARM_GET_LINTEL_DISP_H -70 +#define ARM_GET_LINTEL_DISP_A 40 +void arm_goto_get_lintel_disp(void) +{ + arm_do_xy(&left_arm, ARM_GET_LINTEL_DISP_D, + ARM_GET_LINTEL_DISP_H, + ARM_GET_LINTEL_DISP_A); + arm_do_xy(&right_arm, ARM_GET_LINTEL_DISP_D, + ARM_GET_LINTEL_DISP_H, + ARM_GET_LINTEL_DISP_A); +} + +#define ARM_PREPARE_PUT_LINTEL_DISP_D 130 +#define ARM_PREPARE_PUT_LINTEL_DISP_H 0 +#define ARM_PREPARE_PUT_LINTEL_DISP_A 0 +void arm_goto_prepare_put_lintel(void) +{ + arm_do_xy(&left_arm, ARM_PREPARE_PUT_LINTEL_DISP_D, + ARM_PREPARE_PUT_LINTEL_DISP_H, + ARM_PREPARE_PUT_LINTEL_DISP_A); + arm_do_xy(&right_arm, ARM_PREPARE_PUT_LINTEL_DISP_D, + ARM_PREPARE_PUT_LINTEL_DISP_H, + ARM_PREPARE_PUT_LINTEL_DISP_A); +} + +#define ARM_PUT_LINTEL_DISP_D 30 +#define ARM_PUT_LINTEL_DISP_H -60 +#define ARM_PUT_LINTEL_DISP_A -30 +void arm_goto_put_lintel(uint8_t lintel_count) +{ + arm_do_xy(&left_arm, + ARM_PUT_LINTEL_DISP_D + lintel_count * 30, + ARM_PUT_LINTEL_DISP_H, + ARM_PUT_LINTEL_DISP_A); + arm_do_xy(&right_arm, + ARM_PUT_LINTEL_DISP_D + lintel_count * 30, + ARM_PUT_LINTEL_DISP_H, + ARM_PUT_LINTEL_DISP_A); +} + +/* ****** */ + + +#define ARM_LOADED_D 100 +#define ARM_LOADED_H 0 + +void arm_goto_loaded(uint8_t arm_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + arm_do_xy(arm, ARM_LOADED_D, ARM_LOADED_H, WRIST_ANGLE_PUMP2); +} + + +/* for columns */ +#define ARM_PREPARE_BUILD_INSIDE_D 90 +#define ARM_PREPARE_BUILD_INSIDE_H -45 + +void arm_goto_prepare_build_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + int16_t h; + if (level < 2) + level = 2; + h = (int16_t)level * 30 + ARM_PREPARE_BUILD_INSIDE_H; + if (h > ARM_MAX_H) + h = ARM_MAX_H; + arm_do_xy(arm, ARM_PREPARE_BUILD_INSIDE_D, h, angle); +} + +#define ARM_PREPARE_AUTOBUILD_INSIDE_D 90 +#define ARM_PREPARE_AUTOBUILD_INSIDE_H -70 + +void arm_goto_prepare_autobuild_inside(uint8_t arm_num, uint8_t pump_num, uint8_t level) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + int16_t h; + if (level < 2) + level = 2; + h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_INSIDE_H; + if (h > ARM_MAX_H) + h = ARM_MAX_H; + arm_do_xy(arm, ARM_PREPARE_AUTOBUILD_INSIDE_D, h, angle); +} + +#define ARM_PREPARE_AUTOBUILD_OUTSIDE_D 210 /* not used, see dist below */ +#define ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1 (-110) +#define ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2 (-90) + +void arm_goto_prepare_autobuild_outside(uint8_t arm_num, uint8_t pump_num, + uint8_t level, uint8_t dist) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + int16_t h; + if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM) + h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P1; + else + h = (int16_t)level * 30 + ARM_PREPARE_AUTOBUILD_OUTSIDE_H_P2; + if (h > ARM_MAX_H) + h = ARM_MAX_H; + arm_do_xy(arm, dist, h, angle); +} + +#define ARM_AUTOBUILD_D_P1 208 /* not used, see dist below */ +#define ARM_AUTOBUILD_D_P2 210 /* not used, see dist below */ +#define ARM_AUTOBUILD_D_P1_OFFSET (-2) +#define ARM_AUTOBUILD_D_P2_OFFSET (0) +#define ARM_AUTOBUILD_H_P1 (-133) +#define ARM_AUTOBUILD_H_P2 (-130) + +void arm_goto_autobuild(uint8_t arm_num, uint8_t pump_num, + uint8_t level, uint8_t dist) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t angle = pump_num2angle(pump_num); + int16_t h; + if (pump_num == PUMP_LEFT1_NUM || pump_num == PUMP_RIGHT1_NUM) { + h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P1; + if (h > ARM_MAX_H) + h = ARM_MAX_H; + arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P1_OFFSET, h, angle); + } + else { + h = (int16_t)level * 30 + ARM_AUTOBUILD_H_P2; + if (h > ARM_MAX_H) + h = ARM_MAX_H; + arm_do_xy(arm, dist + ARM_AUTOBUILD_D_P2_OFFSET, h, angle); + } +} + +#define ARM_PUSH_TEMPLE_D 170 +#define ARM_PUSH_TEMPLE_H -165 + +void arm_goto_push_temple(uint8_t arm_num, uint8_t level) +{ + struct arm *arm = arm_num2ptr(arm_num); + int16_t h = ARM_PUSH_TEMPLE_H; + + /* level can be 0 or 1 */ + if (level) + h += 30; + arm_do_xy(arm, ARM_PUSH_TEMPLE_D, h, WRIST_ANGLE_PUMP1); +} + +#define ARM_PREPARE_PUSH_TEMPLE_D 120 +#define ARM_PREPARE_PUSH_TEMPLE_H -60 + +void arm_goto_prepare_push_temple(uint8_t arm_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + arm_do_xy(arm, ARM_PREPARE_PUSH_TEMPLE_D, + ARM_PREPARE_PUSH_TEMPLE_H, WRIST_ANGLE_PUMP1); +} + +#define ARM_PUSH_TEMPLE_DISC_D1 215 +#define ARM_PUSH_TEMPLE_DISC_H1 -100 +#define ARM_PUSH_TEMPLE_DISC_D2 190 +#define ARM_PUSH_TEMPLE_DISC_H2 -65 + +void arm_goto_push_temple_disc(uint8_t arm_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + int8_t pump_num; + + pump_num = arm_get_busy_pump(arm_num); + if (pump_num == -1) + arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D1, + ARM_PUSH_TEMPLE_DISC_H1, WRIST_ANGLE_PUMP1); + else + arm_do_xy(arm, ARM_PUSH_TEMPLE_DISC_D2, + ARM_PUSH_TEMPLE_DISC_H2, WRIST_ANGLE_PUMP1); +} + +#define ARM_PREPARE_PUSH_TEMPLE_DISC_D 100 +#define ARM_PREPARE_PUSH_TEMPLE_DISC_H -80 + +void arm_goto_prepare_push_temple_disc(uint8_t arm_num) +{ + struct arm *arm = arm_num2ptr(arm_num); + arm_do_xy(arm, ARM_PREPARE_PUSH_TEMPLE_DISC_D, + ARM_PREPARE_PUSH_TEMPLE_DISC_H, WRIST_ANGLE_PUMP1); +} + +void arm_prepare_free_pumps(void) +{ + int8_t pump_num; + + pump_num = arm_get_free_pump(ARM_LEFT_NUM); + if (pump_num != -1) + arm_goto_prepare_get(ARM_LEFT_NUM, pump_num); + pump_num = arm_get_free_pump(ARM_RIGHT_NUM); + if (pump_num != -1) + arm_goto_prepare_get(ARM_RIGHT_NUM, pump_num); +} + + +/* return the id of a free pump on this arm */ +int8_t arm_get_free_pump(uint8_t arm_num) +{ + switch (arm_num) { + case ARM_LEFT_NUM: + if (pump_is_free(PUMP_LEFT1_NUM) && + pump_is_free(PUMP_LEFT2_NUM)) + return PUMP_LEFT1_NUM; + else if (pump_is_free(PUMP_LEFT2_NUM)) + return PUMP_LEFT2_NUM; + return -1; + case ARM_RIGHT_NUM: + if (pump_is_free(PUMP_RIGHT1_NUM) && + pump_is_free(PUMP_RIGHT2_NUM)) + return PUMP_RIGHT1_NUM; + else if (pump_is_free(PUMP_RIGHT2_NUM)) + return PUMP_RIGHT2_NUM; + return -1; + default: + return -1; + } +} + +/* return the id of a busy pump on this arm */ +int8_t arm_get_busy_pump(uint8_t arm_num) +{ + switch (arm_num) { + case ARM_LEFT_NUM: + if (pump_is_busy(PUMP_LEFT2_NUM)) + return PUMP_LEFT2_NUM; + else if (pump_is_busy(PUMP_LEFT1_NUM)) + return PUMP_LEFT1_NUM; + return -1; + case ARM_RIGHT_NUM: + if (pump_is_busy(PUMP_RIGHT2_NUM)) + return PUMP_RIGHT2_NUM; + else if (pump_is_busy(PUMP_RIGHT1_NUM)) + return PUMP_RIGHT1_NUM; + return -1; + default: + return -1; + } +} + +uint8_t arm_wait_both(uint8_t mask) +{ + uint8_t ret; + ret = arm_wait_traj_end(&left_arm, mask); + if (ret != ARM_TRAJ_END && ret != ARM_TRAJ_NEAR) + return ret; + return arm_wait_traj_end(&right_arm, mask); +} + +uint8_t arm_wait_select(uint8_t left, uint8_t right, uint8_t mask) +{ + if (left && right) + return arm_wait_both(mask); + if (left) + return arm_wait_traj_end(&left_arm, mask); + if (right) + return arm_wait_traj_end(&right_arm, mask); + return ARM_TRAJ_END; +} + +/*********************/ + +int16_t *pump_num2ptr(uint8_t pump_num) +{ + switch (pump_num) { + case PUMP_LEFT1_NUM: + return &mechboard.pump_left1; + case PUMP_RIGHT1_NUM: + return &mechboard.pump_right1; + case PUMP_LEFT2_NUM: + return &mechboard.pump_left2; + case PUMP_RIGHT2_NUM: + return &mechboard.pump_right2; + default: + return NULL; + } +} + +void pump_set(uint8_t pump_num, int16_t val) +{ + int16_t *pump_ptr = pump_num2ptr(pump_num); + + *pump_ptr = val; + + switch (pump_num) { + case PUMP_RIGHT1_NUM: + pwm_ng_set(RIGHT_PUMP1_PWM, val); + break; + case PUMP_RIGHT2_NUM: + pwm_ng_set(RIGHT_PUMP2_PWM, val); + break; + + /* no pwm, it's remote */ + case PUMP_LEFT1_NUM: + case PUMP_LEFT2_NUM: + default: + break; + } +} + +int16_t pump_num2angle(uint8_t pump_num) +{ + switch (pump_num) { + case PUMP_LEFT1_NUM: + case PUMP_RIGHT1_NUM: + return WRIST_ANGLE_PUMP1; + case PUMP_LEFT2_NUM: + case PUMP_RIGHT2_NUM: + return WRIST_ANGLE_PUMP2; + default: + return 0; + } +} + +void pump_mark_busy(uint8_t pump_num) +{ + switch (pump_num) { + case PUMP_LEFT1_NUM: + mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L1; + break; + case PUMP_RIGHT1_NUM: + mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R1; + break; + case PUMP_LEFT2_NUM: + mechboard.column_flags |= I2C_MECHBOARD_COLUMN_L2; + break; + case PUMP_RIGHT2_NUM: + mechboard.column_flags |= I2C_MECHBOARD_COLUMN_R2; + break; + default: + break; + } + +} + +void pump_mark_free(uint8_t pump_num) +{ + switch (pump_num) { + case PUMP_LEFT1_NUM: + mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L1); + break; + case PUMP_RIGHT1_NUM: + mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R1); + break; + case PUMP_LEFT2_NUM: + mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_L2); + break; + case PUMP_RIGHT2_NUM: + mechboard.column_flags &= (~I2C_MECHBOARD_COLUMN_R2); + break; + default: + break; + } + +} + +uint8_t pump_is_free(uint8_t pump_num) +{ + switch (pump_num) { + case PUMP_LEFT1_NUM: + return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1); + case PUMP_RIGHT1_NUM: + return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1); + case PUMP_LEFT2_NUM: + return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2); + case PUMP_RIGHT2_NUM: + return !(mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2); + default: + return 0; + } +} + +uint8_t pump_is_busy(uint8_t pump_num) +{ + return !pump_is_free(pump_num); +} diff --git a/projects/microb2010/mechboard/arm_highlevel.h b/projects/microb2010/mechboard/arm_highlevel.h new file mode 100644 index 0000000..eaf3ae2 --- /dev/null +++ b/projects/microb2010/mechboard/arm_highlevel.h @@ -0,0 +1,84 @@ +/* + * 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 + * (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: arm_highlevel.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#define ARM_LEFT_NUM 0 +#define ARM_RIGHT_NUM 1 + +#define PUMP_LEFT1_NUM 0 +#define PUMP_RIGHT1_NUM 1 +#define PUMP_LEFT2_NUM 2 +#define PUMP_RIGHT2_NUM 3 + +struct arm *arm_num2ptr(uint8_t arm_num); + +void arm_goto_straight(uint8_t arm_num, uint8_t pump_num); +void arm_goto_get_column(uint8_t arm_num, uint8_t pump_num); +void arm_goto_prepare_get(uint8_t arm_num, uint8_t pump_num); +void arm_goto_intermediate_get(uint8_t arm_num, uint8_t pump_num); +void arm_goto_intermediate_front_get(uint8_t arm_num, uint8_t pump_num); +void arm_goto_prepare_eject(uint8_t arm_num, uint8_t pump_num); +void arm_goto_eject(uint8_t arm_num, uint8_t pump_num); +void arm_goto_loaded(uint8_t arm_num); +void arm_goto_prepare_build_inside(uint8_t arm_num, uint8_t pump_num, + uint8_t level); +void arm_goto_prepare_autobuild_inside(uint8_t arm_num, uint8_t pump_num, + uint8_t level); +void arm_goto_prepare_autobuild_outside(uint8_t arm_num, uint8_t pump_num, + uint8_t level, uint8_t dist); +void arm_goto_autobuild(uint8_t arm_num, uint8_t pump_num, + uint8_t level, uint8_t dist); + +void arm_goto_prepare_get_lintel_inside1(void); +void arm_goto_prepare_get_lintel_inside2(uint8_t lintel_count); +void arm_goto_get_lintel_inside(uint8_t lintel_count); +void arm_goto_prepare_build_lintel1(void); +void arm_goto_prepare_build_lintel2(uint8_t level); +void arm_goto_prepare_build_lintel3(uint8_t level); +void arm_goto_build_lintel(uint8_t level); + +void arm_goto_prepare_get_lintel_disp(void); +void arm_goto_get_lintel_disp(void); + +void arm_goto_prepare_put_lintel(void); +void arm_goto_put_lintel(uint8_t lintel_count); +void arm_goto_prepare_push_temple(uint8_t arm_num); +void arm_goto_push_temple(uint8_t arm_num, uint8_t level); +void arm_goto_prepare_push_temple_disc(uint8_t arm_num); +void arm_goto_push_temple_disc(uint8_t arm_num); + +void arm_prepare_free_pumps(void); +uint8_t arm_wait_both(uint8_t mask); +uint8_t arm_wait_select(uint8_t left, uint8_t right, uint8_t mask); + +/* return the id of the free pump for the arm, or return -1 */ +int8_t arm_get_free_pump(uint8_t arm_num); +int8_t arm_get_busy_pump(uint8_t arm_num); + +#define PUMP_ON 3400 +#define PUMP_OFF 0 +#define PUMP_REVERSE -3400 + +void pump_set(uint8_t pump_num, int16_t val); +int16_t pump_num2angle(uint8_t pump_num); +void pump_mark_busy(uint8_t pump_num); +void pump_mark_free(uint8_t pump_num); +uint8_t pump_is_free(uint8_t pump_num); +uint8_t pump_is_busy(uint8_t pump_num); diff --git a/projects/microb2010/mechboard/arm_xy.c b/projects/microb2010/mechboard/arm_xy.c new file mode 100644 index 0000000..2690844 --- /dev/null +++ b/projects/microb2010/mechboard/arm_xy.c @@ -0,0 +1,809 @@ +/* + * 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 + * (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: arm_xy.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + * Fabrice DESCLAUX + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "main.h" +#include "cmdline.h" +#include "arm_xy.h" +#include "ax12_user.h" + +#define ARM_DEBUG(args...) DEBUG(E_USER_ARM, args) +#define ARM_NOTICE(args...) NOTICE(E_USER_ARM, args) +#define ARM_ERROR(args...) ERROR(E_USER_ARM, args) + +#define DEG(x) (((double)(x)) * (180.0 / M_PI)) +#define RAD(x) (((double)(x)) * (M_PI / 180.0)) +#define M_2PI (2*M_PI) + +/* physical location/dimensions of arm */ +#define ARM_S_LEN 124. +#define ARM_E_LEN 130. + +/* timeout after 1 second if position is not reached */ +#define ARM_GLOBAL_TIMEOUT 1000000L + +/* timeout 100ms after position is reached if not in window */ +#define ARM_WINDOW_TIMEOUT 200000L + +/* default (template) period, but real one is variable */ +#define ARM_PERIOD 50000L +#define ARM_MAX_DIST 40L + +/* we pos reached, check arm in window every period */ +#define ARM_SURVEY_PERIOD 25000UL /* in us */ + +/* number of steps/s */ +#define ARM_AX12_MAX_SPEED (800L) + +/* Maximum number of steps in one ARM_PERIOD */ +#define ARM_MAX_E (((ARM_AX12_MAX_SPEED*ARM_PERIOD)/1000000L)) +/* 4000 steps/CS => 800step/ms */ +#define ARM_MAX_S ((800L*ARM_PERIOD)/1000L) + + +/* window limits in ax12/cs unit */ +#define ARM_SHOULDER_WINDOW_POS 250 +#define ARM_ELBOW_WINDOW_POS 8 +#define ARM_ELBOW_WINDOW_SPEED 100 +#define ARM_WRIST_WINDOW_POS 8 +#define ARM_WRIST_WINDOW_SPEED 100 + +/* default and max speeds */ +#define SHOULDER_DEFAULT_SPEED 800 +#define ELBOW_DEFAULT_SPEED 0x3ff +#define SHOULDER_MAX_SPEED 10000 +#define ELBOW_MAX_SPEED 0x3ff + +/* window status flags */ +#define SHOULDER_NOT_IN_WIN 1 +#define ELBOW_NOT_IN_WIN 2 +#define WRIST_NOT_IN_WIN 4 + +static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out); +static void angle_rad2robot_l(double shoulder_rad, double elbow_rad, + double *shoulder_robot, double *elbow_robot); +static void angle_robot2rad_l(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad); +static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out); +static void angle_rad2robot_r(double shoulder_rad, double elbow_rad, + double *shoulder_robot, double *elbow_robot); +static void angle_robot2rad_r(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad); + +static void arm_schedule_event(struct arm *arm, uint32_t time); + +struct arm left_arm = { + .config = { + .wrist_angle_deg2robot = wrist_angle_deg2robot_l, + .angle_rad2robot = angle_rad2robot_l, + .angle_robot2rad = angle_robot2rad_l, + .elbow_ax12 = L_ELBOW_AX12, + .wrist_ax12 = L_WRIST_AX12, + }, +}; + +struct arm right_arm = { + .config = { + .wrist_angle_deg2robot = wrist_angle_deg2robot_r, + .angle_rad2robot = angle_rad2robot_r, + .angle_robot2rad = angle_robot2rad_r, + .elbow_ax12 = R_ELBOW_AX12, + .wrist_ax12 = R_WRIST_AX12, + }, +}; + +/* process shoulder + elbow angles from height and distance */ +int8_t cart2angle(int32_t h_int, int32_t d_int, double *alpha, double *beta) +{ + double h, d, l; + double elbow, shoulder; + + d = d_int; + h = h_int; + l = sqrt(d*d + h*h); + if (l > (ARM_S_LEN + ARM_E_LEN)) + return -1; + + elbow = -acos((d*d + h*h - ARM_E_LEN*ARM_E_LEN - + ARM_S_LEN*ARM_S_LEN) / (2*ARM_S_LEN*ARM_E_LEN)); + shoulder = atan2(h,d) - atan2(ARM_E_LEN*sin(elbow), + ARM_S_LEN+ARM_E_LEN*cos(elbow)); + + *alpha = shoulder; + *beta = elbow; + + return 0; +} + + +/* process height and distance from shoulder + elbow angles */ +void angle2cart(double alpha, double beta, int32_t *h, int32_t *d) +{ + double tmp_a; + int32_t tmp_h, tmp_d; + + tmp_h = ARM_S_LEN * sin(alpha); + tmp_d = ARM_S_LEN * cos(alpha); + + tmp_a = alpha+beta; + *h = tmp_h + ARM_E_LEN * sin(tmp_a); + *d = tmp_d + ARM_E_LEN * cos(tmp_a); +} + +/*** left arm */ + +#define ARM_LEFT_S_OFFSET -1150. +#define ARM_LEFT_E_OFFSET 476. +#define ARM_LEFT_W_OFFSET 90. + +static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out) +{ + *wrist_out = -wrist_deg * 3.41 + ARM_LEFT_W_OFFSET; +} + +/* convert an angle in radian into a robot-specific unit + * for shoulder and elbow for LEFT ARM*/ +static void angle_rad2robot_l(double shoulder_rad, double elbow_rad, + double *shoulder_robot, double *elbow_robot) +{ + *shoulder_robot = shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_LEFT_S_OFFSET; + *elbow_robot = -elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_LEFT_E_OFFSET; +} + +/* convert a robot-specific unit into an angle in radian + * for shoulder and elbow for LEFT ARM */ +static void angle_robot2rad_l(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad) +{ + *shoulder_rad = ((shoulder_robot - ARM_LEFT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.); + *elbow_rad = -((elbow_robot - ARM_LEFT_E_OFFSET) * (2*M_PI))/(3.41 * 360.); +} + +/*** right arm */ + +#define ARM_RIGHT_S_OFFSET 1150. +#define ARM_RIGHT_E_OFFSET 673. +#define ARM_RIGHT_W_OFFSET 935. + +static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out) +{ + *wrist_out = wrist_deg * 3.41 + ARM_RIGHT_W_OFFSET; +} + +/* convert an angle in radian into a robot-specific unit + * for shoulder and elbow */ +static void angle_rad2robot_r(double shoulder_rad, double elbow_rad, + double *shoulder_robot, double *elbow_robot) +{ + *shoulder_robot = -shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_RIGHT_S_OFFSET; + *elbow_robot = elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_RIGHT_E_OFFSET; +} + +/* convert a robot-specific unit into an angle in radian + * for shoulder and elbow */ +static void angle_robot2rad_r(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad) +{ + *shoulder_rad = -((shoulder_robot - ARM_RIGHT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.); + *elbow_rad = ((elbow_robot - ARM_RIGHT_E_OFFSET) * (2*M_PI))/(3.41 * 360.); +} + + +/* + * Fill the arm_status structure according to request. + * + * return: + * 0 => success + * < 0 => error + */ +static int8_t arm_do_step(struct arm *arm) +{ + const struct arm_config *conf = &arm->config; + const struct arm_request *req = &arm->req; + struct arm_status *status = &arm->status; + + int8_t ret; + int32_t diff_h, diff_d; /* position delta in steps */ + int32_t next_h, next_d; /* next position in steps */ + int32_t l; /* distance between cur pos and next pos */ + + double as_cur_rad, ae_cur_rad; /* current angle in rad */ + double as_next_rad, ae_next_rad; /* next angle in rad */ + double as_cur, ae_cur; /* current angle in angle_steps */ + double as_next, ae_next; /* next angle in angle_steps */ + + int32_t as_diff, ae_diff; /* angle delta in angle_steps */ + int32_t s_speed, e_speed; /* elbow/shoulder speed in angle_steps */ + + double as_coef, ae_coef; + + /* process diff between final request and current pos */ + diff_h = req->h_mm - status->h_mm; + diff_d = req->d_mm - status->d_mm; + ARM_NOTICE("goal:d=%ld,h=%ld cur:d=%ld,h=%ld diff:d=%ld,h=%ld", + req->d_mm, req->h_mm, status->d_mm, status->h_mm, + diff_d, diff_h); + + /* if distance to next point is too large, saturate it */ + l = sqrt(diff_h*diff_h + diff_d*diff_d); + if (l > ARM_MAX_DIST) { + diff_h = diff_h * ARM_MAX_DIST / l; + diff_d = diff_d * ARM_MAX_DIST / l; + } + ARM_NOTICE("l=%ld ; after max dist: diff:d=%ld,h=%ld", l, diff_d, diff_h); + + /* process next position */ + next_h = status->h_mm + diff_h; + next_d = status->d_mm + diff_d; + ARM_DEBUG("next:d=%ld,h=%ld", next_d, next_h); + + /* calculate the current angle of arm in radian */ + ret = cart2angle(status->h_mm, status->d_mm, &as_cur_rad, &ae_cur_rad); + if (ret) + return ret; + ARM_DEBUG("as_cur_rad=%f ae_cur_rad=%f", as_cur_rad, ae_cur_rad); + + /* calculate the next angle of arm in radian */ + ret = cart2angle(next_h, next_d, &as_next_rad, &ae_next_rad); + if (ret) + return ret; + ARM_DEBUG("as_next_rad=%f ae_next_rad=%f", as_next_rad, ae_next_rad); + + /* convert radian in angle_steps */ + conf->angle_rad2robot(as_cur_rad, ae_cur_rad, + &as_cur, &ae_cur); + ARM_DEBUG("as_cur=%f ae_cur=%f", as_cur, ae_cur); + conf->angle_rad2robot(as_next_rad, ae_next_rad, + &as_next, &ae_next); + ARM_DEBUG("as_next=%f ae_next=%f", as_next, ae_next); + + /* process angle delta in angle_steps */ + as_diff = as_next - as_cur; + ae_diff = ae_next - ae_cur; + ARM_DEBUG("as_diff=%ld ae_diff=%ld", as_diff, ae_diff); + + /* update position status */ + status->h_mm = next_h; + status->d_mm = next_d; + status->shoulder_angle_steps = as_next; + status->elbow_angle_steps = ae_next; + status->shoulder_angle_rad = as_next_rad; + status->elbow_angle_rad = ae_next_rad; + + /* we reached destination, nothing to do */ + if (as_diff == 0 && ae_diff == 0) { + status->shoulder_speed = SHOULDER_DEFAULT_SPEED; + status->elbow_speed = ELBOW_DEFAULT_SPEED; + status->next_update_time = 0; + ARM_NOTICE("reaching end"); + return 0; + } + + /* test if one actuator is already in position */ + if (as_diff == 0) { + ARM_DEBUG("shoulder reached destination"); + ae_coef = (double)ARM_MAX_E / (double)ae_diff; + status->next_update_time = ARM_PERIOD * ABS(ae_coef); + e_speed = ABS(ae_coef) * ABS(ae_diff); + s_speed = ARM_MAX_S; + } + else if (ae_diff == 0) { + ARM_DEBUG("elbow reached destination"); + as_coef = (double)ARM_MAX_S / (double)as_diff; + status->next_update_time = ARM_PERIOD / ABS(as_coef); + e_speed = ARM_MAX_E; + s_speed = ABS(as_coef) * ABS(as_diff); + } + + else { + as_coef = (double)ARM_MAX_S / (double)as_diff; + ae_coef = (double)ARM_MAX_E / (double)ae_diff; + + ARM_DEBUG("as_coef=%f ae_coef=%f", as_coef, ae_coef); + + /* if elbow is limitating */ + if (ABS(as_coef) >= ABS(ae_coef)) { + ARM_DEBUG("elbow limit"); + status->next_update_time = ARM_PERIOD / ABS(ae_coef); + s_speed = ABS(ae_coef) * ABS(as_diff); + e_speed = ABS(ae_coef) * ABS(ae_diff); + } + /* else, shoulder is limitating */ + else { + ARM_DEBUG("shoulder limit"); + status->next_update_time = ARM_PERIOD / ABS(as_coef); + s_speed = ABS(as_coef) * ABS(as_diff); + e_speed = ABS(as_coef) * ABS(ae_diff); + } + } + + ARM_NOTICE("next update: %ld", status->next_update_time); + + /* convert speed in specific unit */ + status->shoulder_speed = (s_speed * CS_PERIOD) / ARM_PERIOD; + status->elbow_speed = (e_speed * 0x3ff) / ARM_MAX_E; + + ARM_DEBUG("speeds: s=%ld e=%ld", status->shoulder_speed, status->elbow_speed); + + /* avoid limits */ + if (status->shoulder_speed == 0) + status->shoulder_speed = 1; + if (status->elbow_speed == 0) + status->elbow_speed = 1; + if (status->shoulder_speed >= SHOULDER_MAX_SPEED) + status->shoulder_speed = SHOULDER_MAX_SPEED; + if (status->elbow_speed >= ELBOW_MAX_SPEED) + status->elbow_speed = ELBOW_MAX_SPEED; + + ARM_DEBUG("speeds (sat): s=%ld e=%ld", status->shoulder_speed, status->elbow_speed); + + return 0; +} + +static void arm_delete_event(struct arm *arm) +{ + if (arm->status.event == -1) + return; + ARM_DEBUG("Delete arm event"); + scheduler_del_event(arm->status.event); + arm->status.event = -1; +} + +/* write values to ax12 + cs */ +static void arm_apply(struct arm *arm) +{ + struct cs_block *csb = arm->config.csb; + const struct arm_status *st = &arm->status; + + ARM_DEBUG("arm_apply"); + + if (arm->config.simulate) + return; + + /* set speed and pos of shoulder */ + quadramp_set_1st_order_vars(&csb->qr, + st->shoulder_speed, + st->shoulder_speed); + cs_set_consign(&csb->cs, st->shoulder_angle_steps); + + /* set speed and position of elbow */ + ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12, + AA_MOVING_SPEED_L, st->elbow_speed); + ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12, + AA_GOAL_POSITION_L, st->elbow_angle_steps); +} + +/* return true if one of the mask condition is true */ +uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask) +{ + if ((mask & ARM_TRAJ_END) && (arm->status.state & ARM_FLAG_IN_WINDOW)) + return ARM_TRAJ_END; + + if ((mask & ARM_TRAJ_NEAR) && (arm->status.state & ARM_FLAG_LAST_STEP)) + return ARM_TRAJ_NEAR; + + if ((mask & ARM_TRAJ_TIMEOUT) && (arm->status.state & ARM_FLAG_TIMEOUT)) + return ARM_TRAJ_TIMEOUT; + + if ((mask & ARM_TRAJ_ERROR) && (arm->status.state & ARM_FLAG_ERROR)) + return ARM_TRAJ_ERROR; + + return 0; +} + +uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask) +{ + uint8_t ret; + while(1) { + ret = arm_test_traj_end(arm, mask); + if (ret) + return ret; + } +} + +/* return true if one of the mask condition is true */ +uint8_t arm_in_window(struct arm *arm, uint8_t *status) +{ + int8_t err; +/* uint16_t spd; */ + int16_t pos; + int32_t cs_err; + + *status = 0; + + if (arm->config.simulate) + return 1; + + /* shoulder, just check position */ + cs_err = cs_get_error(&arm->config.csb->cs); + if (ABS(cs_err) > ARM_SHOULDER_WINDOW_POS) + *status |= SHOULDER_NOT_IN_WIN; + +#if 0 + /* check elbow speed */ + err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12, + AA_PRESENT_SPEED_L, &spd); + if (err) + goto fail; + if (spd > ARM_ELBOW_WINDOW_SPEED) + return 0; + + /* check wrist speed */ + err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, + AA_PRESENT_SPEED_L, &spd); + if (err) + goto fail; + if (spd > ARM_WRIST_WINDOW_SPEED) + return 0; +#endif + /* check elbow pos */ + err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12, + AA_PRESENT_POSITION_L, (uint16_t *)&pos); + if (err) + goto fail; + if (ABS(arm->status.elbow_angle_steps - pos) > ARM_ELBOW_WINDOW_POS) + *status |= ELBOW_NOT_IN_WIN; + + /* check wrist pos */ + err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, + AA_PRESENT_POSITION_L, (uint16_t *)&pos); + if (err) + goto fail; + if (ABS(arm->status.wrist_angle_steps - pos) > ARM_WRIST_WINDOW_POS) + *status |= WRIST_NOT_IN_WIN; + + if (*status) + return 0; + + ARM_NOTICE("arm is in window (%ld us after reach pos)", + time_get_us2() - arm->status.pos_reached_time); + return 1; /* ok, we are in window */ + + fail: + return 0; +} + +/* process wrist pos and apply it. it's done only once. */ +static int8_t arm_set_wrist(struct arm *arm) +{ + int8_t err; + int32_t as_deg, ae_deg, aw_deg; + uint16_t wrist_out_u16; + double wrist_out, as_rad, ae_rad; + int16_t pos; + uint32_t diff_time; + + /* calculate the destination angle of arm in radian */ + err = cart2angle(arm->req.h_mm, arm->req.d_mm, + &as_rad, &ae_rad); + if (err) + return -1; + + /* calc angle destination */ + as_deg = DEG(as_rad); + ae_deg = DEG(ae_rad); + ARM_DEBUG("as_dest_deg=%d ae_dest_deg=%d", as_deg, ae_deg); + aw_deg = as_deg + ae_deg - arm->req.w_deg; + arm->config.wrist_angle_deg2robot(aw_deg, &wrist_out); + wrist_out_u16 = wrist_out; + + ARM_DEBUG("set wrist to %ld degrees (%d steps)", aw_deg, + wrist_out_u16); + + /* process the theorical reach time for the wrist */ + if (arm->config.simulate) { + pos = arm->status.wrist_angle_steps; + } + else { + err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, + AA_PRESENT_POSITION_L, (uint16_t *)&pos); + if (err) + pos = arm->status.wrist_angle_steps; + } + /* 600 is the number of steps/s */ + diff_time = (ABS((int16_t)wrist_out_u16 - pos) * 1000000L) / 600; + arm->status.wrist_reach_time = arm->status.start_time + diff_time; + ARM_DEBUG("wrist reach time is %ld (diff=%ld)", + arm->status.wrist_reach_time, diff_time); + + /* update current position to destination */ + arm->status.wrist_angle_steps = wrist_out_u16; + + if (arm->config.simulate) + return 0; + + /* send it to ax12 */ + ax12_user_write_int(&gen.ax12, arm->config.wrist_ax12, + AA_GOAL_POSITION_L, wrist_out_u16); + return 0; +} + +/* event callback */ +static void arm_do_xy_cb(struct arm *arm) +{ + uint8_t win_status; + + arm->status.event = -1; + + /* if consign haven't reach destination */ + if ((arm->status.state & ARM_FLAG_LAST_STEP) == 0) { + if (arm_do_step(arm)) + arm->status.state |= ARM_FLAG_ERROR; + + /* it's the first call for the traj */ + if (arm->status.state == ARM_STATE_INIT) { + arm->status.state |= ARM_FLAG_MOVING; + if (arm_set_wrist(arm)) + arm->status.state |= ARM_FLAG_ERROR; + } + + /* we have more steps to do */ + if (arm->status.next_update_time == 0) { + arm->status.state &= ~ARM_FLAG_MOVING; + arm->status.state |= ARM_FLAG_LAST_STEP; + arm->status.pos_reached_time = time_get_us2(); + } + arm_apply(arm); + } + /* last step is reached, we can check that arm is in window */ + else if ((arm->status.state & ARM_FLAG_IN_WINDOW) == 0) { + if (arm_in_window(arm, &win_status)) + arm->status.state |= ARM_FLAG_IN_WINDOW; + + /* check for window arm timeout */ + else { + microseconds t; + int32_t diff1, diff2; + t = time_get_us2(); + diff1 = t - arm->status.pos_reached_time; + diff2 = t - arm->status.wrist_reach_time; + if (diff1 > ARM_WINDOW_TIMEOUT && + diff2 > ARM_WINDOW_TIMEOUT) { + ARM_NOTICE("win timeout at %ld win_status=%x", + t, win_status); + arm->status.state |= ARM_FLAG_TIMEOUT; + } + } + } + + /* check for global arm timeout */ + if ((time_get_us2() - arm->status.start_time) > ARM_GLOBAL_TIMEOUT) { + ARM_NOTICE("global timeout at %ld", time_get_us2()); + arm->status.state |= ARM_FLAG_TIMEOUT; + } + + /* reload event if needed */ + if ((arm->status.state & ARM_FLAG_FINISHED) == ARM_FLAG_FINISHED) { + ARM_NOTICE("arm traj finished"); + return; /* no more event, position reached */ + } + if (arm->status.state & (ARM_FLAG_ERROR|ARM_FLAG_TIMEOUT)) { + ARM_NOTICE("error or timeout"); + return; /* no more event */ + } + else if (arm->status.state & ARM_FLAG_LAST_STEP) { + /* theorical position is reached, but reload an event + * for position survey (window), every 25ms */ + arm_schedule_event(arm, ARM_SURVEY_PERIOD); + } + else { + /* reload event for next position step */ + arm_schedule_event(arm, arm->status.next_update_time); + } +} + +/* schedule a single event for this arm */ +static void arm_schedule_event(struct arm *arm, uint32_t time) +{ + uint8_t flags; + int8_t ret; + + arm_delete_event(arm); + if (time < SCHEDULER_UNIT) + time = SCHEDULER_UNIT; + IRQ_LOCK(flags); + ret = scheduler_add_event(SCHEDULER_SINGLE, + (void *)arm_do_xy_cb, + arm, time/SCHEDULER_UNIT, ARM_PRIO); + if (ret == -1) { + IRQ_UNLOCK(flags); + ARM_ERROR("Cannot load arm event"); + return; + } + arm->status.event = ret; + IRQ_UNLOCK(flags); +} + +int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg) +{ + ARM_NOTICE("arm_do_xy: d_mm=%d h_mm=%d w_deg=%d", d_mm, h_mm, w_deg); + + /* remove previous event if any */ + arm_delete_event(arm); + + /* init mandatory params */ + arm->req.d_mm = d_mm; + arm->req.h_mm = h_mm; + arm->req.w_deg = w_deg; + arm->status.start_time = time_get_us2(); + arm->status.state = ARM_STATE_INIT; + + /* all the job will be done asynchronously now */ + arm_schedule_event(arm, 0); + return 0; +} + +void arm_dump(struct arm *arm) +{ + printf_P(PSTR("config: simulate=%d\r\n"), + arm->config.simulate); + printf_P(PSTR("req: d_mm=%ld h_mm=%ld w_deg=%ld\r\n"), + arm->req.d_mm, arm->req.h_mm, arm->req.w_deg); + printf_P(PSTR("status: ")); + if (arm->status.state == ARM_STATE_INIT) + printf_P(PSTR("ARM_STATE_INIT ")); + if (arm->status.state & ARM_FLAG_MOVING) + printf_P(PSTR("ARM_FLAG_MOVING ")); + if (arm->status.state & ARM_FLAG_LAST_STEP) + printf_P(PSTR("ARM_FLAG_LAST_STEP ")); + if (arm->status.state & ARM_FLAG_IN_WINDOW) + printf_P(PSTR("ARM_FLAG_IN_WINDOW ")); + if (arm->status.state & ARM_FLAG_ERROR) + printf_P(PSTR("ARM_FLAG_ERROR ")); + if (arm->status.state & ARM_FLAG_TIMEOUT) + printf_P(PSTR("ARM_FLAG_TIMEOUT ")); + printf_P(PSTR("\r\n")); + + printf_P(PSTR(" d_mm=%ld h_mm=%ld goal_w_steps=%d\r\n"), + arm->status.d_mm, arm->status.h_mm, arm->status.wrist_angle_steps); + printf_P(PSTR(" cur_shl_steps=%ld cur_elb_steps=%ld\r\n"), + arm->status.shoulder_angle_steps, arm->status.elbow_angle_steps); + printf_P(PSTR(" cur_shl_rad=%f cur_elb_rad=%f\r\n"), + arm->status.shoulder_angle_rad, arm->status.elbow_angle_rad); + printf_P(PSTR(" cur_shl_deg=%f cur_elb_deg=%f\r\n"), + DEG(arm->status.shoulder_angle_rad), DEG(arm->status.elbow_angle_rad)); + printf_P(PSTR(" event=%d next_update_time=%ld\r\n"), + arm->status.event, arm->status.next_update_time); + printf_P(PSTR(" start_time=%ld pos_reached_time=%ld wrist_reach_time=%ld\r\n"), + arm->status.start_time, arm->status.pos_reached_time, + arm->status.wrist_reach_time); +} + +#define CALIB_ANGLE (RAD(-93.)) + +void arm_calibrate(void) +{ + double shoulder, elbow; + + pwm_ng_set(LEFT_ARM_PWM, 500); + pwm_ng_set(RIGHT_ARM_PWM, -500); + wait_ms(200); + + pwm_ng_set(LEFT_ARM_PWM, 300); + pwm_ng_set(RIGHT_ARM_PWM, -300); + wait_ms(700); + + printf_P(PSTR("Init arm, please wait...")); + ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_TORQUE_ENABLE, 0x1); + ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_ALARM_SHUTDOWN, 0x04); + + angle_rad2robot_r(0, CALIB_ANGLE, &shoulder, &elbow); + ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_GOAL_POSITION_L, elbow); + ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_GOAL_POSITION_L, 628); + + angle_rad2robot_l(0, CALIB_ANGLE, &shoulder, &elbow); + ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_GOAL_POSITION_L, elbow); + ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_GOAL_POSITION_L, 394); + pwm_ng_set(LEFT_ARM_PWM, -100); + pwm_ng_set(RIGHT_ARM_PWM, 100); + + wait_ms(2000); + + cs_set_consign(&mechboard.left_arm.cs, 0); + cs_set_consign(&mechboard.right_arm.cs, 0); + encoders_spi_set_value(LEFT_ARM_ENCODER, 0); + encoders_spi_set_value(RIGHT_ARM_ENCODER, 0); + + printf_P(PSTR("ok\r\n")); +} + +/* init arm config */ +void arm_init(void) +{ + uint32_t shoulder_robot; + uint16_t elbow_robot, wrist_robot; + double shoulder_rad, elbow_rad; + int32_t h, d; + uint8_t err = 0; + + memset(&left_arm.status, 0, sizeof(left_arm.status)); + memset(&right_arm.status, 0, sizeof(right_arm.status)); + left_arm.status.event = -1; + right_arm.status.event = -1; + + arm_calibrate(); + + /* set des slopes XXX */ + + /* set maximum moving speeds */ + err |= ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff); + err |= ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff); + err |= ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff); + err |= ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff); + + /* left arm init */ + shoulder_robot = encoders_spi_get_value(LEFT_ARM_ENCODER); + err |= ax12_user_read_int(&gen.ax12, L_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot); + err |= ax12_user_read_int(&gen.ax12, L_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot); + + angle_robot2rad_l(shoulder_robot, elbow_robot, + &shoulder_rad, &elbow_rad); + angle2cart(shoulder_rad, elbow_rad, &h, &d); + printf_P(PSTR("left arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot); + left_arm.status.h_mm = h; + left_arm.status.d_mm = d; + left_arm.status.wrist_angle_steps = wrist_robot; + left_arm.status.state = ARM_FLAG_FINISHED; + left_arm.config.csb = &mechboard.left_arm; + + /* left arm init */ + shoulder_robot = encoders_spi_get_value(RIGHT_ARM_ENCODER); + err |= ax12_user_read_int(&gen.ax12, R_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot); + err |= ax12_user_read_int(&gen.ax12, R_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot); + + angle_robot2rad_r(shoulder_robot, elbow_robot, + &shoulder_rad, &elbow_rad); + angle2cart(shoulder_rad, elbow_rad, &h, &d); + printf_P(PSTR("right arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot); + right_arm.status.h_mm = h; + right_arm.status.d_mm = d; + right_arm.status.wrist_angle_steps = wrist_robot; + right_arm.status.state = ARM_FLAG_FINISHED; + right_arm.config.csb = &mechboard.right_arm; + + if (err) + ARM_ERROR("ARM INIT ERROR"); +} diff --git a/projects/microb2010/mechboard/arm_xy.h b/projects/microb2010/mechboard/arm_xy.h new file mode 100644 index 0000000..3b1407f --- /dev/null +++ b/projects/microb2010/mechboard/arm_xy.h @@ -0,0 +1,123 @@ +/* + * 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 + * (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: arm_xy.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $ + * + * Fabrice DESCLAUX + * Olivier MATZ + */ + +/* all static configuration for an arm */ +struct arm_config { + void (*wrist_angle_deg2robot)(double wrist_edg, + double *wrist_out); + + void (*angle_rad2robot)(double shoulder_deg, double elbow_deg, + double *shoulder_out, double *elbow_out); + + void (*angle_robot2rad)(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad); + + /* ax12 identifiers */ + int8_t elbow_ax12; + int8_t wrist_ax12; + + /* related control system */ + struct cs_block *csb; + + /* if true, don't apply to ax12 / cs */ + uint8_t simulate; +}; + +/* request for a final position, in mm */ +struct arm_request { + int32_t h_mm; + int32_t d_mm; + int32_t w_deg; +}; + +/* returned by arm_test_traj_end() */ +#define ARM_TRAJ_END 0x01 +#define ARM_TRAJ_NEAR 0x02 +#define ARM_TRAJ_TIMEOUT 0x04 +#define ARM_TRAJ_ERROR 0x08 + +#define ARM_TRAJ_ALL (ARM_TRAJ_END|ARM_TRAJ_TIMEOUT|ARM_TRAJ_ERROR) +#define ARM_TRAJ_ALL_NEAR (ARM_TRAJ_END|ARM_TRAJ_NEAR|ARM_TRAJ_TIMEOUT|ARM_TRAJ_ERROR) +#define ARM_TRAJ_END_NEAR (ARM_TRAJ_END|ARM_TRAJ_NEAR) + +#define ARM_STATE_INIT 0 +#define ARM_FLAG_MOVING 0x01 /* arm is currently moving */ +#define ARM_FLAG_LAST_STEP 0x02 /* no more step is needed */ +#define ARM_FLAG_IN_WINDOW 0x04 /* arm speed and pos are ok */ +#define ARM_FLAG_TIMEOUT 0x08 /* too much time too reach pos */ +#define ARM_FLAG_ERROR 0x10 /* error */ +#define ARM_FLAG_FINISHED (ARM_FLAG_LAST_STEP | ARM_FLAG_IN_WINDOW) + +/* Describes the current position of the arm. Mainly filled by + * arm_do_step(), arm_set_wrist(), arm_do_xy_cb(), ... */ +struct arm_status { + /* current position */ + int32_t h_mm; + int32_t d_mm; + + /* wrist goal position (set once at init) */ + int16_t wrist_angle_steps; + + /* current angles in steps */ + int32_t elbow_angle_steps; + int32_t shoulder_angle_steps; + + /* current angles in radian */ + double elbow_angle_rad; + double shoulder_angle_rad; + + /* time before next update */ + uint32_t next_update_time; + + /* what speed to be applied, in specific speed unit */ + uint32_t shoulder_speed; + uint32_t elbow_speed; + + volatile int8_t state; /* see list of flags above */ + int8_t event; /* scheduler event, -1 if not running */ + + microseconds start_time; /* when we started that command */ + microseconds wrist_reach_time; /* when the wrist should reach dest */ + microseconds pos_reached_time; /* when last step is sent */ +}; + +struct arm { + struct arm_config config; + struct arm_status status; + struct arm_request req; +}; + +extern struct arm left_arm; +extern struct arm right_arm; + +uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask); +uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask); + +/* do a specific move to distance, height. This function _must_ be + * called from a context with a prio < ARM_PRIO to avoid any race + * condition. */ +int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg); + +void arm_dump(struct arm *arm); +void arm_calibrate(void); +void arm_init(void); diff --git a/projects/microb2010/mechboard/ax12_config.h b/projects/microb2010/mechboard/ax12_config.h new file mode 100755 index 0000000..072e8fb --- /dev/null +++ b/projects/microb2010/mechboard/ax12_config.h @@ -0,0 +1,7 @@ +#ifndef _AX12_CONFIG_H_ +#define _AX12_CONFIG_H_ + +#define AX12_MAX_PARAMS 32 + + +#endif/*_AX12_CONFIG_H_*/ diff --git a/projects/microb2010/mechboard/ax12_user.c b/projects/microb2010/mechboard/ax12_user.c new file mode 100644 index 0000000..9000bbd --- /dev/null +++ b/projects/microb2010/mechboard/ax12_user.c @@ -0,0 +1,305 @@ +/* + * 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 + * (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: ax12_user.c,v 1.4 2009-04-24 19:30:42 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "ax12_user.h" + +#define AX12_ERROR(args...) ERROR(E_USER_AX12, args) +#define AX12_NOTICE(args...) NOTICE(E_USER_AX12, args) +#define AX12_MAX_TRIES 3 + +/* + * Cmdline interface for AX12. Use the PC to command a daisy-chain of + * AX12 actuators with a nice command line interface. + * + * The circuit should be as following: + * + * |----------| + * | uart3|------->--- PC (baudrate=57600) + * | |-------<--- + * | atmega128| + * | | + * | uart0|---->---+-- AX12 (baudrate 115200) + * | |----<---| + * |----------| + * + * Note that RX and TX pins of UART1 are connected together to provide + * a half-duplex UART emulation. + * + */ + +#define UCSRxB UCSR0B +#define AX12_TIMEOUT 15000L /* in us */ + +static uint32_t ax12_stats_ops = 0; /* total ops */ +static uint32_t ax12_stats_fails = 0; /* number of fails */ +static uint32_t ax12_stats_drops = 0; /* number of drops (3 fails) */ +static uint32_t ax12_dropped_logs = 0; /* error messages that were not displayed */ +static microseconds t_prev_msg = 0; + +/********************************* AX12 commands */ + +/* + * We use synchronous access (not interrupt driven) to the hardware + * UART, because we have to be sure that the transmission/reception is + * really finished when we return from the functions. + * + * We don't use the CM-5 circuit as described in the AX12 + * documentation, we simply connect TX and RX and use TXEN + RXEN + + * DDR to manage the port directions. + */ + +static volatile uint8_t ax12_state = AX12_STATE_READ; +extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */ +static volatile uint8_t ax12_nsent = 0; + +/* Called by ax12 module to send a character on serial line. Count the + * number of transmitted bytes. It will be used in ax12_recv_char() to + * drop the bytes that we transmitted. */ +static int8_t ax12_send_char(uint8_t c) +{ + uart_send(UART_AX12_NUM, c); + ax12_nsent++; + return 0; +} + +/* for atmega256 */ +#ifndef TXEN +#define TXEN TXEN0 +#endif + +/* called by uart module when the character has been written in + * UDR. It does not mean that the byte is physically transmitted. */ +static void ax12_send_callback(__attribute__((unused)) char c) +{ + if (ax12_state == AX12_STATE_READ) { + /* disable TX when last byte is pushed. */ + if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM])) + UCSRxB &= ~(1< AX12_TIMEOUT) + return -1; + } + return c; +} + +/* called by ax12 module when we want to switch serial line. As we + * work in interruption mode, this function can be called to switch + * back in read mode even if the bytes are not really transmitted on + * the line. That's why in this case we do nothing, we will fall back + * in read mode in any case when xmit is finished -- see in + * ax12_send_callback() -- */ +static void ax12_switch_uart(uint8_t state) +{ + uint8_t flags; + + if (state == AX12_STATE_WRITE) { + IRQ_LOCK(flags); + ax12_nsent=0; + while (uart_recv_nowait(UART_AX12_NUM) != -1); + UCSRxB |= (1< + * + * 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: ax12_user.h,v 1.3 2009-04-07 20:03:48 zer0 Exp $ + * + */ + +/* This is the ax12 user interface. It initializes the aversive AX12 + * module so that it works in background, using interrupt driver uart. + * + * Be carreful, a call to AX12 module is synchronous and uses + * interruptions, so interrupts must be enabled. On the other side, a + * call _must not_ interrupt another one. That's why all calls to the + * module are done either in init() functions or in a scheduler event + * with prio=ARM_PRIO. + */ + +/* XXX do a safe_ax12() function that will retry once or twice if we + * see some problems. */ + +#define UART_AX12_NUM 0 + +void ax12_user_init(void); + +void ax12_dump_stats(void); + +#define ax12_user_write_byte(ax12, id, addr, data) \ + __ax12_user_write_byte(ax12, id, addr, data, __LINE__) + +#define ax12_user_write_int(ax12, id, addr, data) \ + __ax12_user_write_int(ax12, id, addr, data, __LINE__) + +#define ax12_user_read_byte(ax12, id, addr, data) \ + __ax12_user_read_byte(ax12, id, addr, data, __LINE__) + +#define ax12_user_read_int(ax12, id, addr, data) \ + __ax12_user_read_int(ax12, id, addr, data, __LINE__) + +/** @brief Write byte in AX-12 memory + * @return Error code from AX-12 (0 means okay) */ +uint8_t __ax12_user_write_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address, + uint8_t data, uint16_t line); + +/** @brief Write integer (2 bytes) in AX-12 memory + * @return Error code from AX-12 (0 means okay) + * + * address : data low + * address+1 : data high + */ +uint8_t __ax12_user_write_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address, + uint16_t data, uint16_t line); + +/** @brief Read byte from AX-12 memory + * @return Error code from AX-12 (0 means okay) */ +uint8_t __ax12_user_read_byte(AX12 *ax12, uint8_t id, AX12_ADDRESS address, + uint8_t *val, uint16_t line); + +/** @brief Write integer (2 bytes) from AX-12 memory + * @return Error code from AX-12 (0 means okay) */ +uint8_t __ax12_user_read_int(AX12 *ax12, uint8_t id, AX12_ADDRESS address, + uint16_t *val, uint16_t line); diff --git a/projects/microb2010/mechboard/cmdline.c b/projects/microb2010/mechboard/cmdline.c new file mode 100644 index 0000000..f784b35 --- /dev/null +++ b/projects/microb2010/mechboard/cmdline.c @@ -0,0 +1,149 @@ +/* + * 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 + * (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: cmdline.c,v 1.3 2009-04-24 19:30:42 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cmdline.h" + + +/******** See in commands.c for the list of commands. */ +extern parse_pgm_ctx_t main_ctx[]; + +static void write_char(char c) +{ + uart_send(CMDLINE_UART, c); +} + +static void +valid_buffer(const char *buf, __attribute__((unused)) uint8_t size) +{ + int8_t ret; + ret = parse(main_ctx, buf); + if (ret == PARSE_AMBIGUOUS) + printf_P(PSTR("Ambiguous command\r\n")); + else if (ret == PARSE_NOMATCH) + printf_P(PSTR("Command not found\r\n")); + else if (ret == PARSE_BAD_ARGS) + printf_P(PSTR("Bad arguments\r\n")); +} + +static int8_t +complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize, + int16_t *state) +{ + return complete(main_ctx, buf, state, dstbuf, dstsize); +} + + +/* sending "pop" on cmdline uart resets the robot */ +void emergency(char c) +{ + static uint8_t i = 0; + + if ((i == 0 && c == 'p') || + (i == 1 && c == 'o') || + (i == 2 && c == 'p')) + i++; + else if ( !(i == 1 && c == 'p') ) + i = 0; + if (i == 3) + reset(); +} + +/* log function, add a command to configure + * it dynamically */ +void mylog(struct error * e, ...) +{ + va_list ap; + u16 stream_flags = stdout->flags; + uint8_t i; + time_h tv; + + if (e->severity > ERROR_SEVERITY_ERROR) { + if (gen.log_level < e->severity) + return; + + for (i=0; ierr_num) + break; + if (i == NB_LOGS+1) + return; + } + + va_start(ap, e); + tv = time_get_time(); + printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL)); + vfprintf_P(stdout, e->text, ap); + printf_P(PSTR("\r\n")); + va_end(ap); + stdout->flags = stream_flags; +} + +int cmdline_interact(void) +{ + const char *history, *buffer; + int8_t ret, same = 0; + int16_t c; + + rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer); + snprintf(gen.prompt, sizeof(gen.prompt), "mechboard > "); + rdline_newline(&gen.rdl, gen.prompt); + + while (1) { + c = uart_recv_nowait(CMDLINE_UART); + if (c == -1) + continue; + ret = rdline_char_in(&gen.rdl, c); + if (ret != 2 && ret != 0) { + buffer = rdline_get_buffer(&gen.rdl); + history = rdline_get_history_item(&gen.rdl, 0); + if (history) { + same = !memcmp(buffer, history, strlen(history)) && + buffer[strlen(history)] == '\n'; + } + else + same = 0; + if (strlen(buffer) > 1 && !same) + rdline_add_history(&gen.rdl, buffer); + rdline_newline(&gen.rdl, gen.prompt); + } + } + + return 0; +} diff --git a/projects/microb2010/mechboard/cmdline.h b/projects/microb2010/mechboard/cmdline.h new file mode 100644 index 0000000..e5bb569 --- /dev/null +++ b/projects/microb2010/mechboard/cmdline.h @@ -0,0 +1,40 @@ +/* + * 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 + * (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: cmdline.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#define CMDLINE_UART 1 + +/* uart rx callback for reset() */ +void emergency(char c); + +/* log function */ +void mylog(struct error * e, ...); + +/* launch cmdline */ +int cmdline_interact(void); + +static inline uint8_t cmdline_keypressed(void) { + return (uart_recv_nowait(CMDLINE_UART) != -1); +} + +static inline int16_t cmdline_getchar(void) { + return uart_recv_nowait(CMDLINE_UART); +} diff --git a/projects/microb2010/mechboard/commands.c b/projects/microb2010/mechboard/commands.c new file mode 100644 index 0000000..1c2186d --- /dev/null +++ b/projects/microb2010/mechboard/commands.c @@ -0,0 +1,150 @@ +/* + * 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 + * (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: commands.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include + +/* commands_gen.c */ +extern parse_pgm_inst_t cmd_reset; +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_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; +extern parse_pgm_inst_t cmd_stack_space; +extern parse_pgm_inst_t cmd_scheduler; + +/* commands_ax12.c */ +extern parse_pgm_inst_t cmd_baudrate; +extern parse_pgm_inst_t cmd_uint16_read; +extern parse_pgm_inst_t cmd_uint16_write; +extern parse_pgm_inst_t cmd_uint8_read; +extern parse_pgm_inst_t cmd_uint8_write; +extern parse_pgm_inst_t cmd_ax12_stress; +extern parse_pgm_inst_t cmd_ax12_dump_stats; + +/* commands_cs.c */ +extern parse_pgm_inst_t cmd_gain; +extern parse_pgm_inst_t cmd_gain_show; +extern parse_pgm_inst_t cmd_speed; +extern parse_pgm_inst_t cmd_speed_show; +extern parse_pgm_inst_t cmd_derivate_filter; +extern parse_pgm_inst_t cmd_derivate_filter_show; +extern parse_pgm_inst_t cmd_consign; +extern parse_pgm_inst_t cmd_maximum; +extern parse_pgm_inst_t cmd_maximum_show; +extern parse_pgm_inst_t cmd_quadramp; +extern parse_pgm_inst_t cmd_quadramp_show; +extern parse_pgm_inst_t cmd_cs_status; +extern parse_pgm_inst_t cmd_blocking_i; +extern parse_pgm_inst_t cmd_blocking_i_show; + +/* commands_mechboard.c */ +extern parse_pgm_inst_t cmd_event; +extern parse_pgm_inst_t cmd_color; +extern parse_pgm_inst_t cmd_arm_show; +extern parse_pgm_inst_t cmd_arm_goto; +extern parse_pgm_inst_t cmd_arm_goto_fixed; +extern parse_pgm_inst_t cmd_arm_simulate; +extern parse_pgm_inst_t cmd_finger; +extern parse_pgm_inst_t cmd_pump; +extern parse_pgm_inst_t cmd_state1; +extern parse_pgm_inst_t cmd_state2; +extern parse_pgm_inst_t cmd_state3; +extern parse_pgm_inst_t cmd_state4; +extern parse_pgm_inst_t cmd_state5; +extern parse_pgm_inst_t cmd_state_debug; +extern parse_pgm_inst_t cmd_state_machine; +extern parse_pgm_inst_t cmd_servo_lintel; +extern parse_pgm_inst_t cmd_pump_current; +extern parse_pgm_inst_t cmd_manivelle; +extern parse_pgm_inst_t cmd_test; + + +/* in progmem */ +parse_pgm_ctx_t main_ctx[] = { + + /* commands_gen.c */ + (parse_pgm_inst_t *)&cmd_reset, + (parse_pgm_inst_t *)&cmd_bootloader, + (parse_pgm_inst_t *)&cmd_encoders, + (parse_pgm_inst_t *)&cmd_pwm, + (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, + (parse_pgm_inst_t *)&cmd_stack_space, + (parse_pgm_inst_t *)&cmd_scheduler, + + /* commands_ax12.c */ + (parse_pgm_inst_t *)&cmd_baudrate, + (parse_pgm_inst_t *)&cmd_uint16_read, + (parse_pgm_inst_t *)&cmd_uint16_write, + (parse_pgm_inst_t *)&cmd_uint8_read, + (parse_pgm_inst_t *)&cmd_uint8_write, + (parse_pgm_inst_t *)&cmd_ax12_stress, + (parse_pgm_inst_t *)&cmd_ax12_dump_stats, + + /* commands_cs.c */ + (parse_pgm_inst_t *)&cmd_gain, + (parse_pgm_inst_t *)&cmd_gain_show, + (parse_pgm_inst_t *)&cmd_speed, + (parse_pgm_inst_t *)&cmd_speed_show, + (parse_pgm_inst_t *)&cmd_consign, + (parse_pgm_inst_t *)&cmd_derivate_filter, + (parse_pgm_inst_t *)&cmd_derivate_filter_show, + (parse_pgm_inst_t *)&cmd_maximum, + (parse_pgm_inst_t *)&cmd_maximum_show, + (parse_pgm_inst_t *)&cmd_quadramp, + (parse_pgm_inst_t *)&cmd_quadramp_show, + (parse_pgm_inst_t *)&cmd_cs_status, + (parse_pgm_inst_t *)&cmd_blocking_i, + (parse_pgm_inst_t *)&cmd_blocking_i_show, + + /* commands_mechboard.c */ + (parse_pgm_inst_t *)&cmd_event, + (parse_pgm_inst_t *)&cmd_color, + (parse_pgm_inst_t *)&cmd_arm_show, + (parse_pgm_inst_t *)&cmd_arm_goto, + (parse_pgm_inst_t *)&cmd_arm_goto_fixed, + (parse_pgm_inst_t *)&cmd_arm_simulate, + (parse_pgm_inst_t *)&cmd_finger, + (parse_pgm_inst_t *)&cmd_pump, + (parse_pgm_inst_t *)&cmd_state1, + (parse_pgm_inst_t *)&cmd_state2, + (parse_pgm_inst_t *)&cmd_state3, + (parse_pgm_inst_t *)&cmd_state4, + (parse_pgm_inst_t *)&cmd_state5, + (parse_pgm_inst_t *)&cmd_state_debug, + (parse_pgm_inst_t *)&cmd_state_machine, + (parse_pgm_inst_t *)&cmd_servo_lintel, + (parse_pgm_inst_t *)&cmd_pump_current, + (parse_pgm_inst_t *)&cmd_manivelle, + (parse_pgm_inst_t *)&cmd_test, + + NULL, +}; diff --git a/projects/microb2010/mechboard/commands_ax12.c b/projects/microb2010/mechboard/commands_ax12.c new file mode 100644 index 0000000..b052944 --- /dev/null +++ b/projects/microb2010/mechboard/commands_ax12.c @@ -0,0 +1,427 @@ +/* + * 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 + * (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: commands_ax12.c,v 1.3 2009-04-24 19:30:42 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "ax12_user.h" + +uint8_t addr_from_string(const char *s) +{ + /* 16 bits */ + if (!strcmp_P(s, PSTR("cw_angle_limit"))) + return AA_CW_ANGLE_LIMIT_L; + if (!strcmp_P(s, PSTR("ccw_angle_limit"))) + return AA_CCW_ANGLE_LIMIT_L; + if (!strcmp_P(s, PSTR("max_torque"))) + return AA_MAX_TORQUE_L; + if (!strcmp_P(s, PSTR("down_calibration"))) + return AA_DOWN_CALIBRATION_L; + if (!strcmp_P(s, PSTR("up_calibration"))) + return AA_UP_CALIBRATION_L; + if (!strcmp_P(s, PSTR("torque_limit"))) + return AA_TORQUE_LIMIT_L; + if (!strcmp_P(s, PSTR("position"))) + return AA_PRESENT_POSITION_L; + if (!strcmp_P(s, PSTR("speed"))) + return AA_PRESENT_SPEED_L; + if (!strcmp_P(s, PSTR("load"))) + return AA_PRESENT_LOAD_L; + if (!strcmp_P(s, PSTR("moving_speed"))) + return AA_MOVING_SPEED_L; + if (!strcmp_P(s, PSTR("model"))) + return AA_MODEL_NUMBER_L; + if (!strcmp_P(s, PSTR("goal_pos"))) + return AA_GOAL_POSITION_L; + if (!strcmp_P(s, PSTR("punch"))) + return AA_PUNCH_L; + + /* 8 bits */ + if (!strcmp_P(s, PSTR("firmware"))) + return AA_FIRMWARE; + if (!strcmp_P(s, PSTR("id"))) + return AA_ID; + if (!strcmp_P(s, PSTR("baudrate"))) + return AA_BAUD_RATE; + if (!strcmp_P(s, PSTR("delay"))) + return AA_DELAY_TIME; + if (!strcmp_P(s, PSTR("high_lim_temp"))) + return AA_HIGHEST_LIMIT_TEMP; + if (!strcmp_P(s, PSTR("low_lim_volt"))) + return AA_LOWEST_LIMIT_VOLTAGE; + if (!strcmp_P(s, PSTR("high_lim_volt"))) + return AA_HIGHEST_LIMIT_VOLTAGE; + if (!strcmp_P(s, PSTR("status_return"))) + return AA_STATUS_RETURN_LEVEL; + if (!strcmp_P(s, PSTR("alarm_led"))) + return AA_ALARM_LED; + if (!strcmp_P(s, PSTR("alarm_shutdown"))) + return AA_ALARM_SHUTDOWN; + if (!strcmp_P(s, PSTR("torque_enable"))) + return AA_TORQUE_ENABLE; + if (!strcmp_P(s, PSTR("led"))) + return AA_LED; + if (!strcmp_P(s, PSTR("cw_comp_margin"))) + return AA_CW_COMPLIANCE_MARGIN; + if (!strcmp_P(s, PSTR("ccw_comp_margin"))) + return AA_CCW_COMPLIANCE_MARGIN; + if (!strcmp_P(s, PSTR("cw_comp_slope"))) + return AA_CW_COMPLIANCE_SLOPE; + if (!strcmp_P(s, PSTR("ccw_comp_slope"))) + return AA_CCW_COMPLIANCE_SLOPE; + if (!strcmp_P(s, PSTR("voltage"))) + return AA_PRESENT_VOLTAGE; + if (!strcmp_P(s, PSTR("temp"))) + return AA_PRESENT_TEMP; + if (!strcmp_P(s, PSTR("reginst"))) + return AA_PRESENT_REGINST; + if (!strcmp_P(s, PSTR("moving"))) + return AA_MOVING; + if (!strcmp_P(s, PSTR("lock"))) + return AA_LOCK; + + return 0; +} + +/**********************************************************/ +/* Ax12_Stress */ + +/* this structure is filled when cmd_ax12_stress is parsed successfully */ +struct cmd_ax12_stress_result { + fixed_string_t arg0; + uint8_t id; +}; + +/* function called when cmd_ax12_stress is parsed successfully */ +static void cmd_ax12_stress_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_ax12_stress_result *res = parsed_result; + int i, nb_errs, id; + uint8_t val; + microseconds t; + + t = time_get_us2(); + nb_errs = 0; + for (i=0; i<1000; i++) { + if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0) + nb_errs ++; + } + + printf_P(PSTR("%d errors / 1000\r\n"), nb_errs); + t = (time_get_us2() - t) / 1000; + printf_P(PSTR("Test done in %d ms\r\n"), (int)t); + + t = time_get_us2(); + nb_errs = 0; + for (i=0; i<1000; i++) { + if (AX12_write_int(&gen.ax12, res->id, AA_GOAL_POSITION_L, 500)) + nb_errs ++; + } + + printf_P(PSTR("%d errors / 1000\r\n"), nb_errs); + t = (time_get_us2() - t) / 1000; + printf_P(PSTR("Test done in %d ms\r\n"), (int)t); + + /* test 5 servos */ + t = time_get_us2(); + nb_errs = 0; + id = 1; + for (i=0; i<100; i++) { + if (AX12_write_int(&gen.ax12, id, AA_GOAL_POSITION_L, 500)) + nb_errs ++; + id ++; + if (id > 5) + id = 1; + } + + printf_P(PSTR("%d errors / 100\r\n"), nb_errs); + t = (time_get_us2() - t) / 1000; + printf_P(PSTR("Test done in %d ms\r\n"), (int)t); + + +} + +prog_char str_ax12_stress_arg0[] = "ax12_stress"; +parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0); +parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8); + +prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands (id)"; +parse_pgm_inst_t cmd_ax12_stress = { + .f = cmd_ax12_stress_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ax12_stress, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ax12_stress_arg0, + (prog_void *)&cmd_ax12_stress_id, + NULL, + }, +}; + +/**********************************************************/ +/* Ax12_Dump_Stats */ + +/* this structure is filled when cmd_ax12_dump_stats is parsed successfully */ +struct cmd_ax12_dump_stats_result { + fixed_string_t arg0; +}; + +/* function called when cmd_ax12_dump_stats is parsed successfully */ +static void cmd_ax12_dump_stats_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + ax12_dump_stats(); +} + +prog_char str_ax12_dump_stats_arg0[] = "ax12_dump_stats"; +parse_pgm_token_string_t cmd_ax12_dump_stats_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_dump_stats_result, arg0, str_ax12_dump_stats_arg0); + +prog_char help_ax12_dump_stats[] = "Dump AX12 stats"; +parse_pgm_inst_t cmd_ax12_dump_stats = { + .f = cmd_ax12_dump_stats_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ax12_dump_stats, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ax12_dump_stats_arg0, + NULL, + }, +}; + +/**********************************************************/ + +/* this structure is filled when cmd_baudrate is parsed successfully */ +struct cmd_baudrate_result { + fixed_string_t arg0; + uint32_t arg1; +}; + +/* function called when cmd_baudrate is parsed successfully */ +static void cmd_baudrate_parsed(void * parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_baudrate_result *res = parsed_result; + struct uart_config c; + + uart_getconf(UART_AX12_NUM, &c); + c.baudrate = res->arg1; + uart_setconf(UART_AX12_NUM, &c); +} + +prog_char str_baudrate_arg0[] = "baudrate"; +parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0); +parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32); + +prog_char help_baudrate[] = "Change ax12 baudrate"; +parse_pgm_inst_t cmd_baudrate = { + .f = cmd_baudrate_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_baudrate, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_baudrate_arg0, + (prog_void *)&cmd_baudrate_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Uint16 */ + + +/* this structure is filled when cmd_uint16_read is parsed successfully */ +struct cmd_uint16_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t num; + uint16_t val; +}; + +/* function called when cmd_uint16_read is parsed successfully */ +static void cmd_uint16_read_parsed(void * parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_uint16_result *res = parsed_result; + uint8_t ret; + uint16_t val; + uint8_t addr = addr_from_string(res->arg1); + ret = ax12_user_read_int(&gen.ax12, res->num, addr, &val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); + printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val); +} + +prog_char str_uint16_arg0[] = "read"; +parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0); +prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#" + "max_torque#down_calibration#up_calibration#torque_limit#" + "position#speed#load#punch"; +parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1); +parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8); + +prog_char help_uint16_read[] = "Read uint16 value (type, num)"; +parse_pgm_inst_t cmd_uint16_read = { + .f = cmd_uint16_read_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint16_read, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint16_arg0, + (prog_void *)&cmd_uint16_arg1, + (prog_void *)&cmd_uint16_num, + NULL, + }, +}; + +/* function called when cmd_uint16_write is parsed successfully */ +static void cmd_uint16_write_parsed(void * parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_uint16_result *res = parsed_result; + uint8_t ret; + uint8_t addr = addr_from_string(res->arg1); + printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1, + res->val, res->val); + ret = ax12_user_write_int(&gen.ax12, res->num, addr, res->val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); +} + +prog_char str_uint16_arg0_w[] = "write"; +parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w); +prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#" + "max_torque#torque_limit#punch"; +parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w); +parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16); + +prog_char help_uint16_write[] = "Write uint16 value (write, num, val)"; +parse_pgm_inst_t cmd_uint16_write = { + .f = cmd_uint16_write_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint16_write, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint16_arg0_w, + (prog_void *)&cmd_uint16_arg1_w, + (prog_void *)&cmd_uint16_num, + (prog_void *)&cmd_uint16_val, + NULL, + }, +}; + +/**********************************************************/ +/* Uint8 */ + + +/* this structure is filled when cmd_uint8_read is parsed successfully */ +struct cmd_uint8_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t num; + uint8_t val; +}; + +/* function called when cmd_uint8_read is parsed successfully */ +static void cmd_uint8_read_parsed(void * parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_uint8_result *res = parsed_result; + uint8_t ret; + uint8_t val; + uint8_t addr = addr_from_string(res->arg1); + + ret = ax12_user_read_byte(&gen.ax12, res->num, addr, &val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); + printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val); +} + +prog_char str_uint8_arg0[] = "read"; +parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0); +prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#" + "low_lim_volt#high_lim_volt#status_return#alarm_led#" + "alarm_shutdown#torque_enable#led#cw_comp_margin#" + "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#" + "voltage#temp#reginst#moving#lock"; +parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1); +parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8); + +prog_char help_uint8_read[] = "Read uint8 value (type, num)"; +parse_pgm_inst_t cmd_uint8_read = { + .f = cmd_uint8_read_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint8_read, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint8_arg0, + (prog_void *)&cmd_uint8_arg1, + (prog_void *)&cmd_uint8_num, + NULL, + }, +}; + +/* function called when cmd_uint8_write is parsed successfully */ +static void cmd_uint8_write_parsed(void * parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_uint8_result *res = parsed_result; + uint8_t addr = addr_from_string(res->arg1); + uint8_t ret; + printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1, + res->val, res->val); + ret = ax12_user_write_byte(&gen.ax12, res->num, addr, res->val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); +} + +prog_char str_uint8_arg0_w[] = "write"; +parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w); +prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#" + "low_lim_volt#high_lim_volt#status_return#alarm_led#" + "alarm_shutdown#torque_enable#led#cw_comp_margin#" + "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#" + "reginst#lock"; +parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w); +parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8); + +prog_char help_uint8_write[] = "Write uint8 value (write, num, val)"; +parse_pgm_inst_t cmd_uint8_write = { + .f = cmd_uint8_write_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint8_write, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint8_arg0_w, + (prog_void *)&cmd_uint8_arg1_w, + (prog_void *)&cmd_uint8_num, + (prog_void *)&cmd_uint8_val, + NULL, + }, +}; diff --git a/projects/microb2010/mechboard/commands_cs.c b/projects/microb2010/mechboard/commands_cs.c new file mode 100644 index 0000000..c2937f5 --- /dev/null +++ b/projects/microb2010/mechboard/commands_cs.c @@ -0,0 +1,670 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: commands_cs.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cs.h" +#include "cmdline.h" + +struct csb_list { + const prog_char *name; + struct cs_block *csb; +}; + +prog_char csb_left_arm_str[] = "left_arm"; +prog_char csb_right_arm_str[] = "right_arm"; +struct csb_list csb_list[] = { + { .name = csb_left_arm_str, .csb = &mechboard.left_arm }, + { .name = csb_right_arm_str, .csb = &mechboard.right_arm }, +}; + +struct cmd_cs_result { + fixed_string_t cmdname; + fixed_string_t csname; +}; + +/* token to be used for all cs-related commands */ +prog_char str_csb_name[] = "left_arm#right_arm"; +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) +{ + unsigned int i; + + for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) { + if (!strcmp_P(name, csb_list[i].name)) + return csb_list[i].csb; + } + return NULL; +} + +/**********************************************************/ +/* Gains for control system */ + +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_result { + struct cmd_cs_result cs; + int16_t p; + int16_t i; + int16_t d; +}; + +/* function called when cmd_gain is parsed successfully */ +static void cmd_gain_parsed(void * parsed_result, void *show) +{ + struct cmd_gain_result *res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_gains(&csb->pid, res->p, res->i, res->d); + + printf_P(PSTR("%s %s %d %d %d\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_gain_P(&csb->pid), + pid_get_gain_I(&csb->pid), + pid_get_gain_D(&csb->pid)); +} + +prog_char str_gain_arg0[] = "gain"; +parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0); +parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16); +parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16); +parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16); + +prog_char help_gain[] = "Set gain values for PID"; +parse_pgm_inst_t cmd_gain = { + .f = cmd_gain_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_gain, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_p, + (prog_void *)&cmd_gain_i, + (prog_void *)&cmd_gain_d, + NULL, + }, +}; + +/* show */ +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_gain_show_arg[] = "show"; +parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg); + +prog_char help_gain_show[] = "Show gain values for PID"; +parse_pgm_inst_t cmd_gain_show = { + .f = cmd_gain_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_gain_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Speeds for control system */ + +/* this structure is filled when cmd_speed is parsed successfully */ +struct cmd_speed_result { + struct cmd_cs_result cs; + uint16_t s; +}; + +/* function called when cmd_speed is parsed successfully */ +static void cmd_speed_parsed(void *parsed_result, __attribute__((unused)) void *show) +{ + struct cmd_speed_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + +#if notyet + if (!show) + ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */ + + printf_P(PSTR("%s %lu\r\n"), + res->cs.csname, + ext.r_b.var_pos); +#else + printf_P(PSTR("TODO\r\n")); +#endif +} + +prog_char str_speed_arg0[] = "speed"; +parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0); +parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16); + +prog_char help_speed[] = "Set speed values for ramp filter"; +parse_pgm_inst_t cmd_speed = { + .f = cmd_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_s, + NULL, + }, +}; + +/* show */ +struct cmd_speed_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg); + +prog_char help_speed_show[] = "Show speed values for ramp filter"; +parse_pgm_inst_t cmd_speed_show = { + .f = cmd_speed_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Derivate_Filters for control system */ + +/* this structure is filled when cmd_derivate_filter is parsed successfully */ +struct cmd_derivate_filter_result { + struct cmd_cs_result cs; + uint8_t size; +}; + +/* function called when cmd_derivate_filter is parsed successfully */ +static void cmd_derivate_filter_parsed(void *parsed_result, void *show) +{ + struct cmd_derivate_filter_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_derivate_filter(&csb->pid, res->size); + + printf_P(PSTR("%s %s %u\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_derivate_filter(&csb->pid)); +} + +prog_char str_derivate_filter_arg0[] = "derivate_filter"; +parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0); +parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32); + +prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)"; +parse_pgm_inst_t cmd_derivate_filter = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_derivate_filter, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_size, + NULL, + }, +}; + +/* show */ + +struct cmd_derivate_filter_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_derivate_filter_show_arg[] = "show"; +parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg); + +prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID"; +parse_pgm_inst_t cmd_derivate_filter_show = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_derivate_filter_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_show_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Consign for control system */ + +/* this structure is filled when cmd_consign is parsed successfully */ +struct cmd_consign_result { + struct cmd_cs_result cs; + int32_t p; +}; + +/* function called when cmd_consign is parsed successfully */ +static void cmd_consign_parsed(void * parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_consign_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + cs_set_consign(&csb->cs, res->p); +} + +prog_char str_consign_arg0[] = "consign"; +parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0); +parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32); + +prog_char help_consign[] = "Set consign value"; +parse_pgm_inst_t cmd_consign = { + .f = cmd_consign_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_consign, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_consign_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_consign_p, + NULL, + }, +}; + + +/**********************************************************/ +/* Maximums for control system */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_result { + struct cmd_cs_result cs; + uint32_t in; + uint32_t i; + uint32_t out; +}; + +/* function called when cmd_maximum is parsed successfully */ +static void cmd_maximum_parsed(void *parsed_result, void *show) +{ + struct cmd_maximum_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_maximums(&csb->pid, res->in, res->i, res->out); + + printf_P(PSTR("maximum %s %lu %lu %lu\r\n"), + res->cs.csname, + pid_get_max_in(&csb->pid), + pid_get_max_I(&csb->pid), + pid_get_max_out(&csb->pid)); +} + +prog_char str_maximum_arg0[] = "maximum"; +parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0); +parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32); +parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32); +parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32); + +prog_char help_maximum[] = "Set maximum values for PID (in, I, out)"; +parse_pgm_inst_t cmd_maximum = { + .f = cmd_maximum_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_maximum, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_in, + (prog_void *)&cmd_maximum_i, + (prog_void *)&cmd_maximum_out, + NULL, + }, +}; + +/* show */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; +prog_char str_maximum_show_arg[] = "show"; +parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg); + +prog_char help_maximum_show[] = "Show maximum values for PID"; +parse_pgm_inst_t cmd_maximum_show = { + .f = cmd_maximum_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_maximum_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Quadramp for control system */ + +/* this structure is filled when cmd_quadramp is parsed successfully */ +struct cmd_quadramp_result { + struct cmd_cs_result cs; + uint32_t ap; + uint32_t an; + uint32_t sp; + uint32_t sn; +}; + +/* function called when cmd_quadramp is parsed successfully */ +static void cmd_quadramp_parsed(void *parsed_result, void *show) +{ + struct cmd_quadramp_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) { + quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn); + quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); + } + + printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"), + res->cs.csname, + csb->qr.var_2nd_ord_pos, + csb->qr.var_2nd_ord_neg, + csb->qr.var_1st_ord_pos, + csb->qr.var_1st_ord_neg); +} + +prog_char str_quadramp_arg0[] = "quadramp"; +parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0); +parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32); +parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32); +parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32); +parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32); + +prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)"; +parse_pgm_inst_t cmd_quadramp = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_quadramp, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_ap, + (prog_void *)&cmd_quadramp_an, + (prog_void *)&cmd_quadramp_sp, + (prog_void *)&cmd_quadramp_sn, + + NULL, + }, +}; + +/* show */ + +struct cmd_quadramp_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_quadramp_show_arg[] = "show"; +parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg); + +prog_char help_quadramp_show[] = "Get quadramp values for control system"; +parse_pgm_inst_t cmd_quadramp_show = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_quadramp_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* cs_status show for control system */ + +/* this structure is filled when cmd_cs_status is parsed successfully */ +struct cmd_cs_status_result { + struct cmd_cs_result cs; + fixed_string_t arg; +}; + +/* function called when cmd_cs_status is parsed successfully */ +static void cmd_cs_status_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + 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); + return; + } + else if (strcmp_P(res->arg, PSTR("off")) == 0) { + csb->on = 0; + printf_P(PSTR("%s is off\r\n"), res->cs.csname); + return; + } + 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 { + 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[] = "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"; +parse_pgm_inst_t cmd_cs_status = { + .f = cmd_cs_status_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_cs_status, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_cs_status_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_cs_status_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Blocking_I for control system */ + +/* this structure is filled when cmd_blocking_i is parsed successfully */ +struct cmd_blocking_i_result { + struct cmd_cs_result cs; + int32_t k1; + int32_t k2; + uint32_t i; + uint16_t cpt; +}; + +/* function called when cmd_blocking_i is parsed successfully */ +static void cmd_blocking_i_parsed(void *parsed_result, void *show) +{ + struct cmd_blocking_i_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + bd_set_current_thresholds(&csb->bd, res->k1, res->k2, + res->i, res->cpt); + + printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"), + res->cs.cmdname, + res->cs.csname, + csb->bd.k1, + csb->bd.k2, + csb->bd.i_thres, + csb->bd.cpt_thres); +} + +prog_char str_blocking_i_arg0[] = "blocking"; +parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0); +parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32); +parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32); +parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32); +parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16); + +prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)"; +parse_pgm_inst_t cmd_blocking_i = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_blocking_i, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_k1, + (prog_void *)&cmd_blocking_i_k2, + (prog_void *)&cmd_blocking_i_i, + (prog_void *)&cmd_blocking_i_cpt, + NULL, + }, +}; + +/* show */ + +struct cmd_blocking_i_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_blocking_i_show_arg[] = "show"; +parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg); + +prog_char help_blocking_i_show[] = "Show blocking detection values"; +parse_pgm_inst_t cmd_blocking_i_show = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_blocking_i_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_show_arg, + NULL, + }, +}; + + diff --git a/projects/microb2010/mechboard/commands_gen.c b/projects/microb2010/mechboard/commands_gen.c new file mode 100644 index 0000000..6517ea0 --- /dev/null +++ b/projects/microb2010/mechboard/commands_gen.c @@ -0,0 +1,579 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: commands_gen.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "main.h" +#include "cmdline.h" +#include "sensor.h" + +/**********************************************************/ +/* Reset */ + +/* this structure is filled when cmd_reset is parsed successfully */ +struct cmd_reset_result { + fixed_string_t arg0; +}; + +/* function called when cmd_reset is parsed successfully */ +static void cmd_reset_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + reset(); +} + +prog_char str_reset_arg0[] = "reset"; +parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0); + +prog_char help_reset[] = "Reset the board"; +parse_pgm_inst_t cmd_reset = { + .f = cmd_reset_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_reset, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_reset_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Bootloader */ + +/* this structure is filled when cmd_bootloader is parsed successfully */ +struct cmd_bootloader_result { + fixed_string_t arg0; +}; + +/* function called when cmd_bootloader is parsed successfully */ +static void cmd_bootloader_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + bootloader(); +} + +prog_char str_bootloader_arg0[] = "bootloader"; +parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0); + +prog_char help_bootloader[] = "Launch the bootloader"; +parse_pgm_inst_t cmd_bootloader = { + .f = cmd_bootloader_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_bootloader, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_bootloader_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Encoders tests */ + +/* this structure is filled when cmd_encoders is parsed successfully */ +struct cmd_encoders_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_encoders is parsed successfully */ +static void cmd_encoders_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_encoders_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + encoders_spi_set_value((void *)0, 0); + encoders_spi_set_value((void *)1, 0); + encoders_spi_set_value((void *)2, 0); + encoders_spi_set_value((void *)3, 0); + return; + } + + /* show */ + while(!cmdline_keypressed()) { + printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"), + encoders_spi_get_value((void *)0), + encoders_spi_get_value((void *)1), + encoders_spi_get_value((void *)2), + encoders_spi_get_value((void *)3)); + wait_ms(100); + } +} + +prog_char str_encoders_arg0[] = "encoders"; +parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0); +prog_char str_encoders_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1); + +prog_char help_encoders[] = "Show encoders values"; +parse_pgm_inst_t cmd_encoders = { + .f = cmd_encoders_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_encoders, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_encoders_arg0, + (prog_void *)&cmd_encoders_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Scheduler show */ + +/* this structure is filled when cmd_scheduler is parsed successfully */ +struct cmd_scheduler_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_scheduler is parsed successfully */ +static void cmd_scheduler_parsed( __attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + scheduler_dump_events(); +} + +prog_char str_scheduler_arg0[] = "scheduler"; +parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0); +prog_char str_scheduler_arg1[] = "show"; +parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1); + +prog_char help_scheduler[] = "Show scheduler events"; +parse_pgm_inst_t cmd_scheduler = { + .f = cmd_scheduler_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scheduler, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scheduler_arg0, + (prog_void *)&cmd_scheduler_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Pwms tests */ + +/* this structure is filled when cmd_pwm is parsed successfully */ +struct cmd_pwm_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; +}; + +/* function called when cmd_pwm is parsed successfully */ +static void cmd_pwm_parsed(void * parsed_result, __attribute__((unused)) void *data) +{ + void * pwm_ptr = NULL; + struct cmd_pwm_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("1(4A)"))) + pwm_ptr = &gen.pwm1_4A; + else if (!strcmp_P(res->arg1, PSTR("2(4B)"))) + pwm_ptr = &gen.pwm2_4B; + else if (!strcmp_P(res->arg1, PSTR("3(1A)"))) + pwm_ptr = &gen.pwm3_1A; + else if (!strcmp_P(res->arg1, PSTR("4(1B)"))) + pwm_ptr = &gen.pwm4_1B; + + else if (!strcmp_P(res->arg1, PSTR("s1(3C)"))) + pwm_ptr = &gen.servo1; + else if (!strcmp_P(res->arg1, PSTR("s2(5A)"))) + pwm_ptr = &gen.servo2; + else if (!strcmp_P(res->arg1, PSTR("s3(5B)"))) + pwm_ptr = &gen.servo3; + else if (!strcmp_P(res->arg1, PSTR("s3(5C)"))) + pwm_ptr = &gen.servo4; + + if (pwm_ptr) + pwm_ng_set(pwm_ptr, res->arg2); + + printf_P(PSTR("done\r\n")); +} + +prog_char str_pwm_arg0[] = "pwm"; +parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0); +prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)"; +parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1); +parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16); + +prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]"; +parse_pgm_inst_t cmd_pwm = { + .f = cmd_pwm_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pwm, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pwm_arg0, + (prog_void *)&cmd_pwm_arg1, + (prog_void *)&cmd_pwm_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Adcs tests */ + +/* this structure is filled when cmd_adc is parsed successfully */ +struct cmd_adc_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_adc is parsed successfully */ +static void cmd_adc_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_adc_result *res = parsed_result; + uint8_t i, loop = 0; + + if (!strcmp_P(res->arg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("ADC values: ")); + for (i=0; iarg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("SENSOR values: ")); + for (i=0; iarg1, PSTR("level"))) { + gen.log_level = res->arg2; + } + + /* else it is a show */ + cmd_log_do_show(); +} + +prog_char str_log_arg0[] = "log"; +parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0); +prog_char str_log_arg1[] = "level"; +parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1); +parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32); + +prog_char help_log[] = "Set log options: level (0 -> 5)"; +parse_pgm_inst_t cmd_log = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1, + (prog_void *)&cmd_log_arg2, + NULL, + }, +}; + +prog_char str_log_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show); + +prog_char help_log_show[] = "Show configured logs"; +parse_pgm_inst_t cmd_log_show = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1_show, + NULL, + }, +}; + +/* this structure is filled when cmd_log is parsed successfully */ +struct cmd_log_type_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_log is parsed successfully */ +static void cmd_log_type_parsed(void * parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result; + uint8_t lognum; + uint8_t i; + + lognum = log_name2num(res->arg2); + if (lognum == 0) { + printf_P(PSTR("Cannot find log num\r\n")); + return; + } + + if (!strcmp_P(res->arg3, PSTR("on"))) { + for (i=0; iarg3, PSTR("off"))) { + for (i=0; i + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "sensor.h" +#include "cmdline.h" +#include "state.h" +#include "i2c_protocol.h" +#include "actuator.h" +#include "arm_xy.h" +#include "arm_highlevel.h" +#include "actuator.h" + +extern uint16_t state_debug; + +struct cmd_event_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + + +/* function called when cmd_event is parsed successfully */ +static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + u08 bit=0; + + struct cmd_event_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("all"))) { + bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; + if (!strcmp_P(res->arg2, PSTR("on"))) + mechboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) + mechboard.flags &= bit; + else { /* show */ + printf_P(PSTR("encoders is %s\r\n"), + (DO_ENCODERS & mechboard.flags) ? "on":"off"); + printf_P(PSTR("cs is %s\r\n"), + (DO_CS & mechboard.flags) ? "on":"off"); + printf_P(PSTR("bd is %s\r\n"), + (DO_BD & mechboard.flags) ? "on":"off"); + printf_P(PSTR("power is %s\r\n"), + (DO_POWER & mechboard.flags) ? "on":"off"); + } + return; + } + + if (!strcmp_P(res->arg1, PSTR("encoders"))) + bit = DO_ENCODERS; + else if (!strcmp_P(res->arg1, PSTR("cs"))) { + if (!strcmp_P(res->arg2, PSTR("on"))) + arm_calibrate(); + bit = DO_CS; + } + else if (!strcmp_P(res->arg1, PSTR("bd"))) + bit = DO_BD; + else if (!strcmp_P(res->arg1, PSTR("power"))) + bit = DO_POWER; + + + if (!strcmp_P(res->arg2, PSTR("on"))) + mechboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) { + if (!strcmp_P(res->arg1, PSTR("cs"))) { + pwm_ng_set(LEFT_ARM_PWM, 0); + pwm_ng_set(RIGHT_ARM_PWM, 0); + } + mechboard.flags &= (~bit); + } + printf_P(PSTR("%s is %s\r\n"), res->arg1, + (bit & mechboard.flags) ? "on":"off"); +} + +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[] = "all#encoders#cs#bd#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); + +prog_char help_event[] = "Enable/disable events"; +parse_pgm_inst_t cmd_event = { + .f = cmd_event_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_event, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Color */ + +/* this structure is filled when cmd_color is parsed successfully */ +struct cmd_color_result { + fixed_string_t arg0; + fixed_string_t color; +}; + +/* function called when cmd_color is parsed successfully */ +static void cmd_color_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; + if (!strcmp_P(res->color, PSTR("red"))) { + mechboard.our_color = I2C_COLOR_RED; + } + else if (!strcmp_P(res->color, PSTR("green"))) { + mechboard.our_color = I2C_COLOR_GREEN; + } + printf_P(PSTR("Done\r\n")); +} + +prog_char str_color_arg0[] = "color"; +parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0); +prog_char str_color_color[] = "green#red"; +parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color); + +prog_char help_color[] = "Set our color"; +parse_pgm_inst_t cmd_color = { + .f = cmd_color_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_color, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, + NULL, + }, +}; + +/**********************************************************/ +/* arm_show */ + +/* this structure is filled when cmd_arm_show is parsed successfully */ +struct cmd_arm_show_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_arm_show is parsed successfully */ +static void cmd_arm_show_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_arm_show_result *res = parsed_result; + + if (strcmp_P(res->arg1, PSTR("left")) == 0) + arm_dump(&left_arm); + else if (strcmp_P(res->arg1, PSTR("right")) == 0) + arm_dump(&right_arm); + else { + arm_dump(&left_arm); + arm_dump(&right_arm); + } +} + +prog_char str_arm_show_arg0[] = "arm"; +parse_pgm_token_string_t cmd_arm_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg0, str_arm_show_arg0); +prog_char str_arm_show_arg1[] = "left#right#both"; +parse_pgm_token_string_t cmd_arm_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg1, str_arm_show_arg1); +prog_char str_arm_show_arg2[] = "show"; +parse_pgm_token_string_t cmd_arm_show_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg2, str_arm_show_arg2); + +prog_char help_arm_show[] = "Show arm status"; +parse_pgm_inst_t cmd_arm_show = { + .f = cmd_arm_show_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_arm_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_arm_show_arg0, + (prog_void *)&cmd_arm_show_arg1, + (prog_void *)&cmd_arm_show_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* arm_goto */ + +/* this structure is filled when cmd_arm_goto is parsed successfully */ +struct cmd_arm_goto_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; + int16_t arg3; + int16_t arg4; +}; + +/* function called when cmd_arm_goto is parsed successfully */ +static void cmd_arm_goto_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_arm_goto_result *res = parsed_result; + uint8_t err; + + if (strcmp_P(res->arg1, PSTR("left")) == 0) { + arm_do_xy(&left_arm, res->arg2, res->arg3, res->arg4); + err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); + if (err != ARM_TRAJ_END) + printf_P(PSTR("err %x\r\n"), err); + } + else if (strcmp_P(res->arg1, PSTR("right")) == 0) { + arm_do_xy(&right_arm, res->arg2, res->arg3, res->arg4); + err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); + if (err != ARM_TRAJ_END) + printf_P(PSTR("err %x\r\n"), err); + } + else { + arm_do_xy(&left_arm, res->arg2, res->arg3, res->arg4); + arm_do_xy(&right_arm, res->arg2, res->arg3, res->arg4); + err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); + if (err != ARM_TRAJ_END) + printf_P(PSTR("left err %x\r\n"), err); + err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); + if (err != ARM_TRAJ_END) + printf_P(PSTR("right err %x\r\n"), err); + } +} + +prog_char str_arm_goto_arg0[] = "arm"; +parse_pgm_token_string_t cmd_arm_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg0, str_arm_goto_arg0); +prog_char str_arm_goto_arg1[] = "left#right#both"; +parse_pgm_token_string_t cmd_arm_goto_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg1, str_arm_goto_arg1); +parse_pgm_token_num_t cmd_arm_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg2, INT16); +parse_pgm_token_num_t cmd_arm_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg3, INT16); +parse_pgm_token_num_t cmd_arm_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg4, INT16); + +prog_char help_arm_goto[] = "Arm goto d_mm,h_mm,w_deg"; +parse_pgm_inst_t cmd_arm_goto = { + .f = cmd_arm_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_arm_goto, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_arm_goto_arg0, + (prog_void *)&cmd_arm_goto_arg1, + (prog_void *)&cmd_arm_goto_arg2, + (prog_void *)&cmd_arm_goto_arg3, + (prog_void *)&cmd_arm_goto_arg4, + NULL, + }, +}; + +/**********************************************************/ +/* arm_goto_fixed */ + +/* this structure is filled when cmd_arm_goto_fixed is parsed successfully */ +struct cmd_arm_goto_fixed_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_arm_goto_fixed is parsed successfully */ +static void cmd_arm_goto_fixed_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_arm_goto_fixed_result *res = parsed_result; + void (*f)(uint8_t, uint8_t) = NULL; + uint8_t err, pump_num = 0; + + if (strcmp_P(res->arg2, PSTR("prepare")) == 0) + f = arm_goto_prepare_get; + else if (strcmp_P(res->arg2, PSTR("get")) == 0) + f = arm_goto_get_column; + else if (strcmp_P(res->arg2, PSTR("inter")) == 0) + f = arm_goto_intermediate_get; + else if (strcmp_P(res->arg2, PSTR("inter")) == 0) + f = arm_goto_straight; + + if (f == NULL) + return; + + /* no matter if it's left or right here */ + if (strcmp_P(res->arg3, PSTR("p1")) == 0) + pump_num = PUMP_LEFT1_NUM; + if (strcmp_P(res->arg3, PSTR("p2")) == 0) + pump_num = PUMP_LEFT2_NUM; + + /* /!\ strcmp() inverted logic do handle "both" case */ + if (strcmp_P(res->arg1, PSTR("right"))) + f(ARM_LEFT_NUM, pump_num); + if (strcmp_P(res->arg1, PSTR("left"))) + f(ARM_RIGHT_NUM, pump_num); + + if (strcmp_P(res->arg1, PSTR("right"))) { + err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL); + if (err != ARM_TRAJ_END) + printf_P(PSTR("left err %x\r\n"), err); + } + if (strcmp_P(res->arg1, PSTR("left"))) { + err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL); + if (err != ARM_TRAJ_END) + printf_P(PSTR("right err %x\r\n"), err); + } +} + +prog_char str_arm_goto_fixed_arg0[] = "arm"; +parse_pgm_token_string_t cmd_arm_goto_fixed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg0, str_arm_goto_fixed_arg0); +prog_char str_arm_goto_fixed_arg1[] = "left#right#both"; +parse_pgm_token_string_t cmd_arm_goto_fixed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg1, str_arm_goto_fixed_arg1); +prog_char str_arm_goto_fixed_arg2[] = "prepare#get#inter#straight"; +parse_pgm_token_string_t cmd_arm_goto_fixed_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg2, str_arm_goto_fixed_arg2); +prog_char str_arm_goto_fixed_arg3[] = "p1#p2"; +parse_pgm_token_string_t cmd_arm_goto_fixed_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg3, str_arm_goto_fixed_arg3); + +prog_char help_arm_goto_fixed[] = "Goto fixed positions"; +parse_pgm_inst_t cmd_arm_goto_fixed = { + .f = cmd_arm_goto_fixed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_arm_goto_fixed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_arm_goto_fixed_arg0, + (prog_void *)&cmd_arm_goto_fixed_arg1, + (prog_void *)&cmd_arm_goto_fixed_arg2, + (prog_void *)&cmd_arm_goto_fixed_arg3, + NULL, + }, +}; + +/**********************************************************/ +/* arm_simulate */ + +/* this structure is filled when cmd_arm_simulate is parsed successfully */ +struct cmd_arm_simulate_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_arm_simulate is parsed successfully */ +static void cmd_arm_simulate_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_arm_simulate_result *res = parsed_result; + uint8_t val; + + if (strcmp_P(res->arg2, PSTR("simulate")) == 0) + val = 1; + else + val = 0; + + if (strcmp_P(res->arg1, PSTR("left")) == 0) + left_arm.config.simulate = 1; + else if (strcmp_P(res->arg1, PSTR("right")) == 0) + right_arm.config.simulate = 1; + else { + left_arm.config.simulate = 1; + right_arm.config.simulate = 1; + } +} + +prog_char str_arm_simulate_arg0[] = "arm"; +parse_pgm_token_string_t cmd_arm_simulate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg0, str_arm_simulate_arg0); +prog_char str_arm_simulate_arg1[] = "left#right#both"; +parse_pgm_token_string_t cmd_arm_simulate_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg1, str_arm_simulate_arg1); +prog_char str_arm_simulate_arg2[] = "simulate#real"; +parse_pgm_token_string_t cmd_arm_simulate_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg2, str_arm_simulate_arg2); + +prog_char help_arm_simulate[] = "Simulation or real for arm"; +parse_pgm_inst_t cmd_arm_simulate = { + .f = cmd_arm_simulate_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_arm_simulate, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_arm_simulate_arg0, + (prog_void *)&cmd_arm_simulate_arg1, + (prog_void *)&cmd_arm_simulate_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* finger */ + +/* this structure is filled when cmd_finger is parsed successfully */ +struct cmd_finger_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_finger is parsed successfully */ +static void cmd_finger_parsed(void *parsed_result, __attribute__((unused)) void *data) +{ + struct cmd_finger_result *res = parsed_result; + uint16_t dest = 0; + + if (strcmp_P(res->arg1, PSTR("left")) == 0) + dest = FINGER_LEFT; + else if (strcmp_P(res->arg1, PSTR("right")) == 0) + dest = FINGER_RIGHT; + else if (strcmp_P(res->arg1, PSTR("center")) == 0) + dest = FINGER_CENTER; + finger_goto(dest); +} + +prog_char str_finger_arg0[] = "finger"; +parse_pgm_token_string_t cmd_finger_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_finger_result, arg0, str_finger_arg0); +prog_char str_finger_arg1[] = "left#right#center"; +parse_pgm_token_string_t cmd_finger_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_finger_result, arg1, str_finger_arg1); + +prog_char help_finger[] = "Move finger"; +parse_pgm_inst_t cmd_finger = { + .f = cmd_finger_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_finger, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_finger_arg0, + (prog_void *)&cmd_finger_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* pump */ + +/* this structure is filled when cmd_pump is parsed successfully */ +struct cmd_pump_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_pump is parsed successfully */ +static void cmd_pump_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_pump_result *res = parsed_result; + int8_t pump_num = 0; + int16_t val = 0; + + if (strcmp_P(res->arg1, PSTR("left1")) == 0) + pump_num = PUMP_LEFT1_NUM; + else if (strcmp_P(res->arg1, PSTR("right1")) == 0) + pump_num = PUMP_RIGHT1_NUM; + else if (strcmp_P(res->arg1, PSTR("left2")) == 0) + pump_num = PUMP_LEFT2_NUM; + else if (strcmp_P(res->arg1, PSTR("right2")) == 0) + pump_num = PUMP_RIGHT2_NUM; + + if (strcmp_P(res->arg2, PSTR("on")) == 0) + val = PUMP_ON; + else if (strcmp_P(res->arg2, PSTR("off")) == 0) + val = PUMP_OFF; + else if (strcmp_P(res->arg2, PSTR("reverse")) == 0) + val = PUMP_REVERSE; + + pump_set(pump_num, val); +} + +prog_char str_pump_arg0[] = "pump"; +parse_pgm_token_string_t cmd_pump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg0, str_pump_arg0); +prog_char str_pump_arg1[] = "left1#left2#right1#right2"; +parse_pgm_token_string_t cmd_pump_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg1, str_pump_arg1); +prog_char str_pump_arg2[] = "on#off#reverse"; +parse_pgm_token_string_t cmd_pump_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg2, str_pump_arg2); + +prog_char help_pump[] = "activate pump"; +parse_pgm_inst_t cmd_pump = { + .f = cmd_pump_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pump, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pump_arg0, + (prog_void *)&cmd_pump_arg1, + (prog_void *)&cmd_pump_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* State1 */ + +/* this structure is filled when cmd_state1 is parsed successfully */ +struct cmd_state1_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_state1 is parsed successfully */ +static void cmd_state1_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state1_result *res = parsed_result; + struct i2c_cmd_mechboard_set_mode command; + + if (!strcmp_P(res->arg1, PSTR("init"))) { + state_init(); + return; + } + + if (!strcmp_P(res->arg1, PSTR("manual"))) + command.mode = I2C_MECHBOARD_MODE_MANUAL; + else if (!strcmp_P(res->arg1, PSTR("harvest"))) + command.mode = I2C_MECHBOARD_MODE_HARVEST; + else if (!strcmp_P(res->arg1, PSTR("lazy_harvest"))) + command.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; + else if (!strcmp_P(res->arg1, PSTR("pickup"))) + command.mode = I2C_MECHBOARD_MODE_PICKUP; + else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel"))) + command.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("get_lintel"))) + command.mode = I2C_MECHBOARD_MODE_GET_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("put_lintel"))) + command.mode = I2C_MECHBOARD_MODE_PUT_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("clear"))) + command.mode = I2C_MECHBOARD_MODE_CLEAR; + else if (!strcmp_P(res->arg1, PSTR("loaded"))) + command.mode = I2C_MECHBOARD_MODE_LOADED; + else if (!strcmp_P(res->arg1, PSTR("store"))) + command.mode = I2C_MECHBOARD_MODE_STORE; + else if (!strcmp_P(res->arg1, PSTR("lazy_pickup"))) + command.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; + state_set_mode(&command); +} + +prog_char str_state1_arg0[] = "mechboard"; +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#pickup#prepare_get_lintel#get_lintel#put_lintel#clear#lazy_harvest#harvest#loaded#store#lazy_pickup"; +parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1); + +prog_char help_state1[] = "set mechboard mode"; +parse_pgm_inst_t cmd_state1 = { + .f = cmd_state1_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state1_arg0, + (prog_void *)&cmd_state1_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* State2 */ + +/* this structure is filled when cmd_state2 is parsed successfully */ +struct cmd_state2_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_state2 is parsed successfully */ +static void cmd_state2_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state2_result *res = parsed_result; + struct i2c_cmd_mechboard_set_mode command; + uint8_t side; + + if (!strcmp_P(res->arg2, PSTR("left"))) + side = I2C_LEFT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("right"))) + side = I2C_RIGHT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("center"))) + side = I2C_CENTER_SIDE; + else + side = I2C_AUTO_SIDE; + + if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) { + command.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; + command.prep_pickup.side = side; + command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; + } + else if (!strcmp_P(res->arg1, PSTR("push_temple_disc"))) { + command.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC; + command.push_temple_disc.side = side; + } + + + state_set_mode(&command); +} + +prog_char str_state2_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0); +prog_char str_state2_arg1[] = "prepare_pickup#push_temple_disc"; +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#auto#center"; +parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2); + +prog_char help_state2[] = "set mechboard mode"; +parse_pgm_inst_t cmd_state2 = { + .f = cmd_state2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state2_arg0, + (prog_void *)&cmd_state2_arg1, + (prog_void *)&cmd_state2_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* State3 */ + +/* this structure is filled when cmd_state3 is parsed successfully */ +struct cmd_state3_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t level; +}; + +/* function called when cmd_state3 is parsed successfully */ +static void cmd_state3_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state3_result *res = parsed_result; + struct i2c_cmd_mechboard_set_mode command; + + if (!strcmp_P(res->arg1, PSTR("prepare_build"))) { + command.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD; + command.prep_build.level_l = res->level; + command.prep_build.level_r = res->level; + } + else if (!strcmp_P(res->arg1, PSTR("prepare_inside"))) { + command.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE; + command.prep_inside.level_l = res->level; + command.prep_inside.level_r = res->level; + } + else if (!strcmp_P(res->arg1, PSTR("autobuild"))) { + command.mode = I2C_MECHBOARD_MODE_AUTOBUILD; + command.autobuild.level_left = res->level; + command.autobuild.level_right = res->level; + command.autobuild.count_left = 2; + command.autobuild.count_right = 2; + command.autobuild.distance_left = I2C_AUTOBUILD_DEFAULT_DIST; + command.autobuild.distance_right = I2C_AUTOBUILD_DEFAULT_DIST; + command.autobuild.do_lintel = 1; + } + else if (!strcmp_P(res->arg1, PSTR("push_temple"))) { + command.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE; + command.push_temple.level = res->level; + } + state_set_mode(&command); +} + +prog_char str_state3_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0); +prog_char str_state3_arg1[] = "prepare_build#autobuild#prepare_inside"; +parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1); +parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, level, UINT8); + +prog_char help_state3[] = "set mechboard mode"; +parse_pgm_inst_t cmd_state3 = { + .f = cmd_state3_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state3, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state3_arg0, + (prog_void *)&cmd_state3_arg1, + (prog_void *)&cmd_state3_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* State4 */ + +/* this structure is filled when cmd_state4 is parsed successfully */ +struct cmd_state4_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t level_l; + uint8_t count_l; + uint8_t dist_l; + uint8_t level_r; + uint8_t count_r; + uint8_t dist_r; + uint8_t do_lintel; +}; + +/* function called when cmd_state4 is parsed successfully */ +static void cmd_state4_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state4_result *res = parsed_result; + struct i2c_cmd_mechboard_set_mode command; + + if (!strcmp_P(res->arg1, PSTR("autobuild"))) { + command.mode = I2C_MECHBOARD_MODE_AUTOBUILD; + command.autobuild.distance_left = res->dist_l; + command.autobuild.distance_right = res->dist_r; + command.autobuild.level_left = res->level_l; + command.autobuild.level_right = res->level_r; + command.autobuild.count_left = res->count_l; + command.autobuild.count_right = res->count_r; + command.autobuild.do_lintel = res->do_lintel; + } + state_set_mode(&command); +} + +prog_char str_state4_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_state4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state4_result, arg0, str_state4_arg0); +prog_char str_state4_arg1[] = "autobuild"; +parse_pgm_token_string_t cmd_state4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state4_result, arg1, str_state4_arg1); +parse_pgm_token_num_t cmd_state4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, level_l, UINT8); +parse_pgm_token_num_t cmd_state4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, count_l, UINT8); +parse_pgm_token_num_t cmd_state4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, dist_l, UINT8); +parse_pgm_token_num_t cmd_state4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, level_r, UINT8); +parse_pgm_token_num_t cmd_state4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, count_r, UINT8); +parse_pgm_token_num_t cmd_state4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, dist_r, UINT8); +parse_pgm_token_num_t cmd_state4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, do_lintel, UINT8); + +prog_char help_state4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)"; +parse_pgm_inst_t cmd_state4 = { + .f = cmd_state4_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state4, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state4_arg0, + (prog_void *)&cmd_state4_arg1, + (prog_void *)&cmd_state4_arg2, + (prog_void *)&cmd_state4_arg3, + (prog_void *)&cmd_state4_arg4, + (prog_void *)&cmd_state4_arg5, + (prog_void *)&cmd_state4_arg6, + (prog_void *)&cmd_state4_arg7, + (prog_void *)&cmd_state4_arg8, + NULL, + }, +}; + +/**********************************************************/ +/* State5 */ + +/* this structure is filled when cmd_state5 is parsed successfully */ +struct cmd_state5_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_state5 is parsed successfully */ +static void cmd_state5_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state5_result *res = parsed_result; + struct i2c_cmd_mechboard_set_mode command; + uint8_t side; + + if (!strcmp_P(res->arg2, PSTR("left"))) + side = I2C_LEFT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("right"))) + side = I2C_RIGHT_SIDE; + else if (!strcmp_P(res->arg2, PSTR("center"))) + side = I2C_CENTER_SIDE; + else + side = I2C_AUTO_SIDE; + + command.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP; + command.prep_pickup.side = side; + + if (!strcmp_P(res->arg3, PSTR("harvest"))) + command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_HARVEST; + else if (!strcmp_P(res->arg3, PSTR("lazy_harvest"))) + command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST; + else if (!strcmp_P(res->arg3, PSTR("pickup"))) + command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PICKUP; + else if (!strcmp_P(res->arg3, PSTR("clear"))) + command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_CLEAR; + else if (!strcmp_P(res->arg3, PSTR("store"))) + command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_STORE; + else if (!strcmp_P(res->arg3, PSTR("lazy_pickup"))) + command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP; + + state_set_mode(&command); +} + +prog_char str_state5_arg0[] = "mechboard"; +parse_pgm_token_string_t cmd_state5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg0, str_state5_arg0); +prog_char str_state5_arg1[] = "prepare_pickup"; +parse_pgm_token_string_t cmd_state5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg1, str_state5_arg1); +prog_char str_state5_arg2[] = "left#right#auto#center"; +parse_pgm_token_string_t cmd_state5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg2, str_state5_arg2); +prog_char str_state5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear"; +parse_pgm_token_string_t cmd_state5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg3, str_state5_arg3); + +prog_char help_state5[] = "set mechboard mode 2"; +parse_pgm_inst_t cmd_state5 = { + .f = cmd_state5_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state5, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state5_arg0, + (prog_void *)&cmd_state5_arg1, + (prog_void *)&cmd_state5_arg2, + (prog_void *)&cmd_state5_arg3, + NULL, + }, +}; + +/**********************************************************/ +/* State_Machine */ + +/* this structure is filled when cmd_state_machine is parsed successfully */ +struct cmd_state_machine_result { + fixed_string_t arg0; +}; + +/* function called when cmd_state_machine is parsed successfully */ +static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + state_machine(); +} + +prog_char str_state_machine_arg0[] = "state_machine"; +parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0); + +prog_char help_state_machine[] = "launch state machine"; +parse_pgm_inst_t cmd_state_machine = { + .f = cmd_state_machine_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state_machine, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state_machine_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* State_Debug */ + +/* this structure is filled when cmd_state_debug is parsed successfully */ +struct cmd_state_debug_result { + fixed_string_t arg0; + uint8_t on; +}; + +/* function called when cmd_state_debug is parsed successfully */ +static void cmd_state_debug_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_state_debug_result *res = parsed_result; + state_debug = res->on; +} + +prog_char str_state_debug_arg0[] = "state_debug"; +parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0); +parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8); + +prog_char help_state_debug[] = "Set debug timer for state machine"; +parse_pgm_inst_t cmd_state_debug = { + .f = cmd_state_debug_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_state_debug, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_state_debug_arg0, + (prog_void *)&cmd_state_debug_on, + NULL, + }, +}; + +/**********************************************************/ +/* Servo_Lintel */ + +/* this structure is filled when cmd_servo_lintel is parsed successfully */ +struct cmd_servo_lintel_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_servo_lintel is parsed successfully */ +static void cmd_servo_lintel_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_servo_lintel_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("out"))) + servo_lintel_out(); + else if (!strcmp_P(res->arg1, PSTR("1lin"))) + servo_lintel_1lin(); + else if (!strcmp_P(res->arg1, PSTR("2lin"))) + servo_lintel_2lin(); + +} + +prog_char str_servo_lintel_arg0[] = "servo_lintel"; +parse_pgm_token_string_t cmd_servo_lintel_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_lintel_result, arg0, str_servo_lintel_arg0); +prog_char str_servo_lintel_arg1[] = "out#1lin#2lin"; +parse_pgm_token_string_t cmd_servo_lintel_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_lintel_result, arg1, str_servo_lintel_arg1); + +prog_char help_servo_lintel[] = "Servo_Lintel function"; +parse_pgm_inst_t cmd_servo_lintel = { + .f = cmd_servo_lintel_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_servo_lintel, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_servo_lintel_arg0, + (prog_void *)&cmd_servo_lintel_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Pump_Current */ + +/* this structure is filled when cmd_pump_current is parsed successfully */ +struct cmd_pump_current_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_pump_current is parsed successfully */ +static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"), + mechboard.pump_left1_current, mechboard.pump_left2_current, + sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4)); +} + +prog_char str_pump_current_arg0[] = "pump_current"; +parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0); +prog_char str_pump_current_arg1[] = "show"; +parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1); + +prog_char help_pump_current[] = "dump pump current"; +parse_pgm_inst_t cmd_pump_current = { + .f = cmd_pump_current_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pump_current, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pump_current_arg0, + (prog_void *)&cmd_pump_current_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Manivelle */ + +/* this structure is filled when cmd_manivelle is parsed successfully */ +struct cmd_manivelle_result { + fixed_string_t arg0; + uint8_t step; +}; + +/* function called when cmd_manivelle is parsed successfully */ +static void cmd_manivelle_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_manivelle_result *res = parsed_result; + state_manivelle(res->step); +} + +prog_char str_manivelle_arg0[] = "manivelle"; +parse_pgm_token_string_t cmd_manivelle_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_manivelle_result, arg0, str_manivelle_arg0); +parse_pgm_token_num_t cmd_manivelle_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_manivelle_result, step, UINT8); + +prog_char help_manivelle[] = "Manivelle function"; +parse_pgm_inst_t cmd_manivelle = { + .f = cmd_manivelle_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_manivelle, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_manivelle_arg0, + (prog_void *)&cmd_manivelle_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Test */ + +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_test is parsed successfully */ +static void cmd_test_parsed(__attribute__((unused)) void *parsed_result, + __attribute__((unused)) void *data) +{ +} + +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); + +prog_char help_test[] = "Test function"; +parse_pgm_inst_t cmd_test = { + .f = cmd_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_test_arg0, + NULL, + }, +}; diff --git a/projects/microb2010/mechboard/cs.c b/projects/microb2010/mechboard/cs.c new file mode 100644 index 0000000..29a2cab --- /dev/null +++ b/projects/microb2010/mechboard/cs.c @@ -0,0 +1,163 @@ +/* + * 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 + * (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: cs.c,v 1.4 2009-04-24 19:30:42 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "actuator.h" + +/* called every 5 ms */ +static void do_cs(__attribute__((unused)) void *dummy) +{ + /* read encoders */ + if (mechboard.flags & DO_ENCODERS) { + encoders_spi_manage(NULL); + } + /* control system */ + if (mechboard.flags & DO_CS) { + if (mechboard.left_arm.on) + cs_manage(&mechboard.left_arm.cs); + if (mechboard.right_arm.on) + cs_manage(&mechboard.right_arm.cs); + } + if (mechboard.flags & DO_BD) { + bd_manage_from_cs(&mechboard.left_arm.bd, &mechboard.left_arm.cs); + bd_manage_from_cs(&mechboard.right_arm.bd, &mechboard.right_arm.cs); + } + if (mechboard.flags & DO_POWER) + BRAKE_OFF(); + else + BRAKE_ON(); +} + +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", + 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_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) +{ + /* ---- CS left_arm */ + /* PID */ + pid_init(&mechboard.left_arm.pid); + pid_set_gains(&mechboard.left_arm.pid, 500, 40, 5000); + pid_set_maximums(&mechboard.left_arm.pid, 0, 5000, 2400); /* max is 12 V */ + pid_set_out_shift(&mechboard.left_arm.pid, 10); + pid_set_derivate_filter(&mechboard.left_arm.pid, 4); + + /* QUADRAMP */ + quadramp_init(&mechboard.left_arm.qr); + quadramp_set_1st_order_vars(&mechboard.left_arm.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&mechboard.left_arm.qr, 20, 20); /* set accel */ + + /* CS */ + cs_init(&mechboard.left_arm.cs); + cs_set_consign_filter(&mechboard.left_arm.cs, quadramp_do_filter, &mechboard.left_arm.qr); + cs_set_correct_filter(&mechboard.left_arm.cs, pid_do_filter, &mechboard.left_arm.pid); + cs_set_process_in(&mechboard.left_arm.cs, pwm_ng_set, LEFT_ARM_PWM); + cs_set_process_out(&mechboard.left_arm.cs, encoders_spi_get_value, LEFT_ARM_ENCODER); + cs_set_consign(&mechboard.left_arm.cs, 0); + + /* Blocking detection */ + bd_init(&mechboard.left_arm.bd); + bd_set_speed_threshold(&mechboard.left_arm.bd, 150); + bd_set_current_thresholds(&mechboard.left_arm.bd, 500, 8000, 1000000, 40); + + /* ---- CS right_arm */ + /* PID */ + pid_init(&mechboard.right_arm.pid); + pid_set_gains(&mechboard.right_arm.pid, 500, 40, 5000); + pid_set_maximums(&mechboard.right_arm.pid, 0, 5000, 2400); /* max is 12 V */ + pid_set_out_shift(&mechboard.right_arm.pid, 10); + pid_set_derivate_filter(&mechboard.right_arm.pid, 6); + + /* QUADRAMP */ + quadramp_init(&mechboard.right_arm.qr); + quadramp_set_1st_order_vars(&mechboard.right_arm.qr, 1000, 1000); /* set speed */ + quadramp_set_2nd_order_vars(&mechboard.right_arm.qr, 20, 20); /* set accel */ + + /* CS */ + cs_init(&mechboard.right_arm.cs); + cs_set_consign_filter(&mechboard.right_arm.cs, quadramp_do_filter, &mechboard.right_arm.qr); + cs_set_correct_filter(&mechboard.right_arm.cs, pid_do_filter, &mechboard.right_arm.pid); + cs_set_process_in(&mechboard.right_arm.cs, pwm_ng_set, RIGHT_ARM_PWM); + cs_set_process_out(&mechboard.right_arm.cs, encoders_spi_get_value, RIGHT_ARM_ENCODER); + cs_set_consign(&mechboard.right_arm.cs, 0); + + /* Blocking detection */ + bd_init(&mechboard.right_arm.bd); + bd_set_speed_threshold(&mechboard.right_arm.bd, 150); + bd_set_current_thresholds(&mechboard.right_arm.bd, 500, 8000, 1000000, 40); + + /* set them on !! */ + mechboard.left_arm.on = 1; + mechboard.right_arm.on = 1; + + + scheduler_add_periodical_event_priority(do_cs, NULL, + CS_PERIOD / SCHEDULER_UNIT, + CS_PRIO); +} diff --git a/projects/microb2010/mechboard/cs.h b/projects/microb2010/mechboard/cs.h new file mode 100644 index 0000000..5b3be83 --- /dev/null +++ b/projects/microb2010/mechboard/cs.h @@ -0,0 +1,25 @@ +/* + * 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 + * (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: cs.h,v 1.1 2009-03-05 22:52:35 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); diff --git a/projects/microb2010/mechboard/diagnostic_config.h b/projects/microb2010/mechboard/diagnostic_config.h new file mode 100644 index 0000000..7302bfa --- /dev/null +++ b/projects/microb2010/mechboard/diagnostic_config.h @@ -0,0 +1,44 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: diagnostic_config.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $ + * + */ + +#ifndef _DEBUG_CONFIG_ +#define _DEBUG_CONFIG_ 1.0 // version + + +/** port line definition for the show_int_loop() function */ +/* undefine it to disable this functionnality */ +#define INTERRUPT_SHOW_PORT PORTA +#define INTERRUPT_SHOW_BIT 3 + + + +/** memory mark for the min_stack_space_available() function + the ram is filled with this value after a reset ! */ +#define MARK 0x55 + +/** the mark is inserted in whole RAM if this is enabled + (could lead to problems if you need to hold values through a reset...) + so it's better to disable it. + stack counting is not affected */ +//#define DIAG_FILL_ENTIRE_RAM + + +#endif //_DEBUG_CONFIG_ diff --git a/projects/microb2010/mechboard/encoders_spi_config.h b/projects/microb2010/mechboard/encoders_spi_config.h new file mode 100644 index 0000000..e71e662 --- /dev/null +++ b/projects/microb2010/mechboard/encoders_spi_config.h @@ -0,0 +1,33 @@ +/* + * 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 + * (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: encoders_spi_config.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $ + * + */ +#ifndef _ENCODERS_SPI_CONFIG_H_ +#define _ENCODERS_SPI_CONFIG_H_ + +#define ENCODERS_SPI_NUMBER 4 +#define ENCODERS_SPI_SS_PORT SS_PORT /* PORTB on atmega2560 */ +#define ENCODERS_SPI_SS_BIT SS_BIT /* 0 on atmega2560 */ + +/* see spi configuration */ +#define ENCODERS_SPI_CLK_RATE SPI_CLK_RATE_16 +#define ENCODERS_SPI_FORMAT SPI_FORMAT_3 +#define ENCODERS_SPI_DATA_ORDER SPI_LSB_FIRST + +#endif diff --git a/projects/microb2010/mechboard/error_config.h b/projects/microb2010/mechboard/error_config.h new file mode 100644 index 0000000..2df45d0 --- /dev/null +++ b/projects/microb2010/mechboard/error_config.h @@ -0,0 +1,31 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: error_config.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $ + * + */ + +#ifndef _ERROR_CONFIG_ +#define _ERROR_CONFIG_ + +/** enable the dump of the comment */ +#define ERROR_DUMP_TEXTLOG + +/** enable the dump of filename and line number */ +#define ERROR_DUMP_FILE_LINE + +#endif diff --git a/projects/microb2010/mechboard/i2c_config.h b/projects/microb2010/mechboard/i2c_config.h new file mode 100644 index 0000000..93ce0ec --- /dev/null +++ b/projects/microb2010/mechboard/i2c_config.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: i2c_config.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $ + * + */ + + +#define I2C_BITRATE 1 // divider dor i2c baudrate, see TWBR in doc +#define I2C_PRESCALER 3 // prescaler config, rate = 2^(n*2) + +/* Size of transmission buffer */ +#define I2C_SEND_BUFFER_SIZE 32 + +/* Size of reception buffer */ +#define I2C_RECV_BUFFER_SIZE 32 diff --git a/projects/microb2010/mechboard/i2c_protocol.c b/projects/microb2010/mechboard/i2c_protocol.c new file mode 100644 index 0000000..902207b --- /dev/null +++ b/projects/microb2010/mechboard/i2c_protocol.c @@ -0,0 +1,211 @@ +/* + * 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 + * (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: i2c_protocol.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "sensor.h" +#include "state.h" + +void i2c_protocol_init(void) +{ +} + +/*** LED CONTROL ***/ +void i2c_led_control(uint8_t l, uint8_t state) +{ + switch(l) { + case 1: + state? LED1_ON():LED1_OFF(); + break; + case 2: + state? LED2_ON():LED2_OFF(); + break; + case 3: + state? LED3_ON():LED3_OFF(); + break; + case 4: + state? LED4_ON():LED4_OFF(); + break; + default: + break; + } +} + +#ifdef notyet +static void i2c_test(uint16_t val) +{ + static uint16_t prev=0; + + if ( (val-prev) != 1 ) { + WARNING(E_USER_I2C_PROTO, "Dupplicated msg %d", val); + } + prev = val; + ext.nb_test_cmd ++; +} +#endif + +static void i2c_send_status(void) +{ + struct i2c_ans_mechboard_status ans; + i2c_flush(); + ans.hdr.cmd = I2C_ANS_MECHBOARD_STATUS; + + /* status */ + ans.mode = state_get_mode(); + ans.status = mechboard.status; + + ans.lintel_count = mechboard.lintel_count; + ans.column_flags = mechboard.column_flags; + /* pumps pwm */ + ans.pump_left1 = mechboard.pump_left1; + ans.pump_right1 = mechboard.pump_right1; + ans.pump_left2 = mechboard.pump_left2; + ans.pump_right2 = mechboard.pump_right2; + /* pumps current */ + ans.pump_right1_current = sensor_get_adc(ADC_CSENSE3); + ans.pump_right2_current = sensor_get_adc(ADC_CSENSE4); + /* servos */ + ans.servo_lintel_left = mechboard.servo_lintel_left; + ans.servo_lintel_right = mechboard.servo_lintel_right; + + i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, + sizeof(ans), I2C_CTRL_GENERIC); +} + +static int8_t i2c_set_mode(struct i2c_cmd_mechboard_set_mode *cmd) +{ + state_set_mode(cmd); + return 0; +} + +void i2c_recvevent(uint8_t * buf, int8_t size) +{ + void *void_cmd = buf; + + static uint8_t a = 0; + + a++; + if (a & 0x10) + LED2_TOGGLE(); + + if (size <= 0) { + goto error; + } + + switch (buf[0]) { + + /* Commands (no answer needed) */ + case I2C_CMD_GENERIC_LED_CONTROL: + { + struct i2c_cmd_led_control *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + i2c_led_control(cmd->led_num, cmd->state); + break; + } + + case I2C_CMD_MECHBOARD_SET_MODE: + { + struct i2c_cmd_mechboard_set_mode *cmd = void_cmd; + if (size != sizeof(struct i2c_cmd_mechboard_set_mode)) + goto error; + i2c_set_mode(cmd); + break; + } + + case I2C_CMD_GENERIC_SET_COLOR: + { + struct i2c_cmd_generic_color *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + mechboard.our_color = cmd->color; + break; + } + +#ifdef notyet + case I2C_CMD_EXTENSION_TEST: + { + struct i2c_cmd_extension_test *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + i2c_test(cmd->val); + break; + } +#endif + + /* Add other commands here ...*/ + + + case I2C_REQ_MECHBOARD_STATUS: + { + struct i2c_req_mechboard_status *cmd = void_cmd; + if (size != sizeof (struct i2c_req_mechboard_status)) + goto error; + + mechboard.pump_left1_current = cmd->pump_left1_current; + mechboard.pump_left2_current = cmd->pump_left2_current; + i2c_send_status(); + break; + } + + default: + goto error; + } + + error: + /* log error on a led ? */ + return; +} + +void i2c_recvbyteevent(__attribute__((unused)) uint8_t hwstatus, + __attribute__((unused)) uint8_t i, + __attribute__((unused)) int8_t c) +{ +} + +void i2c_sendevent(__attribute__((unused)) int8_t size) +{ +} + + diff --git a/projects/microb2010/mechboard/i2c_protocol.h b/projects/microb2010/mechboard/i2c_protocol.h new file mode 100644 index 0000000..7f022ef --- /dev/null +++ b/projects/microb2010/mechboard/i2c_protocol.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation (2007) + * + * 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: i2c_protocol.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $ + * + */ + +#include + +void i2c_protocol_init(void); + +void i2c_recvevent(uint8_t * buf, int8_t size); +void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c); +void i2c_sendevent(int8_t size); + +int debug_send(char c, FILE* f); diff --git a/projects/microb2010/mechboard/main.c b/projects/microb2010/mechboard/main.c new file mode 100755 index 0000000..27c0138 --- /dev/null +++ b/projects/microb2010/mechboard/main.c @@ -0,0 +1,269 @@ +/* + * 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 + * (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: main.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "../common/eeprom_mapping.h" +#include "../common/i2c_commands.h" + +#include "main.h" +#include "ax12_user.h" +#include "cmdline.h" +#include "sensor.h" +#include "state.h" +#include "actuator.h" +#include "arm_xy.h" +#include "cs.h" +#include "i2c_protocol.h" + +/* 0 means "programmed" + * ---- with 16 Mhz quartz + * CKSEL 3-0 : 0111 + * SUT 1-0 : 10 + * CKDIV8 : 1 + * ---- bootloader + * BOOTZ 1-0 : 01 (4K bootloader) + * BOOTRST : 0 (reset on bootloader) + * ---- jtag + * jtagen : 0 + */ + +struct genboard gen; +struct mechboard mechboard; + +/***********************/ + +void bootloader(void) +{ +#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; + TIMSK2 = 0; + TIMSK3 = 0; + TIMSK4 = 0; + TIMSK5 = 0; + EIMSK = 0; + UCSR0B = 0; + UCSR1B = 0; + UCSR2B = 0; + UCSR3B = 0; + SPCR = 0; + TWCR = 0; + ACSR = 0; + ADCSRA = 0; + + EIND = 1; + __asm__ __volatile__ ("ldi r31,0xf8\n"); + __asm__ __volatile__ ("ldi r30,0x00\n"); + __asm__ __volatile__ ("eijmp\n"); + + /* never returns */ +} + +void do_led_blink(__attribute__((unused)) void *dummy) +{ +#if 1 /* simple blink */ + static uint8_t a=0; + + if(a) + LED1_ON(); + else + LED1_OFF(); + + a = !a; +#endif +} + +static void main_timer_interrupt(void) +{ + static uint8_t cpt = 0; + cpt++; + sei(); + if ((cpt & 0x3) == 0) + scheduler_interrupt(); +} + +int main(void) +{ + /* brake */ + BRAKE_ON(); + BRAKE_DDR(); + + /* CPLD reset on PG3 */ + DDRG |= 1<<3; + PORTG &= ~(1<<3); /* implicit */ + + /* LEDS */ + DDRJ |= 0x0c; + DDRL = 0xc0; + LED1_OFF(); + memset(&gen, 0, sizeof(gen)); + memset(&mechboard, 0, sizeof(mechboard)); + /* cs is enabled after arm_calibrate() */ + mechboard.flags = DO_ENCODERS | DO_POWER; // DO_BD + + /* UART */ + uart_init(); +#if CMDLINE_UART == 3 + fdevopen(uart3_dev_send, uart3_dev_recv); + uart_register_rx_event(3, emergency); +#elif CMDLINE_UART == 1 + fdevopen(uart1_dev_send, uart1_dev_recv); + uart_register_rx_event(1, emergency); +#else +# error not supported +#endif + + //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MECHBOARD); + /* check eeprom to avoid to run the bad program */ + if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != + EEPROM_MAGIC_MECHBOARD) { + sei(); + printf_P(PSTR("Bad eeprom value\r\n")); + while(1); + } + + /* LOGS */ + error_register_emerg(mylog); + error_register_error(mylog); + error_register_warning(mylog); + error_register_notice(mylog); + error_register_debug(mylog); + + /* SPI + ENCODERS */ + encoders_spi_init(); /* this will also init spi hardware */ + + /* I2C */ + i2c_protocol_init(); + i2c_init(I2C_MODE_SLAVE, I2C_MECHBOARD_ADDR); + i2c_register_recv_event(i2c_recvevent); + + /* TIMER */ + timer_init(); + timer0_register_OV_intr(main_timer_interrupt); + + /* PWM */ + 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, + TIMER4_PRESCALER_DIV_1); + + PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED, + &PORTD, 4); + PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED, + &PORTD, 5); + PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED | + PWM_NG_MODE_SIGN_INVERTED, + &PORTD, 6); + PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED | + PWM_NG_MODE_SIGN_INVERTED, + &PORTD, 7); + + + /* servos */ + 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, + TIMER1_PRESCALER_DIV_256); + PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL, + NULL, 0); + PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL, + 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, + LED_PRIO); + /* all cs management */ + microb_cs_init(); + + /* sensors, will also init hardware adc */ + sensor_init(); + + /* TIME */ + time_init(TIME_PRIO); + + /* ax12 */ + ax12_user_init(); + + sei(); + + /* finger + other actuators */ + actuator_init(); + + state_init(); + + printf_P(PSTR("\r\n")); + printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n")); + + /* arm management */ + gen.logs[0] = E_USER_ST_MACH; + gen.log_level = 5; + arm_init(); + mechboard.flags |= DO_CS; + arm_do_xy(&left_arm, 60, -80, 0); + arm_do_xy(&right_arm, 60, -80, 0); + + state_machine(); + cmdline_interact(); + + return 0; +} diff --git a/projects/microb2010/mechboard/main.h b/projects/microb2010/mechboard/main.h new file mode 100755 index 0000000..5422f6c --- /dev/null +++ b/projects/microb2010/mechboard/main.h @@ -0,0 +1,174 @@ +/* + * 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 + * (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: main.h,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#define LED_TOGGLE(port, bit) do { \ + if (port & _BV(bit)) \ + port &= ~_BV(bit); \ + else \ + port |= _BV(bit); \ + } while(0) + +#define LED1_ON() sbi(PORTJ, 2) +#define LED1_OFF() cbi(PORTJ, 2) +#define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) + +#define LED2_ON() sbi(PORTL, 7) +#define LED2_OFF() cbi(PORTL, 7) +#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) + +#define LED3_ON() sbi(PORTJ, 3) +#define LED3_OFF() cbi(PORTJ, 3) +#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) + +#define LED4_ON() sbi(PORTL, 6) +#define LED4_OFF() cbi(PORTL, 6) +#define LED4_TOGGLE() LED_TOGGLE(PORTL, 6) + +#define BRAKE_DDR() do { DDRJ |= 0xF0; } while(0) +#define BRAKE_ON() do { PORTJ |= 0xF0; } while(0) +#define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0) + +#define LEFT_ARM_ENCODER ((void *)0) +#define RIGHT_ARM_ENCODER ((void *)1) + +#define LEFT_ARM_PWM ((void *)&gen.pwm1_4A) +#define RIGHT_ARM_PWM ((void *)&gen.pwm2_4B) + +#define RIGHT_PUMP1_PWM ((void *)&gen.pwm3_1A) +#define RIGHT_PUMP2_PWM ((void *)&gen.pwm4_1B) + +#define R_ELBOW_AX12 1 +#define R_WRIST_AX12 2 +#define L_ELBOW_AX12 4 +#define L_WRIST_AX12 3 +#define FINGER_AX12 5 + +/** ERROR NUMS */ +#define E_USER_I2C_PROTO 195 +#define E_USER_SENSOR 196 +#define E_USER_ARM 197 +#define E_USER_FINGER 198 +#define E_USER_ST_MACH 199 +#define E_USER_CS 200 +#define E_USER_AX12 201 + +#define LED_PRIO 170 +#define TIME_PRIO 160 +#define ADC_PRIO 120 +#define CS_PRIO 100 +#define ARM_PRIO 50 +#define I2C_POLL_PRIO 20 + +#define CS_PERIOD 5000L + +#define NB_LOGS 4 + +/* generic to all boards */ +struct genboard { + /* command line interface */ + struct rdline rdl; + char prompt[RDLINE_PROMPT_SIZE]; + + /* motors */ + struct pwm_ng pwm1_4A; + struct pwm_ng pwm2_4B; + struct pwm_ng pwm3_1A; + struct pwm_ng pwm4_1B; + + /* servos */ + struct pwm_ng servo1; + struct pwm_ng servo2; + struct pwm_ng servo3; + struct pwm_ng servo4; + + /* ax12 interface */ + AX12 ax12; + + /* log */ + uint8_t logs[NB_LOGS+1]; + uint8_t log_level; + uint8_t debug; +}; + +struct cs_block { + uint8_t on; + struct cs cs; + struct pid_filter pid; + struct quadramp_filter qr; + struct blocking_detection bd; +}; + +/* mechboard specific */ +struct mechboard { +#define DO_ENCODERS 1 +#define DO_CS 2 +#define DO_BD 4 +#define DO_POWER 8 + uint8_t flags; /* misc flags */ + + /* control systems */ + struct cs_block left_arm; + struct cs_block right_arm; + + /* robot status */ + uint8_t our_color; + volatile uint8_t lintel_count; + volatile uint8_t column_flags; + volatile uint8_t status; + + /* local pumps */ + int16_t pump_right1; + int16_t pump_right2; + /* remote pump (on mainboard) */ + int16_t pump_left1; + int16_t pump_left2; + + /* remote pump current */ + int16_t pump_left1_current; + int16_t pump_left2_current; + + uint16_t servo_lintel_left; + uint16_t servo_lintel_right; + +}; + +extern struct genboard gen; +extern struct mechboard mechboard; + +/* start the bootloader */ +void bootloader(void); + +#define DEG(x) (((double)(x)) * (180.0 / M_PI)) +#define RAD(x) (((double)(x)) * (M_PI / 180.0)) +#define M_2PI (2*M_PI) + +#define WAIT_COND_OR_TIMEOUT(cond, timeout) \ +({ \ + microseconds __us = time_get_us2(); \ + uint8_t __ret = 1; \ + while(! (cond)) { \ + if (time_get_us2() - __us > (timeout)*1000L) {\ + __ret = 0; \ + break; \ + } \ + } \ + __ret; \ +}) diff --git a/projects/microb2010/mechboard/pid_config.h b/projects/microb2010/mechboard/pid_config.h new file mode 100755 index 0000000..fa95f08 --- /dev/null +++ b/projects/microb2010/mechboard/pid_config.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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 + * + * + * + */ + +#ifndef PID_CONFIG_H +#define PID_CONFIG_H + +/** the derivate term can be filtered to remove the noise. This value + * is the maxium sample count to keep in memory to do this + * filtering. For an instance of pid, this count is defined o*/ +#define PID_DERIVATE_FILTER_MAX_SIZE 4 + +#endif diff --git a/projects/microb2010/mechboard/rdline_config.h b/projects/microb2010/mechboard/rdline_config.h new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/mechboard/scheduler_config.h b/projects/microb2010/mechboard/scheduler_config.h new file mode 100755 index 0000000..98690be --- /dev/null +++ b/projects/microb2010/mechboard/scheduler_config.h @@ -0,0 +1,47 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: scheduler_config.h,v 1.2 2009-04-07 20:03:48 zer0 Exp $ + * + */ + +#ifndef _SCHEDULER_CONFIG_H_ +#define _SCHEDULER_CONFIG_H_ + +#define _SCHEDULER_CONFIG_VERSION_ 4 + +/** maximum number of allocated events */ +#define SCHEDULER_NB_MAX_EVENT 9 + + +#define SCHEDULER_UNIT_FLOAT 512.0 +#define SCHEDULER_UNIT 512L + +/** number of allowed imbricated scheduler interrupts. The maximum + * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate + * more than once per event. If it is less, it can avoid to browse the + * event table, events are delayed (we loose precision) but it takes + * less CPU */ +#define SCHEDULER_NB_STACKING_MAX SCHEDULER_NB_MAX_EVENT + +/** define it for debug infos (not recommended, because very slow on + * an AVR, it uses printf in an interrupt). It can be useful if + * prescaler is very high, making the timer interrupt period very + * long in comparison to printf() */ +/* #define SCHEDULER_DEBUG */ + +#endif // _SCHEDULER_CONFIG_H_ diff --git a/projects/microb2010/mechboard/sensor.c b/projects/microb2010/mechboard/sensor.c new file mode 100644 index 0000000..a80201f --- /dev/null +++ b/projects/microb2010/mechboard/sensor.c @@ -0,0 +1,254 @@ +/* + * Copyright Droids Corporation (2009) + * 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 + * (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: sensor.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "sensor.h" + +/************ ADC */ + +struct adc_infos { + uint16_t config; + int16_t value; + int16_t prev_val; + int16_t (*filter)(struct adc_infos *, int16_t); +}; + +/* reach 90% of the value in 4 samples */ +int16_t rii_light(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + (int32_t)adc->prev_val / 2; + return adc->prev_val / 2; +} + +/* reach 90% of the value in 8 samples */ +int16_t rii_medium(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4; + return adc->prev_val / 4; +} + +/* reach 90% of the value in 16 samples */ +int16_t rii_strong(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8; + return adc->prev_val / 8; +} + + +#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x ) + +/* 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 }, + + /* 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, see init below */ +static void do_adc(__attribute__((unused)) void *dummy) +{ + /* launch first conversion */ + adc_launch(adc_infos[0].config); +} + +static void adc_event(int16_t result) +{ + static uint8_t i = 0; + + /* filter value if needed */ + if (adc_infos[i].filter) + adc_infos[i].value = adc_infos[i].filter(&adc_infos[i], + result); + else + adc_infos[i].value = result; + + i ++; + if (i >= ADC_MAX) + i = 0; + else + adc_launch(adc_infos[i].config); +} + +int16_t sensor_get_adc(uint8_t i) +{ + int16_t tmp; + uint8_t flags; + + IRQ_LOCK(flags); + tmp = adc_infos[i].value; + 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; + uint8_t invert; +}; + +/* pullup mapping: + * CAP 1,5,6,7,8 + */ +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_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ + [S_COL_LEFT] = { 5, 0, 4, 1, 0, 1 }, /* 4 */ + [S_LEFT] = { 5, 0, 4, 1, 0, 1 }, /* 5 */ + [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ + [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */ + [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ + [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ + [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */ + [S_RESERVED4] = { 1, 0, 0, 1, 0, 0 }, /* 11 */ + [S_RESERVED5] = { 1, 0, 0, 1, 0, 0 }, /* 12 */ + [S_RESERVED6] = { 1, 0, 0, 1, 0, 0 }, /* 13 */ + [S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */ + [S_RESERVED8] = { 1, 0, 0, 1, 0, 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(__attribute__((unused)) void *dummy) +{ + uint8_t i; + uint8_t flags; + uint16_t sensor = sensor_read(); + uint16_t tmp = 0; + + for (i=0; i= 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 && !sensor_filter[i].invert) { + tmp |= (1UL << i); + } + else if (!sensor_filter[i].prev && sensor_filter[i].invert) { + 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(__attribute__((unused)) 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); +} + diff --git a/projects/microb2010/mechboard/sensor.h b/projects/microb2010/mechboard/sensor.h new file mode 100644 index 0000000..27460db --- /dev/null +++ b/projects/microb2010/mechboard/sensor.h @@ -0,0 +1,56 @@ +/* + * Copyright Droids Corporation (2009) + * 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 + * (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: sensor.h,v 1.4 2009-04-24 19:30:42 zer0 Exp $ + * + */ + +/* synchronize with sensor.c */ +#define ADC_CSENSE1 0 +#define ADC_CSENSE2 1 +#define ADC_CSENSE3 2 +#define ADC_CSENSE4 3 +#define ADC_MAX 4 + +/* synchronize with sensor.c */ +#define S_CAP1 0 +#define S_FRONT 1 +#define S_CAP3 2 +#define S_CAP4 3 +#define S_COL_LEFT 4 +#define S_LEFT 5 +#define S_RIGHT 6 +#define S_COL_RIGHT 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); + +/* 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); diff --git a/projects/microb2010/mechboard/spi_config.h b/projects/microb2010/mechboard/spi_config.h new file mode 100644 index 0000000..76697c3 --- /dev/null +++ b/projects/microb2010/mechboard/spi_config.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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 + * + */ + +/* + * Author : Julien LE GUEN - jlg@jleguen.info + */ + + +/* + * Configure HERE your SPI module + */ + + + +/* Number of slave devices in your system + * Each slave have a dedicated SS line that you have to register + * before using the SPI module + */ +#define SPI_MAX_SLAVES 1 + diff --git a/projects/microb2010/mechboard/state.c b/projects/microb2010/mechboard/state.c new file mode 100644 index 0000000..10af0f2 --- /dev/null +++ b/projects/microb2010/mechboard/state.c @@ -0,0 +1,1407 @@ +/* + * 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 + * (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: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "cmdline.h" +#include "sensor.h" +#include "actuator.h" +#include "arm_xy.h" +#include "arm_highlevel.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 MANUAL I2C_MECHBOARD_MODE_MANUAL +#define HARVEST I2C_MECHBOARD_MODE_HARVEST +#define PREPARE_PICKUP I2C_MECHBOARD_MODE_PREPARE_PICKUP +#define PICKUP I2C_MECHBOARD_MODE_PICKUP +#define PREPARE_BUILD I2C_MECHBOARD_MODE_PREPARE_BUILD +#define AUTOBUILD I2C_MECHBOARD_MODE_AUTOBUILD +#define WAIT I2C_MECHBOARD_MODE_WAIT +#define INIT I2C_MECHBOARD_MODE_INIT +#define PREPARE_GET_LINTEL I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL +#define GET_LINTEL I2C_MECHBOARD_MODE_GET_LINTEL +#define PUT_LINTEL I2C_MECHBOARD_MODE_PUT_LINTEL +#define PREPARE_EJECT I2C_MECHBOARD_MODE_PREPARE_EJECT +#define EJECT I2C_MECHBOARD_MODE_EJECT +#define CLEAR I2C_MECHBOARD_MODE_CLEAR +#define LAZY_HARVEST I2C_MECHBOARD_MODE_LAZY_HARVEST +#define LOADED I2C_MECHBOARD_MODE_LOADED +#define PREPARE_INSIDE I2C_MECHBOARD_MODE_PREPARE_INSIDE +#define STORE I2C_MECHBOARD_MODE_STORE +#define LAZY_PICKUP I2C_MECHBOARD_MODE_LAZY_PICKUP +#define MANIVELLE I2C_MECHBOARD_MODE_MANIVELLE +#define PUSH_TEMPLE I2C_MECHBOARD_MODE_PUSH_TEMPLE +#define PUSH_TEMPLE_DISC I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC +#define EXIT I2C_MECHBOARD_MODE_EXIT + +static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode); + +static struct i2c_cmd_mechboard_set_mode mainboard_command; +static struct vt100 local_vt100; +static volatile uint8_t prev_state; +static uint8_t pickup_side; +static volatile uint8_t changed = 0; + +uint8_t state_debug = 0; + +void state_dump_sensors(void) +{ + uint16_t tmp = sensor_get_all(); + prog_char *front = PSTR("no_front"); + prog_char *left = PSTR("no_left"); + prog_char *right = PSTR("no_right"); + + if (tmp & _BV(S_FRONT)) + front = PSTR("FRONT"); + if (tmp & _BV(S_LEFT)) { + if (tmp & _BV(S_COL_LEFT)) + left = PSTR("LEFT(red)"); + else + left = PSTR("LEFT(green)"); + } + if (tmp & _BV(S_RIGHT)) { + if (tmp & _BV(S_COL_RIGHT)) + right = PSTR("RIGHT(red)"); + else + right = PSTR("RIGHT(green)"); + } + + STMCH_DEBUG("sensors = %S %S %S", front, left, right); +} + +/* return 1 if column is there */ +uint8_t arm_get_sensor(uint8_t arm_num) +{ + if (arm_num == ARM_LEFT_NUM) { + return sensor_get(S_LEFT); + } + else if (arm_num == ARM_RIGHT_NUM) { + return sensor_get(S_RIGHT); + } + return 0; +} + +/* return 0 if color is correct, else return -1 */ +int8_t arm_get_color_sensor(uint8_t arm_num) +{ + uint8_t col = 0; + if (arm_num == ARM_LEFT_NUM) { + col = sensor_get(S_COL_LEFT); + } + else if (arm_num == ARM_RIGHT_NUM) { + col = sensor_get(S_COL_RIGHT); + } + + /* if col != 0, column is red */ + if (col) { + if (mechboard.our_color == I2C_COLOR_RED) + return 0; + return -1; + } + else { + if (mechboard.our_color == I2C_COLOR_GREEN) + return 0; + return -1; + } +} + +void state_debug_wait_key_pressed(void) +{ + if (!state_debug) + return; + printf_P(PSTR("press a key\r\n")); + while(!cmdline_keypressed()); +} + +/* set a new state, return 0 on success */ +int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd) +{ + changed = 1; + prev_state = mainboard_command.mode; + memcpy(&mainboard_command, cmd, sizeof(mainboard_command)); + STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode); + return 0; +} + +/* check that state is the one in parameter and that state did not + * changed */ +uint8_t state_check(uint8_t mode) +{ + 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; +} + +uint8_t state_get_mode(void) +{ + return mainboard_command.mode; +} + +void pump_reset_all(void) +{ + uint8_t i; + for (i=0; i<4; i++) { + pump_set(i, PUMP_OFF); + pump_mark_free(i); + } +} + +void pump_check_all(void) +{ + if (pump_is_busy(PUMP_LEFT1_NUM) && + mechboard.pump_left1_current < I2C_MECHBOARD_CURRENT_COLUMN) { + STMCH_DEBUG("Mark l1 as free"); + pump_mark_free(PUMP_LEFT1_NUM); + pump_set(PUMP_LEFT1_NUM, PUMP_OFF); + } + + if (pump_is_busy(PUMP_LEFT2_NUM) && + mechboard.pump_left2_current < I2C_MECHBOARD_CURRENT_COLUMN) { + STMCH_DEBUG("Mark l2 as free"); + pump_mark_free(PUMP_LEFT2_NUM); + pump_set(PUMP_LEFT2_NUM, PUMP_OFF); + } + + if (pump_is_busy(PUMP_RIGHT1_NUM) && + sensor_get_adc(ADC_CSENSE3) < I2C_MECHBOARD_CURRENT_COLUMN) { + STMCH_DEBUG("Mark r1 as free"); + pump_mark_free(PUMP_RIGHT1_NUM); + pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); + } + + if (pump_is_busy(PUMP_RIGHT2_NUM) && + sensor_get_adc(ADC_CSENSE4) < I2C_MECHBOARD_CURRENT_COLUMN) { + STMCH_DEBUG("Mark r2 as free"); + pump_mark_free(PUMP_RIGHT2_NUM); + pump_set(PUMP_RIGHT2_NUM, PUMP_OFF); + } +} + +uint8_t get_free_pump_count(void) +{ + uint8_t i, free_pump_count = 0; + for (i=0; i<4; i++) { + if (pump_is_free(i)) + free_pump_count++; + } + return free_pump_count; +} + +/* move finger if we are not in lazy harvest */ +void state_finger_goto(uint8_t mode, uint16_t position) +{ + if (mode == LAZY_HARVEST) + return; + finger_goto(position); +} + +void state_manivelle(int16_t step_deg) +{ + double add_h = 0.; + double add_d = 160.; + double l = 70.; + double step = RAD(step_deg); + microseconds us; + double al = RAD(0); + double ar = RAD(180); + + time_wait_ms(500); + + us = time_get_us2(); + while (1) { + al += step; + ar += step; + arm_do_xy(&left_arm, add_d+l*sin(al), add_h+l*cos(al), 10); + arm_do_xy(&right_arm, add_d+l*sin(ar), add_h+l*cos(ar), 10); + time_wait_ms(25); + if (time_get_us2() - us > (4000L * 1000L)) + break; + } +} + +static void state_do_manivelle(void) +{ + if (!state_check(MANIVELLE)) + return; + state_manivelle(30); + while (state_check(MANIVELLE)); +} + +/* common function for pickup/harvest */ +static void state_pickup_or_harvest(uint8_t mode) +{ + int8_t arm_num, pump_num; + int8_t other_arm_num, other_pump_num; + struct arm *arm; + microseconds us; + uint8_t flags, bad_color = 0, have_2cols = 0; + + pump_check_all(); + + /* get arm num */ + if (pickup_side == I2C_LEFT_SIDE) { + arm_num = ARM_LEFT_NUM; + other_arm_num = ARM_RIGHT_NUM; + } + else { + arm_num = ARM_RIGHT_NUM; + other_arm_num = ARM_LEFT_NUM; + } + + pump_num = arm_get_free_pump(arm_num); + other_pump_num = arm_get_free_pump(other_arm_num); + + /* pump is not free... skip to other arm */ + if (mode == HARVEST && pump_num == -1) { + STMCH_DEBUG("%s no free pump", __FUNCTION__); + if (arm_num == ARM_RIGHT_NUM) { + state_finger_goto(mode, FINGER_CENTER_RIGHT); + pickup_side = I2C_LEFT_SIDE; + } + else { + state_finger_goto(mode, FINGER_CENTER_LEFT); + pickup_side = I2C_RIGHT_SIDE; + } + return; + } + else if (mode == PICKUP && pump_num == -1) { + /* or exit when we are in pickup mode */ + IRQ_LOCK(flags); + if (mainboard_command.mode == mode) + mainboard_command.mode = WAIT; + IRQ_UNLOCK(flags); + } + + us = time_get_us2(); + /* wait front sensor */ + if (mode == HARVEST || mode == LAZY_HARVEST) { + STMCH_DEBUG("%s wait front", __FUNCTION__); + + while (1) { + if (sensor_get(S_FRONT)) + break; + if (state_check(mode) == 0) + return; + /* wait 500ms before reading other + sensors */ + if (time_get_us2() - us < (500 * 1000L)) + continue; + if (arm_get_sensor(arm_num)) + break; + if (arm_get_sensor(other_arm_num)) { + uint8_t tmp; + tmp = arm_num; + arm_num = other_arm_num; + other_arm_num = tmp; + pump_num = arm_get_free_pump(arm_num); + other_pump_num = arm_get_free_pump(other_arm_num); + if (other_pump_num == -1) + return; // XXX + break; + } + } + } + + + STMCH_DEBUG("%s arm_num=%d pump_num=%d", + __FUNCTION__, arm_num, pump_num); + + /* when ready, move finger */ + if (arm_num == ARM_RIGHT_NUM) + state_finger_goto(mode, FINGER_RIGHT); + else + state_finger_goto(mode, FINGER_LEFT); + + state_debug_wait_key_pressed(); + + + arm = arm_num2ptr(arm_num); + + /* prepare arm, should be already done */ + arm_goto_prepare_get(arm_num, pump_num); + while (arm_test_traj_end(arm, ARM_TRAJ_ALL) && + state_check(mode)); + + STMCH_DEBUG("%s arm pos ok", __FUNCTION__); + + state_debug_wait_key_pressed(); + + /* wait to see the column on the sensor */ + us = time_get_us2(); + while (1) { + if (arm_get_sensor(arm_num)) + break; + if (state_check(mode) == 0) + return; + if (mode == PICKUP) /* no timeout in pickup */ + continue; + /* 500ms timeout in harvest, go back */ + if (time_get_us2() - us > 500*1000L) { + STMCH_DEBUG("%s timeout", __FUNCTION__); + + if (arm_num == ARM_RIGHT_NUM) + state_finger_goto(mode, FINGER_LEFT); + else + state_finger_goto(mode, FINGER_RIGHT); + + if (sensor_get(S_FRONT)) + time_wait_ms(500); + + pump_set(pump_num, PUMP_OFF); + return; + } + } + + state_dump_sensors(); + + pump_set(pump_num, PUMP_ON); + /* bad color */ + if (arm_get_color_sensor(arm_num) == -1) { + bad_color = 1; + STMCH_DEBUG("%s prepare eject", __FUNCTION__); + mainboard_command.mode = PREPARE_EJECT; + state_do_eject(arm_num, pump_num, mode); + return; + } + + STMCH_DEBUG("%s sensor ok", __FUNCTION__); + + /* by the way, prepare the other arm */ + if (other_pump_num != -1) + arm_goto_prepare_get(other_arm_num, other_pump_num); + + /* get the column */ + arm_goto_get_column(arm_num, pump_num); + + us = time_get_us2(); + while (1) { + /* wait 50 ms */ + if (time_get_us2() - us > 50*1000L) + break; + if (mode != HARVEST) + continue; + /* if we still see the front sensor, it's because + * there are 2 columns instead of one or because there + * is another column, so send the arm on other + * side. */ + if (sensor_get(S_FRONT) && have_2cols == 0) { + STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__); + have_2cols = 1; + if (finger_get_side() == I2C_LEFT_SIDE) + state_finger_goto(mode, FINGER_RIGHT); + else + state_finger_goto(mode, FINGER_LEFT); + } + } + + if (mode == HARVEST && have_2cols == 0) { + /* just release a bit of effort */ + if (finger_get_side() == I2C_LEFT_SIDE) { + state_finger_goto(mode, FINGER_LEFT_RELAX); + } + else { + state_finger_goto(mode, FINGER_RIGHT_RELAX); + } + } + else if (mode == PICKUP) { + /* no free pump on other arm */ + if (other_pump_num == -1) { + if (finger_get_side() == I2C_LEFT_SIDE) { + state_finger_goto(mode, FINGER_LEFT_RELAX); + } + else { + state_finger_goto(mode, FINGER_RIGHT_RELAX); + } + } + /* else send finger on the other side */ + else { + if (finger_get_side() == I2C_LEFT_SIDE) { + state_finger_goto(mode, FINGER_RIGHT); + } + else { + state_finger_goto(mode, FINGER_LEFT); + } + } + } + + us = time_get_us2(); + while (1) { + /* wait 100 ms */ + if (time_get_us2() - us > 100*1000L) + break; + if (mode != HARVEST) + continue; + /* if we still see the front sensor, it's because + * there are 2 columns instead of one or because there + * is another column, so send the arm on other + * side. */ + if (sensor_get(S_FRONT) && have_2cols == 0) { + STMCH_DEBUG("%s 2 columns, release finger", __FUNCTION__); + have_2cols = 1; + if (finger_get_side() == I2C_LEFT_SIDE) + state_finger_goto(mode, FINGER_RIGHT); + else + state_finger_goto(mode, FINGER_LEFT); + } + } + + /* consider the column as taken */ + pump_mark_busy(pump_num); + + state_debug_wait_key_pressed(); + + arm_goto_intermediate_get(arm_num, pump_num); + arm_wait_traj_end(arm, ARM_TRAJ_ALL_NEAR); + + /* prepare next */ + pump_num = arm_get_free_pump(arm_num); + if (pump_num == -1) + arm_goto_loaded(arm_num); + else + arm_goto_intermediate_get(arm_num, pump_num); + + state_debug_wait_key_pressed(); + + /* switch to wait state */ + if (get_free_pump_count() == 0) { + IRQ_LOCK(flags); + if (mainboard_command.mode == mode) + mainboard_command.mode = WAIT; + IRQ_UNLOCK(flags); + } + + /* next pickup/harvest will be on the other side */ + if (pickup_side == I2C_LEFT_SIDE) + pickup_side = I2C_RIGHT_SIDE; + else + pickup_side = I2C_LEFT_SIDE; +} + + +/* manual mode, arm position is sent from mainboard */ +static void state_do_manual(void) +{ + if (!state_check(MANUAL)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(MANUAL)); +} + +/* wait mode */ +static void state_do_wait(void) +{ + if (!state_check(WAIT)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(WAIT)); +} + +/* init mode */ +static void state_do_init(void) +{ + if (!state_check(INIT)) + return; + state_init(); + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + while (state_check(INIT)); +} + +/* harvest columns elts from area */ +static void state_do_harvest(void) +{ + if (!state_check(HARVEST)) + return; + + if (get_free_pump_count() == 0) { + mainboard_command.mode = WAIT; + return; + } + + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + state_pickup_or_harvest(HARVEST); +} + +/* harvest columns elts from area without moving finger */ +static void state_do_lazy_harvest(void) +{ + if (!state_check(LAZY_HARVEST)) + return; + + if (get_free_pump_count() == 0) { + mainboard_command.mode = WAIT; + return; + } + + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + state_pickup_or_harvest(LAZY_HARVEST); +} + +/* eject a column. always called from pickup mode. */ +static void state_do_eject(uint8_t arm_num, uint8_t pump_num, uint8_t old_mode) +{ + struct arm *arm; + arm = arm_num2ptr(arm_num); + + if (finger_get_side() == I2C_LEFT_SIDE) { + state_finger_goto(old_mode, FINGER_LEFT_RELAX); + } + else { + state_finger_goto(old_mode, FINGER_RIGHT_RELAX); + } + + /* wait mainboard to eject */ + while (state_check(PREPARE_EJECT)); + + if (finger_get_side() == I2C_LEFT_SIDE) { + state_finger_goto(old_mode, FINGER_CENTER_LEFT); + } + else { + state_finger_goto(old_mode, FINGER_CENTER_RIGHT); + } + + arm_goto_get_column(arm_num, pump_num); + arm_wait_traj_end(arm, ARM_TRAJ_ALL); + time_wait_ms(150); + + state_debug_wait_key_pressed(); + + arm_goto_prepare_eject(arm_num, pump_num); + arm_wait_traj_end(arm, ARM_TRAJ_ALL); + + state_debug_wait_key_pressed(); + + if (finger_get_side() == I2C_LEFT_SIDE) { + state_finger_goto(old_mode, FINGER_LEFT_RELAX); + } + else { + state_finger_goto(old_mode, FINGER_RIGHT_RELAX); + } + + state_debug_wait_key_pressed(); + + time_wait_ms(300); + arm_goto_eject(arm_num, pump_num); + time_wait_ms(200); + pump_set(pump_num, PUMP_REVERSE); + arm_wait_traj_end(arm, ARM_TRAJ_ALL); + + arm_goto_intermediate_get(arm_num, pump_num); + pump_set(pump_num, PUMP_OFF); +} + + +/* prepare pickup in a dispenser, or harvest */ +static void state_do_prepare_pickup(void) +{ + uint8_t left_count = 0, right_count = 0; + int8_t pump_l, pump_r; + + if (!state_check(PREPARE_PICKUP)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + pump_check_all(); + + pump_l = arm_get_free_pump(ARM_LEFT_NUM); + if (pump_l == -1) { + arm_goto_loaded(ARM_LEFT_NUM); + } + else { + arm_goto_intermediate_front_get(ARM_LEFT_NUM, pump_l); + } + + pump_r = arm_get_free_pump(ARM_RIGHT_NUM); + if (pump_r == -1) { + arm_goto_loaded(ARM_RIGHT_NUM); + } + else { + arm_goto_intermediate_front_get(ARM_RIGHT_NUM, pump_r); + } + + arm_wait_both(ARM_TRAJ_ALL); + + if (pump_l != -1) + arm_goto_prepare_get(ARM_LEFT_NUM, pump_l); + if (pump_r != -1) + arm_goto_prepare_get(ARM_RIGHT_NUM, pump_r); + + if (mainboard_command.prep_pickup.side == I2C_AUTO_SIDE) { + left_count += pump_is_busy(PUMP_LEFT1_NUM); + left_count += pump_is_busy(PUMP_LEFT2_NUM); + right_count += pump_is_busy(PUMP_RIGHT1_NUM); + right_count += pump_is_busy(PUMP_RIGHT2_NUM); + if (left_count < right_count) + finger_goto(FINGER_RIGHT); + else + finger_goto(FINGER_LEFT); + } + else if (mainboard_command.prep_pickup.side == I2C_LEFT_SIDE) + finger_goto(FINGER_LEFT); + else if (mainboard_command.prep_pickup.side == I2C_RIGHT_SIDE) + finger_goto(FINGER_RIGHT); + else if (mainboard_command.prep_pickup.side == I2C_CENTER_SIDE) + finger_goto(FINGER_CENTER_LEFT); + + /* try to know on which side we have to pickup */ + if (finger_get_side() == I2C_RIGHT_SIDE) { + pickup_side = I2C_LEFT_SIDE; + } + else { + pickup_side = I2C_RIGHT_SIDE; + } + + arm_prepare_free_pumps(); + + mainboard_command.mode = mainboard_command.prep_pickup.next_mode; + + while (state_check(PREPARE_PICKUP)); +} + +/* clear pickup zone, will switch to harvest if needed */ +static void state_do_clear(void) +{ + uint8_t flags, err; + + if (!state_check(CLEAR)) + return; + + if (get_free_pump_count() == 0) { + mainboard_command.mode = WAIT; + return; + } + + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + finger_goto(FINGER_LEFT); + err = WAIT_COND_OR_TIMEOUT(sensor_get(S_LEFT), 500); + if (err) { + IRQ_LOCK(flags); + if (mainboard_command.mode == CLEAR) + mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; + IRQ_UNLOCK(flags); + pickup_side = I2C_LEFT_SIDE; + return; + } + + finger_goto(FINGER_RIGHT); + err = WAIT_COND_OR_TIMEOUT(sensor_get(S_RIGHT), 500); + if (err) { + IRQ_LOCK(flags); + if (mainboard_command.mode == CLEAR) + mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; + IRQ_UNLOCK(flags); + pickup_side = I2C_RIGHT_SIDE; + return; + } + + IRQ_LOCK(flags); + if (mainboard_command.mode == CLEAR) + mainboard_command.mode = I2C_MECHBOARD_MODE_HARVEST; + IRQ_UNLOCK(flags); +} + +/* do a lazy pickup */ +static void state_do_lazy_pickup(void) +{ + int8_t flags, arm_num, pump_num; + uint32_t us; + + if (!state_check(LAZY_PICKUP)) + return; + + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + if (arm_get_sensor(ARM_LEFT_NUM) && + arm_get_sensor(ARM_RIGHT_NUM)) { + IRQ_LOCK(flags); + if (mainboard_command.mode == LAZY_PICKUP) { + mainboard_command.mode = WAIT; + } + IRQ_UNLOCK(flags); + return; + } + + if (finger_get_side() == I2C_RIGHT_SIDE) { + finger_goto(FINGER_LEFT); + arm_num = ARM_LEFT_NUM; + } + else { + finger_goto(FINGER_RIGHT); + arm_num = ARM_RIGHT_NUM; + } + + us = time_get_us2(); + while(1) { + if (state_check(LAZY_PICKUP) == 0) + return; + if (arm_get_sensor(arm_num)) + break; + if (time_get_us2() - us > 500*1000L) { + if (finger_get_side() == I2C_RIGHT_SIDE) + finger_goto(FINGER_LEFT); + else + finger_goto(FINGER_RIGHT); + return; + } + } + + if (arm_get_color_sensor(arm_num) == -1) { + pump_num = arm_get_free_pump(arm_num); + if (pump_num == -1) + return; /* XXX */ + pump_set(pump_num, PUMP_ON); + STMCH_DEBUG("%s prepare eject", __FUNCTION__); + mainboard_command.mode = PREPARE_EJECT; + state_do_eject(arm_num, pump_num, LAZY_PICKUP); + } +} + +/* pickup from a dispenser automatically */ +static void state_do_pickup(void) +{ + if (!state_check(PICKUP)) + return; + + if (get_free_pump_count() == 0) { + mainboard_command.mode = WAIT; + return; + } + + /* XXX check that finger is at correct place */ + + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + state_pickup_or_harvest(PICKUP); +} + +/* store columns without using arms */ +static void state_do_store(void) +{ + int8_t arm_num; + int8_t other_arm_num; + microseconds us; + + if (!state_check(STORE)) + return; + + if (get_free_pump_count() == 0) { + mainboard_command.mode = WAIT; + return; + } + + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + /* get arm num */ + if (pickup_side == I2C_LEFT_SIDE) { + arm_num = ARM_LEFT_NUM; + other_arm_num = ARM_RIGHT_NUM; + } + else { + arm_num = ARM_RIGHT_NUM; + other_arm_num = ARM_LEFT_NUM; + } + + while (1) { + if (sensor_get(S_FRONT)) + break; + if (state_check(STORE) == 0) + return; + } + + /* when ready, move finger */ + if (arm_num == ARM_RIGHT_NUM) + finger_goto(FINGER_RIGHT); + else + finger_goto(FINGER_LEFT); + + /* wait to see the column on the sensor */ + us = time_get_us2(); + while (1) { + if (arm_get_sensor(arm_num)) + break; + if (state_check(STORE) == 0) + return; + /* 500ms timeout in harvest, go back */ + if (time_get_us2() - us > 500*1000L) { + STMCH_DEBUG("%s timeout", __FUNCTION__); + + if (arm_num == ARM_RIGHT_NUM) + finger_goto(FINGER_LEFT); + else + finger_goto(FINGER_RIGHT); + return; + } + } + + if (arm_get_sensor(arm_num) && arm_get_sensor(other_arm_num)) { + STMCH_DEBUG("%s full", __FUNCTION__); + while (state_check(STORE)); + return; + } + + /* next store will be on the other side */ + if (pickup_side == I2C_LEFT_SIDE) + pickup_side = I2C_RIGHT_SIDE; + else + pickup_side = I2C_LEFT_SIDE; +} + +/* prepare the building of a temple */ +static void state_do_prepare_build(void) +{ + int8_t pump_num, level; + if (!state_check(PREPARE_BUILD)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + pump_check_all(); + + if (finger_get_side() == I2C_LEFT_SIDE) + finger_goto(FINGER_LEFT); + else + finger_goto(FINGER_RIGHT); + + pump_num = arm_get_busy_pump(ARM_LEFT_NUM); + level = mainboard_command.prep_build.level_l; + if (pump_num != -1 && level != -1) + arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_num, + level, I2C_AUTOBUILD_DEFAULT_DIST); + + pump_num = arm_get_busy_pump(ARM_RIGHT_NUM); + level = mainboard_command.prep_build.level_r; + if (pump_num != -1 && level != -1) + arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_num, + level, I2C_AUTOBUILD_DEFAULT_DIST); + + while (state_check(PREPARE_BUILD)); +} + +/* prepare the building of a temple */ +static void state_do_push_temple(void) +{ + uint8_t level; + + level = mainboard_command.push_temple.level; + + if (!state_check(PUSH_TEMPLE)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + if (finger_get_side() == I2C_LEFT_SIDE) + finger_goto(FINGER_LEFT); + else + finger_goto(FINGER_RIGHT); + + arm_goto_prepare_push_temple(ARM_LEFT_NUM); + arm_goto_prepare_push_temple(ARM_RIGHT_NUM); + arm_wait_both(ARM_TRAJ_ALL); + + arm_goto_push_temple(ARM_LEFT_NUM, level); + arm_goto_push_temple(ARM_RIGHT_NUM, level); + + while (state_check(PUSH_TEMPLE)); +} + +/* prepare the building of a temple */ +static void state_do_push_temple_disc(void) +{ + uint8_t side; + struct arm *arm; + + if (!state_check(PUSH_TEMPLE_DISC)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + side = mainboard_command.push_temple_disc.side; + + if (side == I2C_LEFT_SIDE) { + arm = arm_num2ptr(ARM_LEFT_NUM); + arm_goto_prepare_push_temple_disc(ARM_LEFT_NUM); + arm_wait_traj_end(arm, ARM_TRAJ_ALL); + arm_goto_push_temple_disc(ARM_LEFT_NUM); + } + else { + arm = arm_num2ptr(ARM_RIGHT_NUM); + arm_goto_prepare_push_temple_disc(ARM_RIGHT_NUM); + arm_wait_traj_end(arm, ARM_TRAJ_ALL); + arm_goto_push_temple_disc(ARM_RIGHT_NUM); + } + + while (state_check(PUSH_TEMPLE_DISC)); +} + +/* prepare the building of a temple (mainly for columns) */ +static void state_do_prepare_inside(void) +{ + int8_t pump_num, level_l, level_r; + if (!state_check(PREPARE_INSIDE)) + return; + + level_l = mainboard_command.prep_inside.level_l; + level_r = mainboard_command.prep_inside.level_r; + STMCH_DEBUG("%s mode=%d level_l=%d, level_r=%d", __FUNCTION__, + state_get_mode(), level_l, level_r); + + pump_check_all(); + + if (finger_get_side() == I2C_LEFT_SIDE) + finger_goto(FINGER_LEFT); + else + finger_goto(FINGER_RIGHT); + + pump_num = arm_get_busy_pump(ARM_LEFT_NUM); + if (pump_num == -1) + pump_num = PUMP_LEFT1_NUM; + if (level_l != -1) + arm_goto_prepare_build_inside(ARM_LEFT_NUM, pump_num, + level_l); + + pump_num = arm_get_busy_pump(ARM_RIGHT_NUM); + if (pump_num == -1) + pump_num = PUMP_RIGHT1_NUM; + if (level_r != -1) + arm_goto_prepare_build_inside(ARM_RIGHT_NUM, pump_num, + level_r); + + while (state_check(PREPARE_INSIDE)); +} + +/* moving position */ +static void state_do_loaded(void) +{ + if (!state_check(LOADED)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + pump_check_all(); + + if (finger_get_side() == I2C_LEFT_SIDE) + finger_goto(FINGER_LEFT); + else + finger_goto(FINGER_RIGHT); + + arm_goto_loaded(ARM_LEFT_NUM); + arm_goto_loaded(ARM_RIGHT_NUM); + + while (state_check(LOADED)); +} + +static void state_do_build_lintel(uint8_t level) +{ + STMCH_DEBUG("%s() level=%d have_lintel=%d", + __FUNCTION__, level, mechboard.lintel_count); + + servo_lintel_out(); + + arm_goto_prepare_get_lintel_inside1(); + arm_wait_both(ARM_TRAJ_ALL); + state_debug_wait_key_pressed(); + + pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE); + pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE); + arm_goto_prepare_get_lintel_inside2(mechboard.lintel_count); + arm_wait_both(ARM_TRAJ_ALL); + state_debug_wait_key_pressed(); + + arm_goto_get_lintel_inside(mechboard.lintel_count); + arm_wait_both(ARM_TRAJ_ALL); + state_debug_wait_key_pressed(); + + time_wait_ms(150); + arm_goto_prepare_build_lintel1(); + arm_wait_both(ARM_TRAJ_ALL); + state_debug_wait_key_pressed(); + + arm_goto_prepare_build_lintel2(level); + arm_wait_both(ARM_TRAJ_ALL); + state_debug_wait_key_pressed(); + + arm_goto_prepare_build_lintel3(level); + arm_wait_both(ARM_TRAJ_ALL); + state_debug_wait_key_pressed(); + + if (mechboard.lintel_count == 1) + servo_lintel_1lin(); + else + servo_lintel_2lin(); + + arm_goto_build_lintel(level); + arm_wait_both(ARM_TRAJ_ALL); + time_wait_ms(170); + pump_set(PUMP_LEFT1_NUM, PUMP_ON); + time_wait_ms(50); /* right arm a bit after */ + pump_set(PUMP_RIGHT1_NUM, PUMP_ON); + time_wait_ms(130); + pump_set(PUMP_LEFT1_NUM, PUMP_OFF); + pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); + + mechboard.lintel_count --; +} + +/* Build one level of column. If pump_r or pump_l is -1, don't build + * with this arm. */ +static void state_do_build_column(uint8_t level_l, int8_t pump_l, + uint8_t dist_l, + uint8_t level_r, int8_t pump_r, + uint8_t dist_r) +{ + STMCH_DEBUG("%s() level_l=%d pump_l=%d level_r=%d pump_r=%d", + __FUNCTION__, level_l, pump_l, level_r, pump_r); + + /* nothing to do */ + if (pump_l == -1 && pump_r == -1) + return; + + /* go above the selected level */ + if (pump_l != -1) + arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, pump_l, level_l, dist_l); + if (pump_r != -1) + arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, pump_r, level_r, dist_r); + STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL), + arm_test_traj_end(&right_arm, ARM_TRAJ_ALL)); + arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL); + STMCH_DEBUG("l=%d r=%d", arm_test_traj_end(&left_arm, ARM_TRAJ_ALL), + arm_test_traj_end(&right_arm, ARM_TRAJ_ALL)); + + state_debug_wait_key_pressed(); + + /* drop columns of P2 */ + if (pump_l != -1) + arm_goto_autobuild(ARM_LEFT_NUM, pump_l, level_l, dist_l); + if (pump_r != -1) + arm_goto_autobuild(ARM_RIGHT_NUM, pump_r, level_r, dist_r); + arm_wait_select(pump_l != -1, pump_r != -1, ARM_TRAJ_ALL); + + state_debug_wait_key_pressed(); + + time_wait_ms(150); + if (pump_l != -1) + pump_set(pump_l, PUMP_REVERSE); + if (pump_r != -1) + pump_set(pump_r, PUMP_REVERSE); + time_wait_ms(150); + if (pump_l != -1) { + pump_set(pump_l, PUMP_OFF); + pump_mark_free(pump_l); + } + if (pump_r != -1) { + pump_set(pump_r, PUMP_OFF); + pump_mark_free(pump_r); + } + + state_debug_wait_key_pressed(); +} + +/* autobuild columns elts from area */ +/* check level to avoid bad things ? */ +/* check if enough cols ? */ +static void state_do_autobuild(void) +{ + int8_t pump_l, pump_r; + /* copy command into local data */ + int8_t level_l = mainboard_command.autobuild.level_left; + int8_t level_r = mainboard_command.autobuild.level_right; + uint8_t count_l = mainboard_command.autobuild.count_left; + uint8_t count_r = mainboard_command.autobuild.count_right; + uint8_t dist_l = mainboard_command.autobuild.distance_left; + uint8_t dist_r = mainboard_command.autobuild.distance_right; + uint8_t do_lintel = mainboard_command.autobuild.do_lintel; + int8_t max_level = level_l; + + + if (!state_check(AUTOBUILD)) + return; + + STMCH_DEBUG("%s mode=%d do_lintel=%d", __FUNCTION__, + state_get_mode(), do_lintel); + STMCH_DEBUG(" left: level=%d count=%d", level_l, count_l); + STMCH_DEBUG(" right: level=%d count=%d", level_r, count_r); + + /* + * build the first level of column if needed + */ + + /* don't build with this arm if no pump or if we don't ask to */ + pump_l = arm_get_busy_pump(ARM_LEFT_NUM); + if (count_l == 0) + pump_l = -1; + pump_r = arm_get_busy_pump(ARM_RIGHT_NUM); + if (count_r == 0) + pump_r = -1; + + if (pump_l == -1 && pump_r == -1) + goto lintel_only; + + state_do_build_column(level_l, pump_l, dist_l, + level_r, pump_r, dist_r); + + /* one level up */ + if (pump_l != -1) { + count_l --; + level_l ++; + max_level = level_l; + } + if (pump_r != -1) { + count_r --; + level_r ++; + if (level_r > max_level) + max_level = level_r; + } + + /* + * build the second level of column if needed + */ + + /* don't build with this arm if no pump or if we don't ask to */ + pump_l = arm_get_busy_pump(ARM_LEFT_NUM); + if (count_l == 0) + pump_l = -1; + pump_r = arm_get_busy_pump(ARM_RIGHT_NUM); + if (count_r == 0) + pump_r = -1; + + state_do_build_column(level_l, pump_l, dist_l, + level_r, pump_r, dist_r); + + /* one level up */ + if (pump_l != -1) { + count_l --; + level_l ++; + max_level = level_l; + } + if (pump_r != -1) { + count_r --; + level_r ++; + if (level_r > max_level) + max_level = level_r; + } + + state_debug_wait_key_pressed(); + + if (mechboard.lintel_count != 0 && do_lintel != 0) { + arm_goto_prepare_autobuild_outside(ARM_LEFT_NUM, + PUMP_LEFT1_NUM, + max_level, + I2C_AUTOBUILD_DEFAULT_DIST); + arm_goto_prepare_autobuild_outside(ARM_RIGHT_NUM, + PUMP_RIGHT1_NUM, + max_level, + I2C_AUTOBUILD_DEFAULT_DIST); + arm_wait_both(ARM_TRAJ_ALL_NEAR); + state_debug_wait_key_pressed(); + + arm_goto_prepare_autobuild_inside(ARM_LEFT_NUM, + PUMP_LEFT1_NUM, + max_level); + arm_goto_prepare_autobuild_inside(ARM_RIGHT_NUM, + PUMP_RIGHT1_NUM, + max_level); + arm_wait_both(ARM_TRAJ_ALL_NEAR); + state_debug_wait_key_pressed(); + } + + lintel_only: + if (mechboard.lintel_count == 0 || do_lintel == 0) { + mainboard_command.mode = WAIT; + return; + } + + state_do_build_lintel(max_level); + mainboard_command.mode = WAIT; +} + +/* prepare to get the lintel */ +static void state_do_prepare_get_lintel(void) +{ + if (!state_check(PREPARE_GET_LINTEL)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + arm_goto_prepare_get_lintel_disp(); + arm_wait_both(ARM_TRAJ_ALL); + + pump_set(PUMP_LEFT1_NUM, PUMP_OFF); + pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); + + /* go fully left or right */ + if (finger_get_side() == I2C_LEFT_SIDE) + finger_goto(FINGER_LEFT); + else + finger_goto(FINGER_RIGHT); + + while (state_check(PREPARE_GET_LINTEL)); +} + +/* get the lintel from the dispenser */ +static void state_do_get_lintel(void) +{ + if (!state_check(GET_LINTEL)) + return; + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + + pump_set(PUMP_LEFT1_NUM, PUMP_REVERSE); + pump_set(PUMP_RIGHT1_NUM, PUMP_REVERSE); + + arm_goto_get_lintel_disp(); + arm_wait_both(ARM_TRAJ_ALL_NEAR); + + time_wait_ms(200); + + STMCH_DEBUG("%s left1=%d left2=%d", __FUNCTION__, + mechboard.pump_left1_current, + sensor_get_adc(ADC_CSENSE3)); + + while (state_check(GET_LINTEL)); + + /* mainboard asked to release lintel, so release pump first */ + if (state_get_mode() == PREPARE_GET_LINTEL) { + pump_set(PUMP_LEFT1_NUM, PUMP_ON); + pump_set(PUMP_RIGHT1_NUM, PUMP_ON); + time_wait_ms(200); + pump_set(PUMP_LEFT1_NUM, PUMP_OFF); + pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); + } +} + +/* put the lintel inside the robot */ +static void state_do_put_lintel(void) +{ + uint8_t prev_lin_count; + + if (!state_check(PUT_LINTEL)) + return; + + STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); + prev_lin_count = mechboard.lintel_count; + mechboard.lintel_count ++; + + arm_goto_prepare_get_lintel_disp(); + arm_wait_both(ARM_TRAJ_ALL); + + servo_lintel_out(); + + arm_goto_prepare_put_lintel(); + arm_wait_both(ARM_TRAJ_ALL_NEAR); + + arm_goto_put_lintel(prev_lin_count); + arm_wait_both(ARM_TRAJ_ALL); + + pump_set(PUMP_LEFT1_NUM, PUMP_ON); + pump_set(PUMP_RIGHT1_NUM, PUMP_ON); + + if (mechboard.lintel_count == 1) + servo_lintel_1lin(); + else + servo_lintel_2lin(); + + time_wait_ms(300); + + pump_set(PUMP_LEFT1_NUM, PUMP_OFF); + pump_set(PUMP_RIGHT1_NUM, PUMP_OFF); + + arm_goto_prepare_put_lintel(); + arm_wait_both(ARM_TRAJ_ALL_NEAR); + + while (state_check(PUT_LINTEL)); +} + +/* main state machine */ +void state_machine(void) +{ + while (state_get_mode() != EXIT) { + changed = 0; + state_do_init(); + state_do_manual(); + state_do_harvest(); + state_do_lazy_harvest(); + state_do_prepare_pickup(); + state_do_pickup(); + state_do_prepare_inside(); + state_do_prepare_build(); + state_do_autobuild(); + state_do_prepare_get_lintel(); + state_do_get_lintel(); + state_do_put_lintel(); + state_do_loaded(); + state_do_clear(); + state_do_lazy_pickup(); + state_do_wait(); + state_do_store(); + state_do_manivelle(); + state_do_push_temple(); + state_do_push_temple_disc(); + } +} + +void state_init(void) +{ + vt100_init(&local_vt100); + mainboard_command.mode = WAIT; + pump_reset_all(); + mechboard.lintel_count = 1; + mechboard.column_flags = 0; + servo_lintel_1lin(); + finger_goto(FINGER_LEFT); +} diff --git a/projects/microb2010/mechboard/state.h b/projects/microb2010/mechboard/state.h new file mode 100644 index 0000000..0b49890 --- /dev/null +++ b/projects/microb2010/mechboard/state.h @@ -0,0 +1,40 @@ +/* + * 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 + * (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: state.h,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +#ifndef _STATE_H_ +#define _STATE_H_ + +extern volatile uint8_t lintel_count; + +void state_manivelle(int16_t step_deg); + +/* set a new state, return 0 on success */ +int8_t state_set_mode(struct i2c_cmd_mechboard_set_mode *cmd); + +/* get current state */ +uint8_t state_get_mode(void); + +/* launch state machine */ +void state_machine(void); + +void state_init(void); + +#endif diff --git a/projects/microb2010/mechboard/time_config.h b/projects/microb2010/mechboard/time_config.h new file mode 100755 index 0000000..6a1e4c5 --- /dev/null +++ b/projects/microb2010/mechboard/time_config.h @@ -0,0 +1,23 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: time_config.h,v 1.3 2009-04-07 20:03:48 zer0 Exp $ + * + */ + +/** precision of the time processor, in us */ +#define TIME_PRECISION 1000l diff --git a/projects/microb2010/mechboard/timer_config.h b/projects/microb2010/mechboard/timer_config.h new file mode 100755 index 0000000..bbfad76 --- /dev/null +++ b/projects/microb2010/mechboard/timer_config.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2006) + * + * 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: timer_config.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $ + * + */ + +#define TIMER0_ENABLED + +/* #define TIMER1_ENABLED */ +/* #define TIMER1A_ENABLED */ +/* #define TIMER1B_ENABLED */ +/* #define TIMER1C_ENABLED */ + +/* #define TIMER2_ENABLED */ + +/* #define TIMER3_ENABLED */ +/* #define TIMER3A_ENABLED */ +/* #define TIMER3B_ENABLED */ +/* #define TIMER3C_ENABLED */ + +#define TIMER0_PRESCALER_DIV 8 diff --git a/projects/microb2010/mechboard/uart_config.h b/projects/microb2010/mechboard/uart_config.h new file mode 100644 index 0000000..a54811e --- /dev/null +++ b/projects/microb2010/mechboard/uart_config.h @@ -0,0 +1,67 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: uart_config.h,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* compile uart0 fonctions, undefine it to pass compilation */ +#define UART0_COMPILE +#define UART0_ENABLED 1 +#define UART0_INTERRUPT_ENABLED 1 +#define UART0_BAUDRATE 57600 +//#define UART0_BAUDRATE 1000000 +#define UART0_USE_DOUBLE_SPEED 1 +#define UART0_RX_FIFO_SIZE 32 +#define UART0_TX_FIFO_SIZE 32 +#define UART0_NBITS 8 +#define UART0_PARITY UART_PARTITY_NONE +#define UART0_STOP_BIT UART_STOP_BITS_2 + +#define UART1_COMPILE +#define UART1_ENABLED 1 +#define UART1_INTERRUPT_ENABLED 1 +#define UART1_BAUDRATE 57600 +#define UART1_USE_DOUBLE_SPEED 1 +#define UART1_RX_FIFO_SIZE 128 +#define UART1_TX_FIFO_SIZE 128 +#define UART1_NBITS 8 +#define UART1_PARITY UART_PARTITY_NONE +#define UART1_STOP_BIT UART_STOP_BITS_1 + +#define UART3_COMPILE +#define UART3_ENABLED 1 +#define UART3_INTERRUPT_ENABLED 1 +#define UART3_BAUDRATE 57600 +#define UART3_USE_DOUBLE_SPEED 1 +#define UART3_RX_FIFO_SIZE 128 +#define UART3_TX_FIFO_SIZE 128 +#define UART3_NBITS 8 +#define UART3_PARITY UART_PARTITY_NONE +#define UART3_STOP_BIT UART_STOP_BITS_1 + +/* .... same for uart 1, 2, 3 ... */ + +#endif + diff --git a/projects/microb2010/microb_cmd/microbcmd.py b/projects/microb2010/microb_cmd/microbcmd.py new file mode 100755 index 0000000..5080927 --- /dev/null +++ b/projects/microb2010/microb_cmd/microbcmd.py @@ -0,0 +1,929 @@ +#! /usr/bin/env python + +import os,sys,termios,atexit +import serial +from select import select +import cmd +#import pylab +from matplotlib import pylab +from math import * + +import numpy +import shlex +import time +import math +import warnings +warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__) + +import logging +log = logging.getLogger("MicrobShell") +_handler = logging.StreamHandler() +_handler.setFormatter(logging.Formatter("%(levelname)s: %(message)s")) +log.addHandler(_handler) +log.setLevel(1) + +MICROB_PATH=os.path.dirname(sys.argv[0]) + +SPM_PAGE_SIZE = 256 + +def crc_ccitt_update (crc, data): + """crc argument is the previous value of 16 bits crc (the initial + value is 0xffff). 'data' is the 8 bits value added to crc. The + function returns the new crc value.""" + + data ^= (crc & 0xff) + data ^= (data << 4) + data &= 0xff + + ret = (data << 8) & 0xffff + ret |= ((crc >> 8) & 0xff) + ret ^= ((data >> 4) & 0xff) + ret ^= ((data << 3) & 0xffff) + return ret + +def do_crc(buf): + i = 0 + crc = 0xffff + sum = 0 + while i < len(buf): + crc = crc_ccitt_update(crc, ord(buf[i])) + sum += ord(buf[i]) + i += 1 + return (crc << 16) + (sum & 0xffff) + +def prog_page(ser, addr, buf): + """program a page from buf at addr""" + + # switch in program mode + ser.flushInput() + ser.write('p') + + # send address + s = ser.readline() + if not s.endswith("addr?\r\n"): + print "failed (don't match addr)" + return -1 + ser.write("%x\n"%addr) + s = ser.readline() + if not s.startswith("ok"): + print "failed" + return -1 + + # fill page with buf data + page = [ '\xff' ] * SPM_PAGE_SIZE + i = 0 + while i < SPM_PAGE_SIZE and i < len(buf): + page[i] = buf[i] + i += 1 + + # send data + i = 0 + while i < SPM_PAGE_SIZE: + c = page[i] + ser.write(c) + i += 1 + + sys.stdout.write(".") + sys.stdout.flush() + + # compare crc + avr_crc = int(ser.readline()[0:8], 16) + + crc = do_crc(page) + if crc != avr_crc: + print "failed: bad crc %x %x"%(crc, avr_crc) + ser.write('n') + return -1 + + ser.write('y') + s = ser.readline() + if not s.startswith("OK"): + print "failed" + return -1 + return 0 + +def read32(ser, addr): + """read a 32 bits value at addr""" + + # switch in program mode + ser.flushInput() + ser.write('d') + + # send address + s = ser.readline() + if not s.endswith("addr?\r\n"): + print "failed (don't match addr)" + return -1 + ser.write("%x\n"%addr) + s = ser.readline() + return int(s) + +def check_crc(ser, buf, offset, size): + """Process the crc of buf, ask for a crc of the flash, and check + that value is correct""" + if size <= 0: + return 0 + + # go in CRC mode + ser.flushInput() + ser.write('c') + + # send addr + s = ser.readline() + if not s.endswith("addr?\r\n"): + print "failed <%s>"%s + return -1 + ser.write("%x\n"%offset) + + # send size + s = ser.readline() + if not s.startswith("size?"): + print "failed" + return -1 + ser.write("%x\n"%size) + + # compare CRC + crc = do_crc(buf[offset:offset+size]) + avr_crc = int(ser.readline()[0:8], 16) + if crc != avr_crc: + return -1 + return 0 + +class SerialLogger: + def __init__(self, ser, filein, fileout=None): + self.ser = ser + self.filein = filein + self.fin = open(filein, "a", 0) + if fileout: + self.fileout = fileout + self.fout = open(fileout, "a", 0) + else: + self.fileout = filein + self.fout = self.fin + def fileno(self): + return self.ser.fileno() + def read(self, *args): + res = self.ser.read(*args) + self.fin.write(res) + return res + def write(self, s): + self.fout.write(s) + self.ser.write(s) + + + + + +""" +fig = figure() + +ax = subplot(111) + + +X = 45. +Y = -10. +l1 = 9. +l2 = 21.13 +l3 = 47.14 + +l_mirror = 249. +h_mirror = 13. + + +def ang2_a_mirror(b): + x2 = X+l1*math.cos(b) + y2 = Y+l1*math.sin(b) + + A = (l3**2+x2**2+y2**2-l2**2)/(2*l3) + + DELTA = -(A**2-x2**2-y2**2) + B = +math.sqrt(DELTA) + + D = x2**2+y2**2 + + c_a = (x2*A+y2*B)/D + s_a = -(x2*B-y2*A)/D + + a = math.atan2(s_a, c_a) + return x2, y2, c_a, s_a, a + + +def ang2_H_L(l_telemetre, c_a, s_a, a): + d = h_mirror*c_a/s_a + H = (l_telemetre - l_mirror - d)*math.sin(2*a) + L = l_mirror + d + H/math.tan(2*a) + return H, L + +all_p = [] +for b in xrange(0, 360, 20): + b = b*2*math.pi / 360. + + x2, y2, c_a, s_a, a = ang2_a_mirror(b) + x1 = l3*c_a + y1 = l3*s_a + + px = [0, x1, x2, X] + py = [0, y1, y2, Y] + + all_p+=[px, py] + + print math.sqrt((x2-x1)**2+(y2-y1)**2) + + H, L = ang2_H_L(400., c_a, s_a, a) + print H, L + +ax.plot(*all_p) + +show() + +""" + + + + +class Interp(cmd.Cmd): + prompt = "Microb> " + def __init__(self, tty, baudrate=57600): + cmd.Cmd.__init__(self) + self.ser = serial.Serial(tty,baudrate=baudrate) + self.escape = "\x01" # C-a + self.quitraw = "\x02" # C-b + self.serial_logging = False + self.default_in_log_file = "/tmp/microb.in.log" + self.default_out_log_file = "/tmp/microb.out.log" + + def do_quit(self, args): + return True + + def do_log(self, args): + """Activate serial logs. + log logs input and output to + log logs input to and output to + log logs to /tmp/microb.log or the last used file""" + + if self.serial_logging: + log.error("Already logging to %s and %s" % (self.ser.filein, + self.ser.fileout)) + else: + self.serial_logging = True + files = [os.path.expanduser(x) for x in args.split()] + if len(files) == 0: + files = [self.default_in_log_file, self.default_out_log_file] + elif len(files) == 1: + self.default_in_log_file = files[0] + self.default_out_log_file = None + elif len(files) == 2: + self.default_in_log_file = files[0] + self.default_out_log_file = files[1] + else: + print "Can't parse arguments" + + self.ser = SerialLogger(self.ser, *files) + log.info("Starting serial logging to %s and %s" % (self.ser.filein, + self.ser.fileout)) + + + def do_unlog(self, args): + if self.serial_logging: + log.info("Stopping serial logging to %s and %s" % (self.ser.filein, + self.ser.fileout)) + self.ser = self.ser.ser + self.serial_logging = False + else: + log.error("No log to stop") + + + def do_raw(self, args): + "Switch to RAW mode" + stdin = os.open("/dev/stdin",os.O_RDONLY) + stdout = os.open("/dev/stdout",os.O_WRONLY) + + stdin_termios = termios.tcgetattr(stdin) + raw_termios = stdin_termios[:] + + try: + log.info("Switching to RAW mode") + + # iflag + raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT | + termios.PARMRK | termios.ISTRIP | + termios.INLCR | termios.IGNCR | + termios.ICRNL | termios.IXON) + # oflag + raw_termios[1] &= ~termios.OPOST; + # cflag + raw_termios[2] &= ~(termios.CSIZE | termios.PARENB); + raw_termios[2] |= termios.CS8; + # lflag + raw_termios[3] &= ~(termios.ECHO | termios.ECHONL | + termios.ICANON | termios.ISIG | + termios.IEXTEN); + + termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios) + + mode = "normal" + while True: + ins,outs,errs=select([stdin,self.ser],[],[]) + for x in ins: + if x == stdin: + c = os.read(stdin,1) + if mode == "escape": + mode =="normal" + if c == self.escape: + self.ser.write(self.escape) + elif c == self.quitraw: + return + else: + self.ser.write(self.escape) + self.ser.write(c) + else: + if c == self.escape: + mode = "escape" + else: + self.ser.write(c) + elif x == self.ser: + os.write(stdout,self.ser.read()) + finally: + termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios) + log.info("Back to normal mode") + + + def do_arm_x(self, args): + fsdf + my_h = 100 + my_r = 220 + my_ang = 90 + + self.ser.write("armxy %d %d %d\n"%(my_h, -my_r, my_ang)) + time.sleep(1) + + for i in xrange(-my_r, my_r, 25): + self.ser.write("armxy %d %d %d\n"%(my_h, i, my_ang)) + self.ser.flushInput() + + time.sleep(0.03) + + def do_arm_y(self, args): + my_x = 80 + my_r = 145 + my_ang = 0 + self.ser.write("armxy %d %d %d\n"%(-my_r, my_x, my_ang)) + time.sleep(1) + + for i in xrange(-my_r, my_r, 25): + self.ser.write("armxy %d %d %d\n"%(i, my_x, my_ang)) + self.ser.flushInput() + + time.sleep(0.03) + + def do_arm_circ(self, args): + add_h = 120 + add_d = 120 + l = 70 + for i in xrange(0, 360, 10): + x = l*math.cos(i*math.pi/180) + y = l*math.sin(i*math.pi/180) + + + self.ser.write("armxy %d %d 90\n"%(x+add_h, y+add_d)) + self.ser.flushInput() + + time.sleep(0.05) + + def do_arm_init(self, args): + self.arm_h = 130 + self.arm_v = 130 + self.mov_max = 20 + + self.ser.write("armxy %d %d\n"%(self.arm_h, self.arm_v)) + + def arm_py_goto(self, h, v, a): + """ + dh, dv = h-self.arm_h, v-self.arm_v + d = math.sqrt(dh**2 + dv**2) + + old_h = self.arm_h + old_v = self.arm_v + + mov_todo = int(d/self.mov_max) + for i in xrange(1, mov_todo): + p_h = dh*i/mov_todo + p_v = dv*i/mov_todo + + new_h = old_h+p_h + new_v = old_v+p_v + + self.ser.write("armxy %d %d %d\n"%(new_h, new_v, a)) + self.ser.flushInput() + self.arm_h = new_h + self.arm_v = new_v + + time.sleep(0.04) + + self.ser.write("armxy %d %d %d\n"%(h, v, a)) + self.ser.flushInput() + """ + + self.ser.write("armxy %d %d %d\n"%(h, v, a)) + self.ser.flushInput() + + time.sleep(0.2) + + + + def do_arm_tt(self, args): + for i in xrange(2): + self.arm_py_goto(80, 80, 200) + self.arm_py_goto(80, 200, 200) + self.arm_py_goto(200, 200, 200) + self.arm_py_goto(200, 80, 200) + + def do_arm_harve(self, args): + angl1 = 1 + angl2 = 100 + my_time = 0.03 + self.arm_py_goto(130,130,angl1) + self.arm_py_goto(-150,60,angl1) + time.sleep(0.1) + + self.ser.write("pwm 1B -3000\n") + self.ser.flushInput() + time.sleep(0.2) + + self.arm_py_goto(-120,60,angl1) + time.sleep(2) + self.arm_py_goto(-120,60,angl2) + time.sleep(2) + self.arm_py_goto(-150,60,angl2) + self.ser.write("pwm 3C -3000\n") + self.ser.flushInput() + time.sleep(0.2) + self.arm_py_goto(-130,60,angl2) + self.arm_py_goto(0,160,angl2) + + #middle point + self.arm_py_goto(-40,200,angl2) + + h = -150 + d = 210 + + self.arm_py_goto(h,d,angl2) + time.sleep(.3) + self.ser.write("pwm 3C 3000\n") + time.sleep(0.1) + self.arm_py_goto(h+60,d,angl2) + time.sleep(0.1) + + self.arm_py_goto(h+60,d,angl1) + time.sleep(0.3) + self.arm_py_goto(h+40,d,angl1) + time.sleep(0.3) + self.arm_py_goto(h+30,d,angl1) + time.sleep(0.3) + self.ser.write("pwm 1B 3000\n") + time.sleep(0.1) + self.arm_py_goto(h+70,d,angl1) + + self.ser.write("pwm 1B 0\n") + self.ser.write("pwm 3C 0\n") + + self.arm_py_goto(130,130,angl2) + + + + def update_graph(self, val): + freq = self.sfreq.val + self.theta_max = freq*math.pi*2.0 + self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r)) + self.theta = self.theta[:len(self.r)] + + self.myplot.set_xdata(self.theta) + draw() + """ + def do_graph(self, args): + self.ser.write("pwm 1A 2000\n") + time.sleep(0.5) + print "sampling..." + self.ser.write("sample start\n") + while True: + l = self.ser.readline() + if "dump end" in l: + break + #time.sleep(2) + self.ser.write("pwm 1A 0\n") + l = self.ser.readline() + l = self.ser.readline() + + print "dumping..." + self.ser.write("sample dump\n") + vals = [] + while True: + l = self.ser.readline() + if l[0] in ['s', 'c', 'a']: + continue + if l[0] in ['e']: + break + tokens = [x for x in shlex.shlex(l)] + v = int(tokens[0]) + #v = min(v, 150) + vals.append(v) + vals.reverse() + print "total vals:", len(vals) + + pylab.subplot(111, polar = True) + self.r = vals + valinit = 5.38 + #theta_max = 4.8*2.3*pi + self.theta_max =valinit*pylab.pi + self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r)) + + self.myplot, = pylab.plot(self.theta, self.r) + + #slide bar + axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03]) + self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit) + self.sfreq.on_changed(self.update_graph) + + pylab.show() + """ + + + def do_dump(self, args): + + t = [x for x in shlex.shlex(args)] + + t.reverse() + do_img = False + + #send speed,debug=off + #self.ser.write("scan_params 500 0\n") + #send algo 1 wrkazone 1 cx 15 cy 15 + self.ser.write("scan_img 1 1 15 15\n") + + print t + while len(t): + x = t.pop() + if x == 'img': + do_img = True + + print "dumping..." + self.ser.write("sample dump 0 0 400 0\n") + + + + while True: + l = self.ser.readline() + + if "start dumping" in l: + tokens = [x for x in shlex.shlex(l)] + num_rows = int(tokens[-1]) + print "num row: ", num_rows + break + print l.strip() + #scan_stop = time.time() + #print "total time:", scan_stop-scan_start + + + vals = [] + while True: + l = self.ser.readline() + + if l[0] in ['s', 'c', 'a']: + continue + if l[0] in ['e']: + break + tokens = [x for x in shlex.shlex(l)] + v = int(tokens[0]) + #v = min(v, 150) + vals.append(v) + + + #vals.reverse() + print "total vals:", len(vals) + valinit = 5 + + #num_rows = int(600/valinit) + #num_cols = int(valinit) + num_rows_orig = num_rows + num_rows *= 1 + num_cols = len(vals)/num_rows + + data = [] + pt_num = 0 + my_min = None + my_max = None + + print "dim", num_rows, num_cols + print "sav img to pgm" + fimg = open("dump.pgm", "wb") + fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols)) + for i in xrange(num_cols): + data.append([]) + #data[-1].append(0.0) + + for j in xrange(num_rows): + if vals[pt_num]>0x10: + p = 0 + else: + p=vals[pt_num] * 0x20 + if (p>0xFF): + p = 0xFF + + fimg.write(chr(p)) + if my_min == None or my_min>p: + my_min = p + if p!=255 and (my_max == None or my_max= 205: + p = 0 + p/=1. + + + + data[-1].append(p) + pt_num+=1 + #data[-1].append(1.) + fimg.close() + print my_min, my_max + #print data + data = numpy.array(data) + + if do_img: + ax = pylab.subplot(111) + ax.imshow(data) + + + #pylab.subplot(111, polar = True) + self.r = vals + #theta_max = 4.8*2.3*pi + self.theta_max =valinit*pylab.pi + self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r)) + + """ + tmp = [] + for x in data: + tmp+=list(x) + self.myplot, = pylab.plot(tmp) + + + """ + if not do_img : + tmpx = [] + tmpy = [] + for x in data: + tmpy+=list(x) + tmpx+=range(len(x)) + self.myplot, = pylab.plot(tmpx, tmpy) + + + #slide bar + #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03]) + #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit) + #self.sfreq.on_changed(self.update_graph) + + pylab.show() + + + def do_scan_params(self, args): + t = [x for x in shlex.shlex(args)] + + if len(t)!=2: + return + t = [int(x) for x in t] + self.ser.write("scan_params %d %d\n"%tuple(t)) + + def do_graph(self, args): + t = [x for x in shlex.shlex(args)] + + t.reverse() + do_img = False + + #send speed,debug=off + #self.ser.write("scan_params 500 0\n") + #send algo 1 wrkazone 1 cx 15 cy 15 + self.ser.write("scan_img 1 1 15 15\n") + + print t + while len(t): + x = t.pop() + if x == 'img': + do_img = True + + + scan_start = time.time() + print "sampling..." + + self.ser.write("scan_do\n") + + flog = open('log.txt', 'w') + + while True: + l = self.ser.readline() + flog.write(l) + + if "dump end" in l: + break + flog.close() + + #time.sleep(2) + #self.ser.write("pwm 1A 0\n") + #l = self.ser.readline() + #l = self.ser.readline() + + + print "dumping..." + self.ser.write("sample dump 0 0 400 0\n") + + + + while True: + l = self.ser.readline() + + if "start dumping" in l: + tokens = [x for x in shlex.shlex(l)] + num_rows = int(tokens[-1]) + print "num row: ", num_rows + break + print l.strip() + scan_stop = time.time() + print "total time:", scan_stop-scan_start + + + vals = [] + while True: + l = self.ser.readline() + + if l[0] in ['s', 'c', 'a']: + continue + if l[0] in ['e']: + break + tokens = [x for x in shlex.shlex(l)] + v = int(tokens[0]) + #v = min(v, 150) + vals.append(v) + + + #vals.reverse() + print "total vals:", len(vals) + valinit = 5 + + #num_rows = int(600/valinit) + #num_cols = int(valinit) + num_rows_orig = num_rows + num_rows *= 1 + num_cols = len(vals)/num_rows + + data = [] + pt_num = 0 + my_min = None + my_max = None + + print "dim", num_rows, num_cols + print "sav img to pgm" + fimg = open("dump.pgm", "wb") + fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols)) + for i in xrange(num_cols): + data.append([]) + #data[-1].append(0.0) + + for j in xrange(num_rows): + if vals[pt_num]>0x10: + p = 0 + else: + p=vals[pt_num] * 0x20 + if (p>0xFF): + p = 0xFF + + fimg.write(chr(p)) + if my_min == None or my_min>p: + my_min = p + if p!=255 and (my_max == None or my_max= 205: + p = 0 + p/=1. + + + + data[-1].append(p) + pt_num+=1 + #data[-1].append(1.) + fimg.close() + print my_min, my_max + #print data + data = numpy.array(data) + + if do_img: + ax = pylab.subplot(111) + ax.imshow(data) + + + #pylab.subplot(111, polar = True) + self.r = vals + #theta_max = 4.8*2.3*pi + self.theta_max =valinit*pylab.pi + self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r)) + + """ + tmp = [] + for x in data: + tmp+=list(x) + self.myplot, = pylab.plot(tmp) + + + """ + if not do_img : + tmpx = [] + tmpy = [] + for x in data: + tmpy+=list(x) + tmpx+=range(len(x)) + self.myplot, = pylab.plot(tmpx, tmpy) + + + #slide bar + #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03]) + #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit) + #self.sfreq.on_changed(self.update_graph) + + pylab.show() + + + def bootloader(self, filename, boardnum): + self.ser.write("\n") + time.sleep(0.4) + self.ser.write("bootloader\n") + time.sleep(0.4) + self.ser.write("\n") + + print "start programming" + self.ser.flushInput() + f = open(filename) + buf = f.read() + addr = 0 + while addr < len(buf): + time.sleep(0.1) + if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0: + sys.stdout.write("*") + sys.stdout.flush() + elif prog_page(self.ser, addr, + buf[addr:addr+SPM_PAGE_SIZE]) != 0: + return + addr += SPM_PAGE_SIZE + if check_crc(self.ser, buf, 0, len(buf)): + print "crc failed" + return + print "Done." + self.ser.write("x") + self.do_raw("") + + def do_bootloader(self, args): + self.bootloader(args, 0) + + def do_mainboard(self, args): + filename = os.path.join(MICROB_PATH, "../mainboard/main.bin") + self.bootloader(filename, 1) + + def do_mechboard(self, args): + filename = os.path.join(MICROB_PATH, "../mechboard/main.bin") + self.bootloader(filename, 2) + + def do_sensorboard(self, args): + filename = os.path.join(MICROB_PATH, "../sensorboard/main.bin") + self.bootloader(filename, 3) + + def do_toto(self, args): + for i in range(10): + time.sleep(1) + self.ser.write("pwm s3(3C) 200\n") + time.sleep(1) + self.ser.write("pwm s3(3C) 250\n") + +if __name__ == "__main__": + try: + import readline,atexit + except ImportError: + pass + else: + histfile = os.path.join(os.environ["HOME"], ".microb_history") + atexit.register(readline.write_history_file, histfile) + try: + readline.read_history_file(histfile) + except IOError: + pass + + device = "/dev/ttyS0" + if len(sys.argv) > 1: + device = sys.argv[1] + interp = Interp(device) + while 1: + try: + interp.cmdloop() + except KeyboardInterrupt: + print + except Exception,e: + l = str(e).strip() + if l: + log.exception("%s" % l.splitlines()[-1]) + continue + break diff --git a/projects/microb2010/sensorboard/.config b/projects/microb2010/sensorboard/.config new file mode 100644 index 0000000..bc9e293 --- /dev/null +++ b/projects/microb2010/sensorboard/.config @@ -0,0 +1,279 @@ +# +# Automatically generated make config: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +# CONFIG_MCU_ATMEGA8 is not set +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +# CONFIG_MCU_ATMEGA128 is not set +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +CONFIG_MCU_ATMEGA2560=y +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +CONFIG_MATH_LIB=y +# CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set +# CONFIG_MINIMAL_PRINTF is not set +# CONFIG_STANDARD_PRINTF is not set +CONFIG_ADVANCED_PRINTF=y +# CONFIG_FORMAT_IHEX is not set +# CONFIG_FORMAT_SREC is not set +CONFIG_FORMAT_BINARY=y + +# +# Base modules +# + +# +# Enable math library in generation options to see all modules +# +CONFIG_MODULE_CIRBUF=y +# CONFIG_MODULE_CIRBUF_LARGE is not set +# CONFIG_MODULE_FIXED_POINT is not set +# CONFIG_MODULE_VECT2 is not set +CONFIG_MODULE_GEOMETRY=y +CONFIG_MODULE_SCHEDULER=y +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +# CONFIG_MODULE_SCHEDULER_TIMER0 is not set +CONFIG_MODULE_SCHEDULER_MANUAL=y +CONFIG_MODULE_TIME=y +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# + +# +# uart needs circular buffer, mf2 client may need scheduler +# +CONFIG_MODULE_UART=y +# CONFIG_MODULE_UART_9BITS is not set +CONFIG_MODULE_UART_CREATE_CONFIG=y +CONFIG_MODULE_SPI=y +CONFIG_MODULE_SPI_CREATE_CONFIG=y +CONFIG_MODULE_I2C=y +CONFIG_MODULE_I2C_MASTER=y +# CONFIG_MODULE_I2C_MULTIMASTER is not set +CONFIG_MODULE_I2C_CREATE_CONFIG=y +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +CONFIG_MODULE_TIMER=y +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +CONFIG_MODULE_PWM_NG=y +CONFIG_MODULE_ADC=y +CONFIG_MODULE_ADC_CREATE_CONFIG=y + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +CONFIG_MODULE_VT100=y +CONFIG_MODULE_RDLINE=y +CONFIG_MODULE_RDLINE_CREATE_CONFIG=y +CONFIG_MODULE_RDLINE_KILL_BUF=y +CONFIG_MODULE_RDLINE_HISTORY=y +CONFIG_MODULE_PARSE=y +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +CONFIG_MODULE_AX12=y +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +# CONFIG_MODULE_ENCODERS_MICROB is not set +# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +CONFIG_MODULE_ENCODERS_SPI=y +CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y + +# +# Robot specific modules +# +# CONFIG_MODULE_ROBOT_SYSTEM is not set +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +# CONFIG_MODULE_POSITION_MANAGER is not set +# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set +# CONFIG_MODULE_TRAJECTORY_MANAGER is not set +CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y +# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set + +# +# Control system modules +# +CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y + +# +# Filters +# +CONFIG_MODULE_PID=y +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +CONFIG_MODULE_QUADRAMP=y +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# + +# +# Some radio devices require SPI to be activated +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# + +# +# Crypto modules depend on utils module +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# + +# +# Encoding modules depend on utils module +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# + +# +# Debug modules depend on utils module +# +CONFIG_MODULE_DIAGNOSTIC=y +CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y +CONFIG_MODULE_ERROR=y +CONFIG_MODULE_ERROR_CREATE_CONFIG=y + +# +# Programmer options +# +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/sensorboard/Makefile b/projects/microb2010/sensorboard/Makefile new file mode 100644 index 0000000..3013479 --- /dev/null +++ b/projects/microb2010/sensorboard/Makefile @@ -0,0 +1,46 @@ +TARGET = main + +# repertoire des modules +AVERSIVE_DIR = ../../.. +# VALUE, absolute or relative path : example ../.. # + +CFLAGS += -Werror +LDFLAGS = -T ../common/avr6.x + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c +SRC += commands_cs.c commands_sensorboard.c commands.c commands_scan.c +SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c +SRC += beacon.c +SRC += img_processing.c +SRC += scanner.c + + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + +######################################## + +-include .aversive_conf +include $(AVERSIVE_DIR)/mk/aversive_project.mk + +scan_h_l.h: + @gcc gen_scan_tab.c -o gen_scan_tab -lm; \ + if ./gen_scan_tab > /dev/null 2>&1; then \ + echo ok; \ + else \ + echo nok; \ + fi + +AVRDUDE_DELAY=50 + +program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep + echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + diff --git a/projects/microb2010/sensorboard/actuator.c b/projects/microb2010/sensorboard/actuator.c new file mode 100644 index 0000000..2895c5a --- /dev/null +++ b/projects/microb2010/sensorboard/actuator.c @@ -0,0 +1,61 @@ +/* + * 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 + * (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.2 2009-04-24 19:30:42 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "main.h" + +#define PICKUP_WHEEL_L_ON 2000 +#define PICKUP_WHEEL_R_ON -2000 +#define PICKUP_WHEEL_L_OFF 0 +#define PICKUP_WHEEL_R_OFF 0 + +void pickup_wheels_on(void) +{ + pwm_ng_set(PICKUP_WHEEL_L_PWM, PICKUP_WHEEL_L_ON); + pwm_ng_set(PICKUP_WHEEL_R_PWM, PICKUP_WHEEL_R_ON); +} + +void pickup_wheels_off(void) +{ + pwm_ng_set(PICKUP_WHEEL_L_PWM, PICKUP_WHEEL_L_OFF); + pwm_ng_set(PICKUP_WHEEL_R_PWM, PICKUP_WHEEL_R_OFF); +} + diff --git a/projects/microb2010/sensorboard/actuator.h b/projects/microb2010/sensorboard/actuator.h new file mode 100644 index 0000000..a1a535d --- /dev/null +++ b/projects/microb2010/sensorboard/actuator.h @@ -0,0 +1,25 @@ +/* + * 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 + * (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.h,v 1.2 2009-04-24 19:30:42 zer0 Exp $ + * + */ + +void pickup_wheels_on(void); +void pickup_wheels_off(void); + + diff --git a/projects/microb2010/sensorboard/adc_config.h b/projects/microb2010/sensorboard/adc_config.h new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/sensorboard/ax12_config.h b/projects/microb2010/sensorboard/ax12_config.h new file mode 100755 index 0000000..072e8fb --- /dev/null +++ b/projects/microb2010/sensorboard/ax12_config.h @@ -0,0 +1,7 @@ +#ifndef _AX12_CONFIG_H_ +#define _AX12_CONFIG_H_ + +#define AX12_MAX_PARAMS 32 + + +#endif/*_AX12_CONFIG_H_*/ diff --git a/projects/microb2010/sensorboard/ax12_user.c b/projects/microb2010/sensorboard/ax12_user.c new file mode 100644 index 0000000..231495e --- /dev/null +++ b/projects/microb2010/sensorboard/ax12_user.c @@ -0,0 +1,169 @@ +/* + * 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 + * (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: ax12_user.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $ + * + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" + +/* + * Cmdline interface for AX12. Use the PC to command a daisy-chain of + * AX12 actuators with a nice command line interface. + * + * The circuit should be as following: + * + * |----------| + * | uart3|------->--- PC (baudrate=57600) + * | |-------<--- + * | atmega128| + * | | + * | uart0|---->---+-- AX12 (baudrate 115200) + * | |----<---| + * |----------| + * + * Note that RX and TX pins of UART1 are connected together to provide + * a half-duplex UART emulation. + * + */ + +#define UART_AX12_NUM 0 +#define UCSRxB UCSR0B +#define AX12_TIMEOUT 5000UL /* in us */ + +/********************************* AX12 commands */ + +/* + * We use synchronous access (not interrupt driven) to the hardware + * UART, because we have to be sure that the transmission/reception is + * really finished when we return from the functions. + * + * We don't use the CM-5 circuit as described in the AX12 + * documentation, we simply connect TX and RX and use TXEN + RXEN + + * DDR to manage the port directions. + */ + +static volatile uint8_t ax12_state = AX12_STATE_READ; +extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */ +static volatile uint8_t ax12_nsent = 0; + +/* Called by ax12 module to send a character on serial line. Count the + * number of transmitted bytes. It will be used in ax12_recv_char() to + * drop the bytes that we transmitted. */ +static int8_t ax12_send_char(uint8_t c) +{ + uart_send(UART_AX12_NUM, c); + ax12_nsent++; + return 0; +} + +/* for atmega256 */ +#ifndef TXEN +#define TXEN TXEN0 +#endif + +/* called by uart module when the character has been written in + * UDR. It does not mean that the byte is physically transmitted. */ +static void ax12_send_callback(char c) +{ + if (ax12_state == AX12_STATE_READ) { + /* disable TX when last byte is pushed. */ + if (CIRBUF_IS_EMPTY(&g_tx_fifo[UART_AX12_NUM])) + UCSRxB &= ~(1< AX12_TIMEOUT) + return -1; + } + return c; +} + +/* called by ax12 module when we want to switch serial line. As we + * work in interruption mode, this function can be called to switch + * back in read mode even if the bytes are not really transmitted on + * the line. That's why in this case we do nothing, we will fall back + * in read mode in any case when xmit is finished -- see in + * ax12_send_callback() -- */ +static void ax12_switch_uart(uint8_t state) +{ + uint8_t flags; + + if (state == AX12_STATE_WRITE) { + IRQ_LOCK(flags); + ax12_nsent=0; + while (uart_recv_nowait(UART_AX12_NUM) != -1); + UCSRxB |= (1< + * + * 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: ax12_user.h,v 1.2 2009-04-07 20:03:48 zer0 Exp $ + * + */ + +/* This is the ax12 user interface. It initializes the aversive AX12 + * module so that it works in background, using interrupt driver uart. + * + * Be carreful, a call to AX12 module is synchronous and uses + * interruptions, so interrupts must be enabled. On the other side, a + * call _must not_ interrupt another one. That's why all calls to the + * module are done either in init() functions or in a scheduler event + * with prio=ARM_PRIO. + */ + +/* XXX do a safe_ax12() function that will retry once or twice if we + * see some problems. */ + +void ax12_user_init(void); diff --git a/projects/microb2010/sensorboard/beacon.c b/projects/microb2010/sensorboard/beacon.c new file mode 100755 index 0000000..34f1686 --- /dev/null +++ b/projects/microb2010/sensorboard/beacon.c @@ -0,0 +1,397 @@ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "sensor.h" + +#include "../common/i2c_commands.h" +#include "main.h" +#include "beacon.h" + +struct beacon beacon; + +#define BEACON_PWM_VALUE 1000 +#define IR_SENSOR() (!!(PINK&(1<<2))) +#define MODULO_TIMER (1023L) +#define COEFF_TIMER (2) +#define COEFF_MULT (100L) +#define COEFF_MULT2 (1000L) +#define BEACON_SIZE (9) +#define BEACON_MAX_SAMPLE (3) + +#define OPPONENT_POS_X (11) +#define OPPONENT_POS_Y (22) + +#define BEACON_DEBUG(args...) DEBUG(E_USER_BEACON, args) +#define BEACON_NOTICE(args...) NOTICE(E_USER_BEACON, args) +#define BEACON_ERROR(args...) ERROR(E_USER_BEACON, args) + +static volatile int32_t rising = -1; +static volatile int32_t falling = -1; + +static int32_t get_dist(float size); +static int32_t get_angle(int32_t middle, int32_t ref); + +static int32_t pos_ref = 0; +static int32_t invalid_count = 0; + +static volatile int32_t beacon_speed; +static volatile int32_t beacon_save_count = 0; +static volatile int32_t beacon_prev_save_count = 0; +static volatile int32_t count = 0; +static volatile int32_t count_diff_rising = 0; +static volatile int32_t count_diff_falling = 0; +static int32_t beacon_coeff = 0; + +static volatile int8_t valid_beacon = 0; + +static volatile int32_t beacon_pos; + +//static int32_t beacon_sample_size[BEACON_MAX_SAMPLE]; + +int32_t encoders_spi_get_value_beacon(void *number) +{ + int32_t ret; + + ret = encoders_spi_get_value(number); + return ret*4; +} + + +void encoders_spi_set_value_beacon(void * number, int32_t v) +{ + encoders_spi_set_value(number, v/4); +} + +int32_t encoders_spi_update_beacon_speed(void * number) +{ + int32_t ret; + uint8_t flags; + + IRQ_LOCK(flags); + ret = encoders_spi_get_value_beacon(number); + beacon_speed = ret - beacon_pos; + beacon_pos = ret; + beacon_prev_save_count = beacon_save_count; + beacon_save_count = TCNT3; + IRQ_UNLOCK(flags); + + beacon_coeff = COEFF_TIMER * COEFF_MULT;//beacon_speed * COEFF_MULT / ((beacon_prev_save_count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER); + + return beacon_speed; +} + + +void beacon_init(void) +{ + //int8_t i; + + beacon_reset_pos(); + pos_ref = encoders_spi_get_value_beacon(BEACON_ENCODER); + + memset(&beacon, 0, sizeof(struct beacon)); + beacon.opponent_x = I2C_OPPONENT_NOT_THERE; + + beacon_speed = 0; + + /*for(i=0;i falling, so invert both and recalculate size and middle */ + if(local_falling < local_rising){ + temp = local_rising; + local_rising = local_falling; + local_falling = temp; + size = BEACON_STEP_TOUR - local_falling + local_rising; + middle = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR); + edge = local_falling; + } + /* else rising > falling */ + else{ + size = local_falling - local_rising; + middle = local_rising + (size / 2); + edge = local_rising; + } + + //for(i=BEACON_MAX_SAMPLE-1;i>0;i--){ + // beacon_sample_size[i] = beacon_sample_size[i-1]; + // total_size += beacon_sample_size[i]; + //} + //beacon_sample_size[0] = size; + //total_size += size; + //total_size /= BEACON_MAX_SAMPLE; + + //BEACON_DEBUG("rising2= %ld\t",local_rising); + //BEACON_DEBUG("falling2= %ld\r\n",local_falling); + /* BEACON_DEBUG("size= %ld %ld\t",size, total_size); */ + BEACON_DEBUG("size= %f\r\n",size); + //BEACON_DEBUG("middle= %ld\r\n",middle); + + local_angle = get_angle(middle,0); + BEACON_NOTICE("opponent angle= %ld\t",local_angle); + + local_dist = get_dist(size); + BEACON_NOTICE("opponent dist= %ld\r\n",local_dist); + + beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y); + + IRQ_LOCK(flags); + beacon.opponent_x = result_x; + beacon.opponent_y = result_y; + beacon.opponent_angle = local_angle; + beacon.opponent_dist = local_dist; + /* for I2C test */ + //beacon.opponent_x = OPPONENT_POS_X; + //beacon.opponent_y = OPPONENT_POS_Y; + IRQ_UNLOCK(flags); + + BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x); + BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y); + } + else { + BEACON_NOTICE("non valid\r\n\n"); + } + + falling = -1; +} + +static int32_t get_dist(float size) +{ + int32_t dist=0; + //int32_t alpha=0; + + //alpha = (size*2*M_PI*COEFF_MULT2); + //dist = ((2*BEACON_SIZE*BEACON_STEP_TOUR*COEFF_MULT2)/alpha)/2; + + /* function found by measuring points up to 80cm */ + //dist = ((size - 600)*(size - 600)) / 2400 +28; + + /* new function */ + /* dist = a0 + a1*x + a2*x² + a3x³ */ + dist = 1157.3 + (1.4146*size) + (-0.013508*size*size) + (0.00001488*size*size*size); + + return dist; + +} + +static int32_t get_angle(int32_t middle, int32_t ref) +{ + int32_t ret_angle; + + ret_angle = (middle - ref) * 360 / BEACON_STEP_TOUR; + + if(ret_angle > 360) + ret_angle -= 360; + + return ret_angle; +} + +void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y) +{ + uint8_t flags; + + int32_t local_x; + int32_t local_y; + int32_t x_opponent; + int32_t y_opponent; + int32_t local_robot_angle; + + IRQ_LOCK(flags); + local_x = beacon.robot_x; + local_y = beacon.robot_y; + local_robot_angle = beacon.robot_angle; + IRQ_UNLOCK(flags); + + if (local_robot_angle < 0) + local_robot_angle += 360; + + x_opponent = cos((local_robot_angle + angle)* 2 * M_PI / 360)* dist; + y_opponent = sin((local_robot_angle + angle)* 2 * M_PI / 360)* dist; + + //BEACON_DEBUG("x_op= %ld\t",x_opponent); + //BEACON_DEBUG("y_op= %ld\r\n",y_opponent); + //BEACON_NOTICE("robot_x= %ld\t",local_x); + //BEACON_NOTICE("robot_y= %ld\t",local_y); + //BEACON_NOTICE("robot_angle= %ld\r\n",local_robot_angle); + + *x = local_x + x_opponent; + *y = local_y + y_opponent; + +} diff --git a/projects/microb2010/sensorboard/beacon.h b/projects/microb2010/sensorboard/beacon.h new file mode 100755 index 0000000..21d7dd5 --- /dev/null +++ b/projects/microb2010/sensorboard/beacon.h @@ -0,0 +1,58 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: beacon.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +struct beacon { + int32_t beacon_speed; + + int32_t opponent_angle; + int32_t opponent_dist; + int32_t prev_opponent_angle; + int32_t prev_opponent_dist; + int32_t robot_x; + int32_t robot_y; + int32_t robot_angle; + int32_t opponent_x; + int32_t opponent_y; +}; + +/* real encoder value: 3531.75 so, multiple by 4 to have round + * value */ +#define BEACON_STEP_TOUR (14127L) +#define BEACON_OFFSET_CALIBRE 40 + +extern struct beacon beacon; + +void beacon_dump(void); +void beacon_init(void); +void beacon_calibre_pos(void); +void beacon_start(void); +void beacon_stop(void); +void beacon_calc(void *dummy); +void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y); + +void beacon_reset_pos(void); +void beacon_set_consign(int32_t val); + +int32_t encoders_spi_get_beacon_speed(void *dummy); +int32_t encoders_spi_update_beacon_speed(void *number); +int32_t encoders_spi_get_value_beacon(void *number); + + diff --git a/projects/microb2010/sensorboard/cmdline.c b/projects/microb2010/sensorboard/cmdline.c new file mode 100644 index 0000000..ceb3efe --- /dev/null +++ b/projects/microb2010/sensorboard/cmdline.c @@ -0,0 +1,149 @@ +/* + * 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 + * (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: cmdline.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cmdline.h" + + +/******** See in commands.c for the list of commands. */ +extern parse_pgm_ctx_t main_ctx[]; + +static void write_char(char c) +{ + uart_send(CMDLINE_UART, c); +} + +static void +valid_buffer(const char *buf, uint8_t size) +{ + int8_t ret; + ret = parse(main_ctx, buf); + if (ret == PARSE_AMBIGUOUS) + printf_P(PSTR("Ambiguous command\r\n")); + else if (ret == PARSE_NOMATCH) + printf_P(PSTR("Command not found\r\n")); + else if (ret == PARSE_BAD_ARGS) + printf_P(PSTR("Bad arguments\r\n")); +} + +static int8_t +complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize, + int16_t *state) +{ + return complete(main_ctx, buf, state, dstbuf, dstsize); +} + + +/* sending "pop" on cmdline uart resets the robot */ +void emergency(char c) +{ + static uint8_t i = 0; + + if ((i == 0 && c == 'p') || + (i == 1 && c == 'o') || + (i == 2 && c == 'p')) + i++; + else if ( !(i == 1 && c == 'p') ) + i = 0; + if (i == 3) + reset(); +} + +/* log function, add a command to configure + * it dynamically */ +void mylog(struct error * e, ...) +{ + va_list ap; + u16 stream_flags = stdout->flags; + uint8_t i; + time_h tv; + + if (e->severity > ERROR_SEVERITY_ERROR) { + if (gen.log_level < e->severity) + return; + + for (i=0; ierr_num) + break; + if (i == NB_LOGS+1) + return; + } + + va_start(ap, e); + tv = time_get_time(); + printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL)); + vfprintf_P(stdout, e->text, ap); + printf_P(PSTR("\r\n")); + va_end(ap); + stdout->flags = stream_flags; +} + +int cmdline_interact(void) +{ + const char *history, *buffer; + int8_t ret, same = 0; + int16_t c; + + rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer); + snprintf(gen.prompt, sizeof(gen.prompt), "sensorboard > "); + rdline_newline(&gen.rdl, gen.prompt); + + while (1) { + c = uart_recv_nowait(CMDLINE_UART); + if (c == -1) + continue; + ret = rdline_char_in(&gen.rdl, c); + if (ret != 2 && ret != 0) { + buffer = rdline_get_buffer(&gen.rdl); + history = rdline_get_history_item(&gen.rdl, 0); + if (history) { + same = !memcmp(buffer, history, strlen(history)) && + buffer[strlen(history)] == '\n'; + } + else + same = 0; + if (strlen(buffer) > 1 && !same) + rdline_add_history(&gen.rdl, buffer); + rdline_newline(&gen.rdl, gen.prompt); + } + } + + return 0; +} diff --git a/projects/microb2010/sensorboard/cmdline.h b/projects/microb2010/sensorboard/cmdline.h new file mode 100644 index 0000000..57dfa49 --- /dev/null +++ b/projects/microb2010/sensorboard/cmdline.h @@ -0,0 +1,40 @@ +/* + * 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 + * (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: cmdline.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +#define CMDLINE_UART 1 + +/* uart rx callback for reset() */ +void emergency(char c); + +/* log function */ +void mylog(struct error * e, ...); + +/* launch cmdline */ +int cmdline_interact(void); + +static inline uint8_t cmdline_keypressed(void) { + return (uart_recv_nowait(CMDLINE_UART) != -1); +} + +static inline uint8_t cmdline_getchar(void) { + return uart_recv_nowait(CMDLINE_UART); +} diff --git a/projects/microb2010/sensorboard/commands.c b/projects/microb2010/sensorboard/commands.c new file mode 100644 index 0000000..539c96d --- /dev/null +++ b/projects/microb2010/sensorboard/commands.c @@ -0,0 +1,128 @@ +/* + * 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 + * (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: commands.c,v 1.2 2009-05-27 20:04:07 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include + +/* commands_gen.c */ +extern parse_pgm_inst_t cmd_reset; +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_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; +extern parse_pgm_inst_t cmd_stack_space; +extern parse_pgm_inst_t cmd_scheduler; + +/* commands_ax12.c */ +extern parse_pgm_inst_t cmd_baudrate; +extern parse_pgm_inst_t cmd_uint16_read; +extern parse_pgm_inst_t cmd_uint16_write; +extern parse_pgm_inst_t cmd_uint8_read; +extern parse_pgm_inst_t cmd_uint8_write; + +/* commands_cs.c */ +extern parse_pgm_inst_t cmd_gain; +extern parse_pgm_inst_t cmd_gain_show; +extern parse_pgm_inst_t cmd_speed; +extern parse_pgm_inst_t cmd_speed_show; +extern parse_pgm_inst_t cmd_derivate_filter; +extern parse_pgm_inst_t cmd_derivate_filter_show; +extern parse_pgm_inst_t cmd_consign; +extern parse_pgm_inst_t cmd_maximum; +extern parse_pgm_inst_t cmd_maximum_show; +extern parse_pgm_inst_t cmd_quadramp; +extern parse_pgm_inst_t cmd_quadramp_show; +extern parse_pgm_inst_t cmd_cs_status; +extern parse_pgm_inst_t cmd_blocking_i; +extern parse_pgm_inst_t cmd_blocking_i_show; + +/* commands_sensorboard.c */ +extern parse_pgm_inst_t cmd_event; +extern parse_pgm_inst_t cmd_test; + +/* commands_scan.c */ +extern parse_pgm_inst_t cmd_sample; +extern parse_pgm_inst_t cmd_sadc; +extern parse_pgm_inst_t cmd_scan_params; +extern parse_pgm_inst_t cmd_scan_calibre; +extern parse_pgm_inst_t cmd_scan_do; +extern parse_pgm_inst_t cmd_scan_img; + + +/* in progmem */ +parse_pgm_ctx_t main_ctx[] = { + + /* commands_gen.c */ + (parse_pgm_inst_t *)&cmd_reset, + (parse_pgm_inst_t *)&cmd_bootloader, + (parse_pgm_inst_t *)&cmd_encoders, + (parse_pgm_inst_t *)&cmd_pwm, + (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, + (parse_pgm_inst_t *)&cmd_stack_space, + (parse_pgm_inst_t *)&cmd_scheduler, + + /* commands_ax12.c */ + (parse_pgm_inst_t *)&cmd_baudrate, + (parse_pgm_inst_t *)&cmd_uint16_read, + (parse_pgm_inst_t *)&cmd_uint16_write, + (parse_pgm_inst_t *)&cmd_uint8_read, + (parse_pgm_inst_t *)&cmd_uint8_write, + + /* commands_cs.c */ + (parse_pgm_inst_t *)&cmd_gain, + (parse_pgm_inst_t *)&cmd_gain_show, + (parse_pgm_inst_t *)&cmd_speed, + (parse_pgm_inst_t *)&cmd_speed_show, + (parse_pgm_inst_t *)&cmd_consign, + (parse_pgm_inst_t *)&cmd_derivate_filter, + (parse_pgm_inst_t *)&cmd_derivate_filter_show, + (parse_pgm_inst_t *)&cmd_maximum, + (parse_pgm_inst_t *)&cmd_maximum_show, + (parse_pgm_inst_t *)&cmd_quadramp, + (parse_pgm_inst_t *)&cmd_quadramp_show, + (parse_pgm_inst_t *)&cmd_cs_status, + (parse_pgm_inst_t *)&cmd_blocking_i, + (parse_pgm_inst_t *)&cmd_blocking_i_show, + + /* commands_sensorboard.c */ + (parse_pgm_inst_t *)&cmd_event, + (parse_pgm_inst_t *)&cmd_test, + + /* commands_scan.c */ + (parse_pgm_inst_t *)&cmd_sample, + (parse_pgm_inst_t *)&cmd_sadc, + (parse_pgm_inst_t *)&cmd_scan_params, + (parse_pgm_inst_t *)&cmd_scan_calibre, + (parse_pgm_inst_t *)&cmd_scan_do, + (parse_pgm_inst_t *)&cmd_scan_img, + + NULL, +}; diff --git a/projects/microb2010/sensorboard/commands_ax12.c b/projects/microb2010/sensorboard/commands_ax12.c new file mode 100644 index 0000000..771fb30 --- /dev/null +++ b/projects/microb2010/sensorboard/commands_ax12.c @@ -0,0 +1,368 @@ +/* + * 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 + * (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: commands_ax12.c,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" + +uint8_t addr_from_string(const char *s) +{ + /* 16 bits */ + if (!strcmp_P(s, PSTR("cw_angle_limit"))) + return AA_CW_ANGLE_LIMIT_L; + if (!strcmp_P(s, PSTR("ccw_angle_limit"))) + return AA_CCW_ANGLE_LIMIT_L; + if (!strcmp_P(s, PSTR("max_torque"))) + return AA_MAX_TORQUE_L; + if (!strcmp_P(s, PSTR("down_calibration"))) + return AA_DOWN_CALIBRATION_L; + if (!strcmp_P(s, PSTR("up_calibration"))) + return AA_UP_CALIBRATION_L; + if (!strcmp_P(s, PSTR("torque_limit"))) + return AA_TORQUE_LIMIT_L; + if (!strcmp_P(s, PSTR("position"))) + return AA_PRESENT_POSITION_L; + if (!strcmp_P(s, PSTR("speed"))) + return AA_PRESENT_SPEED_L; + if (!strcmp_P(s, PSTR("load"))) + return AA_PRESENT_LOAD_L; + if (!strcmp_P(s, PSTR("moving_speed"))) + return AA_MOVING_SPEED_L; + if (!strcmp_P(s, PSTR("model"))) + return AA_MODEL_NUMBER_L; + if (!strcmp_P(s, PSTR("goal_pos"))) + return AA_GOAL_POSITION_L; + if (!strcmp_P(s, PSTR("punch"))) + return AA_PUNCH_L; + + /* 8 bits */ + if (!strcmp_P(s, PSTR("firmware"))) + return AA_FIRMWARE; + if (!strcmp_P(s, PSTR("id"))) + return AA_ID; + if (!strcmp_P(s, PSTR("baudrate"))) + return AA_BAUD_RATE; + if (!strcmp_P(s, PSTR("delay"))) + return AA_DELAY_TIME; + if (!strcmp_P(s, PSTR("high_lim_temp"))) + return AA_HIGHEST_LIMIT_TEMP; + if (!strcmp_P(s, PSTR("low_lim_volt"))) + return AA_LOWEST_LIMIT_VOLTAGE; + if (!strcmp_P(s, PSTR("high_lim_volt"))) + return AA_HIGHEST_LIMIT_VOLTAGE; + if (!strcmp_P(s, PSTR("status_return"))) + return AA_STATUS_RETURN_LEVEL; + if (!strcmp_P(s, PSTR("alarm_led"))) + return AA_ALARM_LED; + if (!strcmp_P(s, PSTR("alarm_shutdown"))) + return AA_ALARM_SHUTDOWN; + if (!strcmp_P(s, PSTR("torque_enable"))) + return AA_TORQUE_ENABLE; + if (!strcmp_P(s, PSTR("led"))) + return AA_LED; + if (!strcmp_P(s, PSTR("cw_comp_margin"))) + return AA_CW_COMPLIANCE_MARGIN; + if (!strcmp_P(s, PSTR("ccw_comp_margin"))) + return AA_CCW_COMPLIANCE_MARGIN; + if (!strcmp_P(s, PSTR("cw_comp_slope"))) + return AA_CW_COMPLIANCE_SLOPE; + if (!strcmp_P(s, PSTR("ccw_comp_slope"))) + return AA_CCW_COMPLIANCE_SLOPE; + if (!strcmp_P(s, PSTR("voltage"))) + return AA_PRESENT_VOLTAGE; + if (!strcmp_P(s, PSTR("temp"))) + return AA_PRESENT_TEMP; + if (!strcmp_P(s, PSTR("reginst"))) + return AA_PRESENT_REGINST; + if (!strcmp_P(s, PSTR("moving"))) + return AA_MOVING; + if (!strcmp_P(s, PSTR("lock"))) + return AA_LOCK; + + return 0; +} + +/**********************************************************/ +/* Ax12_Stress */ + +/* this structure is filled when cmd_ax12_stress is parsed successfully */ +struct cmd_ax12_stress_result { + fixed_string_t arg0; + uint8_t id; +}; + +/* function called when cmd_ax12_stress is parsed successfully */ +static void cmd_ax12_stress_parsed(void *parsed_result, void *data) +{ + struct cmd_ax12_stress_result *res = parsed_result; + int i, nb_errs = 0; + uint8_t val; + microseconds t = time_get_us2(); + + for (i=0; i<1000; i++) { + if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0) + nb_errs ++; + } + + printf_P(PSTR("%d errors / 1000\r\n"), nb_errs); + t = (time_get_us2() - t) / 1000; + printf_P(PSTR("Test done in %d ms\r\n"), (int)t); +} + +prog_char str_ax12_stress_arg0[] = "ax12_stress"; +parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0); +parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8); + +prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands"; +parse_pgm_inst_t cmd_ax12_stress = { + .f = cmd_ax12_stress_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ax12_stress, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ax12_stress_arg0, + (prog_void *)&cmd_ax12_stress_id, + NULL, + }, +}; + +/**********************************************************/ + +/* this structure is filled when cmd_baudrate is parsed successfully */ +struct cmd_baudrate_result { + fixed_string_t arg0; + uint32_t arg1; +}; + +/* function called when cmd_baudrate is parsed successfully */ +static void cmd_baudrate_parsed(void * parsed_result, void * data) +{ + struct cmd_baudrate_result *res = parsed_result; + struct uart_config c; + + printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L); + uart_getconf(1, &c); + c.baudrate = res->arg1; + uart_setconf(1, &c); + printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L); +} + +prog_char str_baudrate_arg0[] = "baudrate"; +parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0); +parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32); + +prog_char help_baudrate[] = "Change ax12 baudrate"; +parse_pgm_inst_t cmd_baudrate = { + .f = cmd_baudrate_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_baudrate, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_baudrate_arg0, + (prog_void *)&cmd_baudrate_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Uint16 */ + + +/* this structure is filled when cmd_uint16_read is parsed successfully */ +struct cmd_uint16_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t num; + uint16_t val; +}; + +/* function called when cmd_uint16_read is parsed successfully */ +static void cmd_uint16_read_parsed(void * parsed_result, void * data) +{ + struct cmd_uint16_result *res = parsed_result; + uint8_t ret; + uint16_t val; + uint8_t addr = addr_from_string(res->arg1); + ret = AX12_read_int(&gen.ax12, res->num, addr, &val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); + printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val); +} + +prog_char str_uint16_arg0[] = "read"; +parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0); +prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#" + "max_torque#down_calibration#up_calibration#torque_limit#" + "position#speed#load#punch"; +parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1); +parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8); + +prog_char help_uint16_read[] = "Read uint16 value (type, num)"; +parse_pgm_inst_t cmd_uint16_read = { + .f = cmd_uint16_read_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint16_read, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint16_arg0, + (prog_void *)&cmd_uint16_arg1, + (prog_void *)&cmd_uint16_num, + NULL, + }, +}; + +/* function called when cmd_uint16_write is parsed successfully */ +static void cmd_uint16_write_parsed(void * parsed_result, void * data) +{ + struct cmd_uint16_result *res = parsed_result; + uint8_t ret; + uint8_t addr = addr_from_string(res->arg1); + printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1, + res->val, res->val); + ret = AX12_write_int(&gen.ax12, res->num, addr, res->val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); +} + +prog_char str_uint16_arg0_w[] = "write"; +parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w); +prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#" + "max_torque#torque_limit#punch"; +parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w); +parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16); + +prog_char help_uint16_write[] = "Write uint16 value (write, num, val)"; +parse_pgm_inst_t cmd_uint16_write = { + .f = cmd_uint16_write_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint16_write, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint16_arg0_w, + (prog_void *)&cmd_uint16_arg1_w, + (prog_void *)&cmd_uint16_num, + (prog_void *)&cmd_uint16_val, + NULL, + }, +}; + +/**********************************************************/ +/* Uint8 */ + + +/* this structure is filled when cmd_uint8_read is parsed successfully */ +struct cmd_uint8_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t num; + uint8_t val; +}; + +/* function called when cmd_uint8_read is parsed successfully */ +static void cmd_uint8_read_parsed(void * parsed_result, void * data) +{ + struct cmd_uint8_result *res = parsed_result; + uint8_t ret; + uint8_t val; + uint8_t addr = addr_from_string(res->arg1); + + ret = AX12_read_byte(&gen.ax12, res->num, addr, &val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); + printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val); +} + +prog_char str_uint8_arg0[] = "read"; +parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0); +prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#" + "low_lim_volt#high_lim_volt#status_return#alarm_led#" + "alarm_shutdown#torque_enable#led#cw_comp_margin#" + "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#" + "voltage#temp#reginst#moving#lock"; +parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1); +parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8); + +prog_char help_uint8_read[] = "Read uint8 value (type, num)"; +parse_pgm_inst_t cmd_uint8_read = { + .f = cmd_uint8_read_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint8_read, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint8_arg0, + (prog_void *)&cmd_uint8_arg1, + (prog_void *)&cmd_uint8_num, + NULL, + }, +}; + +/* function called when cmd_uint8_write is parsed successfully */ +static void cmd_uint8_write_parsed(void * parsed_result, void * data) +{ + struct cmd_uint8_result *res = parsed_result; + uint8_t addr = addr_from_string(res->arg1); + uint8_t ret; + printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1, + res->val, res->val); + ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); +} + +prog_char str_uint8_arg0_w[] = "write"; +parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w); +prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#" + "low_lim_volt#high_lim_volt#status_return#alarm_led#" + "alarm_shutdown#torque_enable#led#cw_comp_margin#" + "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#" + "reginst#lock"; +parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w); +parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8); + +prog_char help_uint8_write[] = "Write uint8 value (write, num, val)"; +parse_pgm_inst_t cmd_uint8_write = { + .f = cmd_uint8_write_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint8_write, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint8_arg0_w, + (prog_void *)&cmd_uint8_arg1_w, + (prog_void *)&cmd_uint8_num, + (prog_void *)&cmd_uint8_val, + NULL, + }, +}; diff --git a/projects/microb2010/sensorboard/commands_cs.c b/projects/microb2010/sensorboard/commands_cs.c new file mode 100644 index 0000000..690922a --- /dev/null +++ b/projects/microb2010/sensorboard/commands_cs.c @@ -0,0 +1,670 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: commands_cs.c,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cs.h" +#include "cmdline.h" + +struct csb_list { + const prog_char *name; + struct cs_block *csb; +}; + +prog_char csb_beacon_str[] = "beacon"; +prog_char csb_scanner_str[] = "scanner"; +struct csb_list csb_list[] = { + { .name = csb_beacon_str, .csb = &sensorboard.beacon }, + { .name = csb_scanner_str, .csb = &sensorboard.scanner }, +}; + +struct cmd_cs_result { + fixed_string_t cmdname; + fixed_string_t csname; +}; + +/* token to be used for all cs-related commands */ +prog_char str_csb_name[] = "beacon#scanner"; +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) +{ + int i; + + for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) { + if (!strcmp_P(name, csb_list[i].name)) + return csb_list[i].csb; + } + return NULL; +} + +/**********************************************************/ +/* Gains for control system */ + +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_result { + struct cmd_cs_result cs; + int16_t p; + int16_t i; + int16_t d; +}; + +/* function called when cmd_gain is parsed successfully */ +static void cmd_gain_parsed(void * parsed_result, void *show) +{ + struct cmd_gain_result *res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_gains(&csb->pid, res->p, res->i, res->d); + + printf_P(PSTR("%s %s %d %d %d\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_gain_P(&csb->pid), + pid_get_gain_I(&csb->pid), + pid_get_gain_D(&csb->pid)); +} + +prog_char str_gain_arg0[] = "gain"; +parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0); +parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16); +parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16); +parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16); + +prog_char help_gain[] = "Set gain values for PID"; +parse_pgm_inst_t cmd_gain = { + .f = cmd_gain_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_gain, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_p, + (prog_void *)&cmd_gain_i, + (prog_void *)&cmd_gain_d, + NULL, + }, +}; + +/* show */ +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_gain_show_arg[] = "show"; +parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg); + +prog_char help_gain_show[] = "Show gain values for PID"; +parse_pgm_inst_t cmd_gain_show = { + .f = cmd_gain_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_gain_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Speeds for control system */ + +/* this structure is filled when cmd_speed is parsed successfully */ +struct cmd_speed_result { + struct cmd_cs_result cs; + uint16_t s; +}; + +/* function called when cmd_speed is parsed successfully */ +static void cmd_speed_parsed(void *parsed_result, void *show) +{ + struct cmd_speed_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + +#if notyet + if (!show) + ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */ + + printf_P(PSTR("%s %lu\r\n"), + res->cs.csname, + ext.r_b.var_pos); +#else + printf_P(PSTR("TODO\r\n")); +#endif +} + +prog_char str_speed_arg0[] = "speed"; +parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0); +parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16); + +prog_char help_speed[] = "Set speed values for ramp filter"; +parse_pgm_inst_t cmd_speed = { + .f = cmd_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_s, + NULL, + }, +}; + +/* show */ +struct cmd_speed_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg); + +prog_char help_speed_show[] = "Show speed values for ramp filter"; +parse_pgm_inst_t cmd_speed_show = { + .f = cmd_speed_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Derivate_Filters for control system */ + +/* this structure is filled when cmd_derivate_filter is parsed successfully */ +struct cmd_derivate_filter_result { + struct cmd_cs_result cs; + uint8_t size; +}; + +/* function called when cmd_derivate_filter is parsed successfully */ +static void cmd_derivate_filter_parsed(void *parsed_result, void *show) +{ + struct cmd_derivate_filter_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_derivate_filter(&csb->pid, res->size); + + printf_P(PSTR("%s %s %u\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_derivate_filter(&csb->pid)); +} + +prog_char str_derivate_filter_arg0[] = "derivate_filter"; +parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0); +parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32); + +prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)"; +parse_pgm_inst_t cmd_derivate_filter = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_derivate_filter, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_size, + NULL, + }, +}; + +/* show */ + +struct cmd_derivate_filter_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_derivate_filter_show_arg[] = "show"; +parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg); + +prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID"; +parse_pgm_inst_t cmd_derivate_filter_show = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_derivate_filter_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_show_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Consign for control system */ + +/* this structure is filled when cmd_consign is parsed successfully */ +struct cmd_consign_result { + struct cmd_cs_result cs; + int32_t p; +}; + +/* function called when cmd_consign is parsed successfully */ +static void cmd_consign_parsed(void * parsed_result, void *data) +{ + struct cmd_consign_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + cs_set_consign(&csb->cs, res->p); +} + +prog_char str_consign_arg0[] = "consign"; +parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0); +parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32); + +prog_char help_consign[] = "Set consign value"; +parse_pgm_inst_t cmd_consign = { + .f = cmd_consign_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_consign, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_consign_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_consign_p, + NULL, + }, +}; + + +/**********************************************************/ +/* Maximums for control system */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_result { + struct cmd_cs_result cs; + uint32_t in; + uint32_t i; + uint32_t out; +}; + +/* function called when cmd_maximum is parsed successfully */ +static void cmd_maximum_parsed(void *parsed_result, void *show) +{ + struct cmd_maximum_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_maximums(&csb->pid, res->in, res->i, res->out); + + printf_P(PSTR("maximum %s %lu %lu %lu\r\n"), + res->cs.csname, + pid_get_max_in(&csb->pid), + pid_get_max_I(&csb->pid), + pid_get_max_out(&csb->pid)); +} + +prog_char str_maximum_arg0[] = "maximum"; +parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0); +parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32); +parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32); +parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32); + +prog_char help_maximum[] = "Set maximum values for PID (in, I, out)"; +parse_pgm_inst_t cmd_maximum = { + .f = cmd_maximum_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_maximum, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_in, + (prog_void *)&cmd_maximum_i, + (prog_void *)&cmd_maximum_out, + NULL, + }, +}; + +/* show */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; +prog_char str_maximum_show_arg[] = "show"; +parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg); + +prog_char help_maximum_show[] = "Show maximum values for PID"; +parse_pgm_inst_t cmd_maximum_show = { + .f = cmd_maximum_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_maximum_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Quadramp for control system */ + +/* this structure is filled when cmd_quadramp is parsed successfully */ +struct cmd_quadramp_result { + struct cmd_cs_result cs; + uint32_t ap; + uint32_t an; + uint32_t sp; + uint32_t sn; +}; + +/* function called when cmd_quadramp is parsed successfully */ +static void cmd_quadramp_parsed(void *parsed_result, void *show) +{ + struct cmd_quadramp_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) { + quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn); + quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); + } + + printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"), + res->cs.csname, + csb->qr.var_2nd_ord_pos, + csb->qr.var_2nd_ord_neg, + csb->qr.var_1st_ord_pos, + csb->qr.var_1st_ord_neg); +} + +prog_char str_quadramp_arg0[] = "quadramp"; +parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0); +parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32); +parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32); +parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32); +parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32); + +prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)"; +parse_pgm_inst_t cmd_quadramp = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_quadramp, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_ap, + (prog_void *)&cmd_quadramp_an, + (prog_void *)&cmd_quadramp_sp, + (prog_void *)&cmd_quadramp_sn, + + NULL, + }, +}; + +/* show */ + +struct cmd_quadramp_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_quadramp_show_arg[] = "show"; +parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg); + +prog_char help_quadramp_show[] = "Get quadramp values for control system"; +parse_pgm_inst_t cmd_quadramp_show = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_quadramp_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* cs_status show for control system */ + +/* this structure is filled when cmd_cs_status is parsed successfully */ +struct cmd_cs_status_result { + struct cmd_cs_result cs; + fixed_string_t arg; +}; + +/* function called when cmd_cs_status is parsed successfully */ +static void cmd_cs_status_parsed(void *parsed_result, void *data) +{ + 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); + return; + } + else if (strcmp_P(res->arg, PSTR("off")) == 0) { + csb->on = 0; + printf_P(PSTR("%s is off\r\n"), res->cs.csname); + return; + } + 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 { + 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[] = "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"; +parse_pgm_inst_t cmd_cs_status = { + .f = cmd_cs_status_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_cs_status, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_cs_status_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_cs_status_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Blocking_I for control system */ + +/* this structure is filled when cmd_blocking_i is parsed successfully */ +struct cmd_blocking_i_result { + struct cmd_cs_result cs; + int32_t k1; + int32_t k2; + uint32_t i; + uint16_t cpt; +}; + +/* function called when cmd_blocking_i is parsed successfully */ +static void cmd_blocking_i_parsed(void *parsed_result, void *show) +{ + struct cmd_blocking_i_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + bd_set_current_thresholds(&csb->bd, res->k1, res->k2, + res->i, res->cpt); + + printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"), + res->cs.cmdname, + res->cs.csname, + csb->bd.k1, + csb->bd.k2, + csb->bd.i_thres, + csb->bd.cpt_thres); +} + +prog_char str_blocking_i_arg0[] = "blocking"; +parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0); +parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32); +parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32); +parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32); +parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16); + +prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)"; +parse_pgm_inst_t cmd_blocking_i = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_blocking_i, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_k1, + (prog_void *)&cmd_blocking_i_k2, + (prog_void *)&cmd_blocking_i_i, + (prog_void *)&cmd_blocking_i_cpt, + NULL, + }, +}; + +/* show */ + +struct cmd_blocking_i_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_blocking_i_show_arg[] = "show"; +parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg); + +prog_char help_blocking_i_show[] = "Show blocking detection values"; +parse_pgm_inst_t cmd_blocking_i_show = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_blocking_i_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_show_arg, + NULL, + }, +}; + + diff --git a/projects/microb2010/sensorboard/commands_gen.c b/projects/microb2010/sensorboard/commands_gen.c new file mode 100644 index 0000000..9f7cdd6 --- /dev/null +++ b/projects/microb2010/sensorboard/commands_gen.c @@ -0,0 +1,573 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: commands_gen.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "main.h" +#include "cmdline.h" +#include "sensor.h" + +/**********************************************************/ +/* Reset */ + +/* this structure is filled when cmd_reset is parsed successfully */ +struct cmd_reset_result { + fixed_string_t arg0; +}; + +/* function called when cmd_reset is parsed successfully */ +static void cmd_reset_parsed(void * parsed_result, void * data) +{ + reset(); +} + +prog_char str_reset_arg0[] = "reset"; +parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0); + +prog_char help_reset[] = "Reset the board"; +parse_pgm_inst_t cmd_reset = { + .f = cmd_reset_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_reset, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_reset_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Bootloader */ + +/* this structure is filled when cmd_bootloader is parsed successfully */ +struct cmd_bootloader_result { + fixed_string_t arg0; +}; + +/* function called when cmd_bootloader is parsed successfully */ +static void cmd_bootloader_parsed(void *parsed_result, void *data) +{ + bootloader(); +} + +prog_char str_bootloader_arg0[] = "bootloader"; +parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0); + +prog_char help_bootloader[] = "Launch the bootloader"; +parse_pgm_inst_t cmd_bootloader = { + .f = cmd_bootloader_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_bootloader, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_bootloader_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Encoders tests */ + +/* this structure is filled when cmd_encoders is parsed successfully */ +struct cmd_encoders_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_encoders is parsed successfully */ +static void cmd_encoders_parsed(void *parsed_result, void *data) +{ + struct cmd_encoders_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + encoders_spi_set_value((void *)0, 0); + encoders_spi_set_value((void *)1, 0); + encoders_spi_set_value((void *)2, 0); + encoders_spi_set_value((void *)3, 0); + return; + } + + /* show */ + while(!cmdline_keypressed()) { + printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"), + encoders_spi_get_value((void *)0), + encoders_spi_get_value((void *)1), + encoders_spi_get_value((void *)2), + encoders_spi_get_value((void *)3)); + wait_ms(100); + } +} + +prog_char str_encoders_arg0[] = "encoders"; +parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0); +prog_char str_encoders_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1); + +prog_char help_encoders[] = "Show encoders values"; +parse_pgm_inst_t cmd_encoders = { + .f = cmd_encoders_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_encoders, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_encoders_arg0, + (prog_void *)&cmd_encoders_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Scheduler show */ + +/* this structure is filled when cmd_scheduler is parsed successfully */ +struct cmd_scheduler_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_scheduler is parsed successfully */ +static void cmd_scheduler_parsed(void *parsed_result, void *data) +{ + scheduler_dump_events(); +} + +prog_char str_scheduler_arg0[] = "scheduler"; +parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0); +prog_char str_scheduler_arg1[] = "show"; +parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1); + +prog_char help_scheduler[] = "Show scheduler events"; +parse_pgm_inst_t cmd_scheduler = { + .f = cmd_scheduler_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scheduler, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scheduler_arg0, + (prog_void *)&cmd_scheduler_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Pwms tests */ + +/* this structure is filled when cmd_pwm is parsed successfully */ +struct cmd_pwm_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; +}; + +/* function called when cmd_pwm is parsed successfully */ +static void cmd_pwm_parsed(void * parsed_result, void * data) +{ + void * pwm_ptr = NULL; + struct cmd_pwm_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("1(4A)"))) + pwm_ptr = &gen.pwm1_4A; + else if (!strcmp_P(res->arg1, PSTR("2(4B)"))) + pwm_ptr = &gen.pwm2_4B; + else if (!strcmp_P(res->arg1, PSTR("3(1A)"))) + pwm_ptr = &gen.pwm3_1A; + else if (!strcmp_P(res->arg1, PSTR("4(1B)"))) + pwm_ptr = &gen.pwm4_1B; + + else if (!strcmp_P(res->arg1, PSTR("s1(3C)"))) + pwm_ptr = &gen.servo1; + else if (!strcmp_P(res->arg1, PSTR("s2(5A)"))) + pwm_ptr = &gen.servo2; + else if (!strcmp_P(res->arg1, PSTR("s3(5B)"))) + pwm_ptr = &gen.servo3; + else if (!strcmp_P(res->arg1, PSTR("s3(5C)"))) + pwm_ptr = &gen.servo4; + + if (pwm_ptr) + pwm_ng_set(pwm_ptr, res->arg2); + + printf_P(PSTR("done\r\n")); +} + +prog_char str_pwm_arg0[] = "pwm"; +parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0); +prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)"; +parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1); +parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16); + +prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]"; +parse_pgm_inst_t cmd_pwm = { + .f = cmd_pwm_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pwm, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pwm_arg0, + (prog_void *)&cmd_pwm_arg1, + (prog_void *)&cmd_pwm_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Adcs tests */ + +/* this structure is filled when cmd_adc is parsed successfully */ +struct cmd_adc_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_adc is parsed successfully */ +static void cmd_adc_parsed(void *parsed_result, void *data) +{ + struct cmd_adc_result *res = parsed_result; + uint8_t i, loop = 0; + + if (!strcmp_P(res->arg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("ADC values: ")); + for (i=0; iarg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("SENSOR values: ")); + for (i=0; iarg1, PSTR("level"))) { + gen.log_level = res->arg2; + } + + /* else it is a show */ + cmd_log_do_show(); +} + +prog_char str_log_arg0[] = "log"; +parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0); +prog_char str_log_arg1[] = "level"; +parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1); +parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32); + +prog_char help_log[] = "Set log options: level (0 -> 5)"; +parse_pgm_inst_t cmd_log = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1, + (prog_void *)&cmd_log_arg2, + NULL, + }, +}; + +prog_char str_log_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show); + +prog_char help_log_show[] = "Show configured logs"; +parse_pgm_inst_t cmd_log_show = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1_show, + NULL, + }, +}; + +/* this structure is filled when cmd_log is parsed successfully */ +struct cmd_log_type_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_log is parsed successfully */ +static void cmd_log_type_parsed(void * parsed_result, void * data) +{ + struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result; + uint8_t lognum; + uint8_t i; + + lognum = log_name2num(res->arg2); + if (lognum == 0) { + printf_P(PSTR("Cannot find log num\r\n")); + return; + } + + if (!strcmp_P(res->arg3, PSTR("on"))) { + for (i=0; iarg3, PSTR("off"))) { + for (i=0; i + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include "main.h" +#include "cmdline.h" +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" +#include "actuator.h" + +#include +#include +#include + +#include "img_processing.h" +#include "scanner.h" + + + +/**********************************************************/ +/* sample ADC */ +/* +extern uint16_t sample_i; +extern float scan_offset_a; +extern float scan_offset_b; +*/ +extern struct scan_params scan_params; + +//extern uint16_t sample_tab[MAX_SAMPLE]; +/* this structure is filled when cmd_sample is parsed successfully */ +struct cmd_sample_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t offset_b; + float offset_a; + uint16_t dump_speed; + uint8_t filter; +}; + +extern int32_t pos_start_scan; +/* function called when cmd_sample is parsed successfully */ + +#define MAX_OBJECTS 10 +Object_bb sac_obj[MAX_OBJECTS]; +static void cmd_sample_parsed(void * parsed_result, void * data) +{ + struct cmd_sample_result * res = parsed_result; + uint16_t i; + + printf_P(PSTR("cmd sample called\r\n")); + printf_P(PSTR("arg %s %d\r\n"), res->arg1, res->offset_b); + + quadramp_set_1st_order_vars(&sensorboard.scanner.qr, res->dump_speed, res->dump_speed); /* set speed */ + + + scan_params.offset_b = (((float)res->offset_b)*M_PI/180.); + scan_params.offset_a = (((float)res->offset_a)*M_PI/180.); + scan_params.filter = res->filter; + + if (!strcmp_P(res->arg1, PSTR("start"))) { + scan_params.sample_i = MAX_SAMPLE; + scan_params.pos_start_scan = encoders_spi_get_value_scanner(SCANNER_ENC); + //printf_P(PSTR("start scan at pos %ld\r\n"), scan_params.pos_start_scan); + + memset(scan_params.sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t)); + + + cs_set_consign(&sensorboard.scanner.cs, scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L); + //printf_P(PSTR("scan dst %ld\r\n"), scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L); + + scan_params.last_col_n = 0; + scan_params.last_row_n = 0; + scan_params.last_sample = 0; + + } + else if (!strcmp_P(res->arg1, PSTR("dump"))) { + printf_P(PSTR("start object detection\r\n")); + for (i=0;ispeed; + scan_params.debug = res->debug; + +} + +prog_char str_scan_params_arg0[] = "scan_params"; +parse_pgm_token_string_t cmd_scan_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_params_result, arg0, str_scan_params_arg0); +parse_pgm_token_num_t cmd_scan_params_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_scan_params_result, speed, INT16); +parse_pgm_token_num_t cmd_scan_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_params_result, debug, UINT8); + +prog_char help_scan_params[] = "Set scanner params (speed, debug)"; +parse_pgm_inst_t cmd_scan_params = { + .f = cmd_scan_params_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_sample, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scan_params_arg0, + (prog_void *)&cmd_scan_params_arg1, + (prog_void *)&cmd_scan_params_arg2, + NULL, + }, +}; + + + +/**********************************************************/ +/* set scanner calibration */ + +/* this structure is filled when cmd_scan_calibre is parsed successfully */ +struct cmd_scan_calibre_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_scan_calibre is parsed successfully */ +static void cmd_scan_calibre_parsed(void * parsed_result, void * data) +{ + struct cmd_scan_calibre_result * res = parsed_result; + + printf_P(PSTR("Starting scanner autocalibration\r\n")); + + + if (!strcmp_P(res->arg1, PSTR("mirror"))) { + scanner_calibre_mirror(); + } + else{ + scanner_calibre_laser(); + } +} + +prog_char str_scan_calibre_arg0[] = "scan_calibre"; + +prog_char str_scan_calibre_what_arg1[] = "mirror#laser"; +parse_pgm_token_string_t cmd_scan_calibre_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_calibre_result, arg0, str_scan_calibre_arg0); +parse_pgm_token_string_t cmd_scan_calibre_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_calibre_result, arg1, str_scan_calibre_what_arg1); + + +prog_char help_scan_calibre[] = "Scanner auto calibration (mirror|laser)"; +parse_pgm_inst_t cmd_scan_calibre = { + .f = cmd_scan_calibre_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scan_calibre, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scan_calibre_arg0, + (prog_void *)&cmd_scan_calibre_arg1, + NULL, + }, +}; + + + +/**********************************************************/ +/* start scan */ + +/* this structure is filled when cmd_scan_do is parsed successfully */ +struct cmd_scan_do_result { + fixed_string_t arg0; +}; + +/* function called when cmd_scan_do is parsed successfully */ +static void cmd_scan_do_parsed(void * parsed_result, void * data) +{ + printf_P(PSTR("Starting scan\r\n")); + scanner_scan_autonomous(); +} + +prog_char str_scan_do_arg0[] = "scan_do"; +parse_pgm_token_string_t cmd_scan_do_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_do_result, arg0, str_scan_do_arg0); + +prog_char help_scan_do[] = "Scan zone"; +parse_pgm_inst_t cmd_scan_do = { + .f = cmd_scan_do_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scan_do, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scan_do_arg0, + NULL, + }, +}; + + + + +/**********************************************************/ +/* set scanner img params */ + +/* this structure is filled when cmd_scan_img is parsed successfully */ +struct cmd_scan_img_result { + fixed_string_t arg0; + uint8_t algo; + uint8_t h; + int16_t x; + int16_t y; + +}; + +/* function called when cmd_scan_img is parsed successfully */ +static void cmd_scan_img_parsed(void * parsed_result, void * data) +{ + struct cmd_scan_img_result * res = parsed_result; + + scan_params.algo = res->algo; + if (res->algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE) { + scan_params.drop_zone.working_zone = res->h; + scan_params.drop_zone.center_x = res->x; + scan_params.drop_zone.center_y = res->y; + } else if (res->algo == I2C_SCANNER_ALGO_CHECK_TEMPLE) { + scan_params.check_temple.level = res->h; + scan_params.check_temple.temple_x = res->x; + scan_params.check_temple.temple_y = res->y; + } else if (res->algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE) { + scan_params.drop_zone.working_zone = res->h; + scan_params.drop_zone.center_x = res->x; + scan_params.drop_zone.center_y = res->y; + } + + + +} + +prog_char str_scan_img_arg0[] = "scan_img"; +parse_pgm_token_string_t cmd_scan_img_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_img_result, arg0, str_scan_img_arg0); +parse_pgm_token_num_t cmd_scan_img_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, algo, UINT8); +parse_pgm_token_num_t cmd_scan_img_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, h, UINT8); +parse_pgm_token_num_t cmd_scan_img_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, x, INT16); +parse_pgm_token_num_t cmd_scan_img_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_img_result, y, INT16); + + +prog_char help_scan_img[] = "Set scanner img processing params (algo, H, x, y)"; +parse_pgm_inst_t cmd_scan_img = { + .f = cmd_scan_img_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scan_img, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scan_img_arg0, + (prog_void *)&cmd_scan_img_arg1, + (prog_void *)&cmd_scan_img_arg2, + (prog_void *)&cmd_scan_img_arg3, + (prog_void *)&cmd_scan_img_arg4, + NULL, + }, +}; + + diff --git a/projects/microb2010/sensorboard/commands_sensorboard.c b/projects/microb2010/sensorboard/commands_sensorboard.c new file mode 100644 index 0000000..9b90bd1 --- /dev/null +++ b/projects/microb2010/sensorboard/commands_sensorboard.c @@ -0,0 +1,197 @@ +/* + * 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 + * (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: commands_sensorboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cmdline.h" +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" +#include "actuator.h" + +struct cmd_event_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + + +/* function called when cmd_event is parsed successfully */ +static void cmd_event_parsed(void *parsed_result, void *data) +{ + u08 bit=0; + + struct cmd_event_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("all"))) { + bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; + if (!strcmp_P(res->arg2, PSTR("on"))) + sensorboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) + sensorboard.flags &= bit; + else { /* show */ + printf_P(PSTR("encoders is %s\r\n"), + (DO_ENCODERS & sensorboard.flags) ? "on":"off"); + printf_P(PSTR("cs is %s\r\n"), + (DO_CS & sensorboard.flags) ? "on":"off"); + printf_P(PSTR("bd is %s\r\n"), + (DO_BD & sensorboard.flags) ? "on":"off"); + printf_P(PSTR("power is %s\r\n"), + (DO_POWER & sensorboard.flags) ? "on":"off"); + } + return; + } + + if (!strcmp_P(res->arg1, PSTR("encoders"))) + bit = DO_ENCODERS; + else if (!strcmp_P(res->arg1, PSTR("cs"))) { + //strat_hardstop(); + bit = DO_CS; + } + else if (!strcmp_P(res->arg1, PSTR("bd"))) + bit = DO_BD; + else if (!strcmp_P(res->arg1, PSTR("power"))) + bit = DO_POWER; + + + if (!strcmp_P(res->arg2, PSTR("on"))) + sensorboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) { + if (!strcmp_P(res->arg1, PSTR("cs"))) { + pwm_ng_set(BEACON_PWM, 0); + pwm_ng_set(SCANNER_PWM, 0); + } + sensorboard.flags &= (~bit); + } + printf_P(PSTR("%s is %s\r\n"), res->arg1, + (bit & sensorboard.flags) ? "on":"off"); +} + +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[] = "all#encoders#cs#bd#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); + +prog_char help_event[] = "Enable/disable events"; +parse_pgm_inst_t cmd_event = { + .f = cmd_event_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_event, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Color */ + +/* this structure is filled when cmd_color is parsed successfully */ +struct cmd_color_result { + fixed_string_t arg0; + fixed_string_t color; +}; + +/* function called when cmd_color is parsed successfully */ +static void cmd_color_parsed(void *parsed_result, void *data) +{ + struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; + if (!strcmp_P(res->color, PSTR("red"))) { + sensorboard.our_color = I2C_COLOR_RED; + } + else if (!strcmp_P(res->color, PSTR("green"))) { + sensorboard.our_color = I2C_COLOR_GREEN; + } + printf_P(PSTR("Done\r\n")); +} + +prog_char str_color_arg0[] = "color"; +parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0); +prog_char str_color_color[] = "green#red"; +parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color); + +prog_char help_color[] = "Set our color"; +parse_pgm_inst_t cmd_color = { + .f = cmd_color_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_color, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, + NULL, + }, +}; + + +/**********************************************************/ +/* Test */ + +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_test is parsed successfully */ +static void cmd_test_parsed(void *parsed_result, void *data) +{ + //struct cmd_test_result *res = parsed_result; + +} + +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); + +prog_char help_test[] = "Test function"; +parse_pgm_inst_t cmd_test = { + .f = cmd_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_test_arg0, + NULL, + }, +}; diff --git a/projects/microb2010/sensorboard/cs.c b/projects/microb2010/sensorboard/cs.c new file mode 100644 index 0000000..010b49e --- /dev/null +++ b/projects/microb2010/sensorboard/cs.c @@ -0,0 +1,145 @@ +/* + * 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 + * (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: cs.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "beacon.h" +#include "scanner.h" +#include "actuator.h" + +/* called every 5 ms */ +static void do_cs(void *dummy) +{ + /* read encoders */ + if (sensorboard.flags & DO_ENCODERS) { + encoders_spi_manage(NULL); + } + /* control system */ + if (sensorboard.flags & DO_CS) { + if (sensorboard.beacon.on) + cs_manage(&sensorboard.beacon.cs); + if (sensorboard.scanner.on) + cs_manage(&sensorboard.scanner.cs); + } + if (sensorboard.flags & DO_BD) { + bd_manage_from_cs(&sensorboard.beacon.bd, &sensorboard.beacon.cs); + bd_manage_from_cs(&sensorboard.scanner.bd, &sensorboard.scanner.cs); + } + if (sensorboard.flags & DO_POWER) + BRAKE_OFF(); + else + BRAKE_ON(); +} + +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) +{ + /* ---- CS beacon */ + /* PID */ + pid_init(&sensorboard.beacon.pid); + pid_set_gains(&sensorboard.beacon.pid, 80, 80, 250); + pid_set_maximums(&sensorboard.beacon.pid, 0, 10000, 2000); + pid_set_out_shift(&sensorboard.beacon.pid, 6); + pid_set_derivate_filter(&sensorboard.beacon.pid, 6); + + /* CS */ + cs_init(&sensorboard.beacon.cs); + cs_set_correct_filter(&sensorboard.beacon.cs, pid_do_filter, &sensorboard.beacon.pid); + cs_set_process_in(&sensorboard.beacon.cs, pwm_ng_set, BEACON_PWM); + cs_set_process_out(&sensorboard.beacon.cs, encoders_spi_update_beacon_speed, BEACON_ENCODER); + cs_set_consign(&sensorboard.beacon.cs, 0); + + /* ---- CS scanner */ + /* PID */ + pid_init(&sensorboard.scanner.pid); + pid_set_gains(&sensorboard.scanner.pid, 200, 5, 250); + pid_set_maximums(&sensorboard.scanner.pid, 0, 10000, 2047); + pid_set_out_shift(&sensorboard.scanner.pid, 6); + pid_set_derivate_filter(&sensorboard.scanner.pid, 6); + + /* QUADRAMP */ + quadramp_init(&sensorboard.scanner.qr); + quadramp_set_1st_order_vars(&sensorboard.scanner.qr, 200, 200); /* set speed */ + quadramp_set_2nd_order_vars(&sensorboard.scanner.qr, 20, 20); /* set accel */ + + /* CS */ + cs_init(&sensorboard.scanner.cs); + cs_set_consign_filter(&sensorboard.scanner.cs, quadramp_do_filter, &sensorboard.scanner.qr); + cs_set_correct_filter(&sensorboard.scanner.cs, pid_do_filter, &sensorboard.scanner.pid); + cs_set_process_in(&sensorboard.scanner.cs, pwm_ng_set, SCANNER_PWM); + cs_set_process_out(&sensorboard.scanner.cs, encoders_spi_update_scanner, SCANNER_ENCODER); + cs_set_consign(&sensorboard.scanner.cs, 0); + + /* Blocking detection */ + bd_init(&sensorboard.scanner.bd); + bd_set_speed_threshold(&sensorboard.scanner.bd, 150); + bd_set_current_thresholds(&sensorboard.scanner.bd, 500, 8000, 1000000, 40); + + /* set them on !! */ + sensorboard.beacon.on = 0; + sensorboard.scanner.on = 1; + + + scheduler_add_periodical_event_priority(do_cs, NULL, + 5000L / SCHEDULER_UNIT, + CS_PRIO); +} diff --git a/projects/microb2010/sensorboard/cs.h b/projects/microb2010/sensorboard/cs.h new file mode 100644 index 0000000..fe50c80 --- /dev/null +++ b/projects/microb2010/sensorboard/cs.h @@ -0,0 +1,25 @@ +/* + * 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 + * (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: cs.h,v 1.1 2009-03-29 18:44:54 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); diff --git a/projects/microb2010/sensorboard/diagnostic_config.h b/projects/microb2010/sensorboard/diagnostic_config.h new file mode 100644 index 0000000..e4ebb75 --- /dev/null +++ b/projects/microb2010/sensorboard/diagnostic_config.h @@ -0,0 +1,44 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: diagnostic_config.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + */ + +#ifndef _DEBUG_CONFIG_ +#define _DEBUG_CONFIG_ 1.0 // version + + +/** port line definition for the show_int_loop() function */ +/* undefine it to disable this functionnality */ +#define INTERRUPT_SHOW_PORT PORTA +#define INTERRUPT_SHOW_BIT 3 + + + +/** memory mark for the min_stack_space_available() function + the ram is filled with this value after a reset ! */ +#define MARK 0x55 + +/** the mark is inserted in whole RAM if this is enabled + (could lead to problems if you need to hold values through a reset...) + so it's better to disable it. + stack counting is not affected */ +//#define DIAG_FILL_ENTIRE_RAM + + +#endif //_DEBUG_CONFIG_ diff --git a/projects/microb2010/sensorboard/encoders_spi_config.h b/projects/microb2010/sensorboard/encoders_spi_config.h new file mode 100644 index 0000000..6a4a68b --- /dev/null +++ b/projects/microb2010/sensorboard/encoders_spi_config.h @@ -0,0 +1,33 @@ +/* + * 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 + * (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: encoders_spi_config.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + */ +#ifndef _ENCODERS_SPI_CONFIG_H_ +#define _ENCODERS_SPI_CONFIG_H_ + +#define ENCODERS_SPI_NUMBER 4 +#define ENCODERS_SPI_SS_PORT SS_PORT /* PORTB on atmega2560 */ +#define ENCODERS_SPI_SS_BIT SS_BIT /* 0 on atmega2560 */ + +/* see spi configuration */ +#define ENCODERS_SPI_CLK_RATE SPI_CLK_RATE_16 +#define ENCODERS_SPI_FORMAT SPI_FORMAT_3 +#define ENCODERS_SPI_DATA_ORDER SPI_LSB_FIRST + +#endif diff --git a/projects/microb2010/sensorboard/error_config.h b/projects/microb2010/sensorboard/error_config.h new file mode 100644 index 0000000..15ca2fc --- /dev/null +++ b/projects/microb2010/sensorboard/error_config.h @@ -0,0 +1,31 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: error_config.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + */ + +#ifndef _ERROR_CONFIG_ +#define _ERROR_CONFIG_ + +/** enable the dump of the comment */ +#define ERROR_DUMP_TEXTLOG + +/** enable the dump of filename and line number */ +#define ERROR_DUMP_FILE_LINE + +#endif diff --git a/projects/microb2010/sensorboard/gen_scan_tab.c b/projects/microb2010/sensorboard/gen_scan_tab.c new file mode 100644 index 0000000..7b94bfd --- /dev/null +++ b/projects/microb2010/sensorboard/gen_scan_tab.c @@ -0,0 +1,680 @@ +#include +#include +#include +#include + +#include + + +#include "scanner.h" + +#define ADC_REF_AVCC 0 +#define MUX_ADC13 0 + + + +double TELEMETRE_A = TELEMETRE_A_INIT; +double TELEMETRE_B = TELEMETRE_B_INIT; + + +#define printf_P printf +#define PSTR(a) (a) +int32_t motor_angle = 0; +int32_t scan_dist = 0; + + +int32_t H_fin, L_fin; +double L, H; + +#define nop() + +#define ABS(val) ({ \ + __typeof(val) __val = (val); \ + if (__val < 0) \ + __val = - __val; \ + __val; \ + }) + +struct scan_params scan_params; + +int32_t encoders_spi_get_value_scanner_interpolation(void *a) +{ + return motor_angle; +} + + +int16_t adc_get_value(uint8_t a) +{ + return scan_dist; +} + + + +/* get motor angle in radian; return mirror angle in radian, cos a sin a */ +void ang2_a_mirror(double b, double * c_a, double* s_a, double* a) +{ + double x2, y2; + double A, DELTA, B, D; + + b+=scan_params.offset_b; + x2 = X + l1*cos(b); + y2 = Y + l1*sin(b); + + A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3); + + DELTA = -(A*A - x2*x2 - y2*y2); + B = sqrt(DELTA); + + D = x2*x2 + y2*y2; + + *c_a = (x2*A + y2*B)/D; + *s_a = -(x2*B - y2*A)/D; + + *a = atan2(*s_a, *c_a); + + *a += scan_params.offset_a; + // *s_a = sin(*a); + // *c_a = cos(*a); + +} + +/* get telemeter dist , cos a, sin a, a and return H, L of scanned point */ +void ang2_H_L(double l_telemetre, double c_a, double s_a, double a, double *H, double *L) +{ + double d; + d = h_mirror*c_a/s_a; + *H = (l_telemetre - l_mirror - d)*sin(2*a); + *L = l_mirror + d + *H/tan(2*a); + + //*H+= 8*sin(a-scan_params.offset_a); +} + + + +//int32_t last_col_n; +//int32_t last_row_n; +//uint8_t last_sample; + +//uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123}; +//uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123}; + +/* last high is dummy, to deal higher columns */ +//uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250}; + + +//uint8_t h_limit[] = {60, 72, 85, 98, 112, 129, 250}; + +//uint8_t h_limit[] = {80, 97, 118, 134, 145, 160, 250}; + + +//uint8_t h_limit[] = {79, 94, 108, 117, 129, 144, 250}; + +uint8_t h_limit[] = {83, 95, 108, 120, 135, 147, 164, 250}; +#define H_MARGIN (-6) + +#define cs_set_consign(a, b) + + + +void do_scan(void * dummy) +{ + + unsigned int i; + int16_t a; + int32_t row_n; + int32_t col_n; + + + int32_t tour_pos; + int32_t pos, last_pos; + int32_t pos_tmp ; + int32_t mot_pos; + double dist; + uint8_t min_sample; + + double b, c_a, s_a, /*H, L,*/ m_a; + // int32_t H_fin; + + + //printf("scan\n"); + if (scan_params.sample_i==0) + return; + + mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); +#if 0 + if (scan_params.sample_i==1){ + printf_P(PSTR("dump end enc %ld %d \r\n"), mot_pos, PIX_PER_SCAN); + //scanner.flags &= (~CS_ON); + + + + /* stop scan at cur pos + 10 round */ + mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); + mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ; + + printf_P(PSTR("set to %ld \r\n"), mot_pos); + + cs_set_consign(&sensorboard.scanner.cs, mot_pos); + //pwm_ng_set(SCANNER_MOT_PWM, 0); + + + } + + mot_pos-= scan_params.pos_start_scan; +#endif + a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 ); + + + //dist = (a-TELEMETRE_B)/TELEMETRE_A; + //dist = TELEMETRE_A * a +TELEMETRE_B; + dist = telemetre_to_cm(a); + + //printf_P(PSTR("enc val = %ld\r\n"), encoders_microb_get_value((void *)SCANNER_ENC)); + + + //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a; + //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0; + + + row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); +#if 0 + /* separe scan forward/backword */ + if (row_n%2){ + row_n/=2; + } + else{ + row_n/=2; + row_n+=30; + } +#endif + + tour_pos = (mot_pos)%(SCANNER_STEP_TOUR); + + b = (2.*M_PI)*(double)tour_pos/(double)(SCANNER_STEP_TOUR); + + ang2_a_mirror(b, &c_a, &s_a, &m_a); + ang2_H_L(dist, c_a, s_a, m_a, &H, &L); + + + if (H >0){ + printf("zarb H\n"); + H = 0; + } + + if (dist> SCAN_MAX_DIST){ + H = 0; + L = 0; + } + + H = H;//m_a*180/M_PI; + L = L;//(L-100)*PIX_PER_SCAN; + + //printf_P(PSTR("polling : ADC0 = %i %f\r\n"),a, dist); + //printf_P(PSTR("%f %f %2.2f %f\r\n"), H, L, m_a*180./M_PI, dist); + + + //printf_P(PSTR("dist, b, a: %2.2f %2.2f %2.2f\r\n"), dist, b*180/M_PI, m_a*180/M_PI); + + H=(H+SCAN_H_MAX)*SCAN_H_COEF; + L-=SCAN_L_MIN; + + + /* XXX may never append */ + if (L<0) + L=0; + + + /* first filter => pixel modulo level */ +#define H_BASE 10 +#define H_MUL 14 + H_fin = H;//+SCAN_H_MAX; + //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE; + + if (scan_params.filter){ + H_fin = 11; // default is level 11 + for (i=0;iPIX_PER_SCAN) + printf("BUG!!! RECALC MAX L\r\n"); + + //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; + + //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; + //pos= row_n*PIX_PER_SCAN+tour_pos; + //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; + + + + pos= row_n*PIX_PER_SCAN+col_n; + last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; + + //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin); + + //a-=0x100; + a-=200; + //a/=10; + + if (0<= pos && pos 0xff?0xFF:a; + //sample_tab[(int)L] = H ; + scan_params.sample_tab[pos] = H_fin; + nop(); + if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ + /* we have a hole, pad it with minimal hight */ + if (H_fin>scan_params.last_sample) + min_sample = scan_params.last_sample; + else + min_sample = H_fin; + + //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n); + + /* fill grow, avoid erasing curent pos */ + if (pos > last_pos){ + pos_tmp = last_pos; + last_pos = pos; + //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); + + } + else{ + pos_tmp = pos+1; + //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); + } + + + for (;pos_tmp< last_pos;pos_tmp++){ + if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; + + //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; + + /* + if (((pos =DIM_DIST) + j = DIM_DIST-1; + + if (i>=DIM_ANGLE) + i = DIM_ANGLE-1; + + + val.u16 = pgm_read_word_near(&array_h_l[j][i]); + + + //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); + //val.u16 = pgm_read_word_near(&array_h_l[a][tp]); + H = val.h_l.h; + L = val.h_l.l; + /* + val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); + + H = val.h_l.h; + L = val.h_l.l; + */ + H_fin = H; + L_fin = L; + + + + col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN); + if (col_n>PIX_PER_SCAN) + printf("BUG!!! RECALC MAX L\r\n"); + + //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; + + //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; + //pos= row_n*PIX_PER_SCAN+tour_pos; + //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; + + row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); + + + pos= row_n*PIX_PER_SCAN+col_n; + last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; + + //printf_P(PSTR("%ld %ld %ld %ld\r\n"), row_n, col_n, pos, H_fin); + + //a-=0x100; + a-=200; + //a/=10; + + if (0<= pos && pos 0xff?0xFF:a; + //sample_tab[(int)L] = H ; + scan_params.sample_tab[pos] = H_fin; + nop(); + if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ + /* we have a hole, pad it with minimal hight */ + if (H_fin>scan_params.last_sample) + min_sample = scan_params.last_sample; + else + min_sample = H_fin; + + //printf("(%ld, %ld) (%ld %ld)\r\n", last_col_n, last_row_n, col_n, row_n); + + /* fill grow, avoid erasing curent pos */ + if (pos > last_pos){ + pos_tmp = last_pos; + last_pos = pos; + //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); + + } + else{ + pos_tmp = pos+1; + //printf("loop1 on (%ld, %ld) %ld\r\n", pos_tmp, last_pos, last_pos-pos_tmp); + } + + + for (;pos_tmp< last_pos;pos_tmp++){ + if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; + + //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; + + /* + if (((pos 1 && !strcmp(argv[1], "1")) + scan_params.filter = 1; + + + printf("max i max j %d %d \n", i, j); + dist_max = j; + angle_max = i; + + f_header = fopen("scan_h_l.h", "w"); + fprintf(f_header, "PROGMEM lookup_h_l array_h_l[%d][%d] = {\n", dist_max, angle_max); + for (j = 0, scan_dist = 0; j + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "actuator.h" +#include "beacon.h" +#include "scanner.h" + +void i2c_protocol_init(void) +{ +} + +/*** LED CONTROL ***/ +void i2c_led_control(uint8_t l, uint8_t state) +{ + switch(l) { + case 1: + state? LED1_ON():LED1_OFF(); + break; + case 2: + state? LED2_ON():LED2_OFF(); + break; + default: + break; + } +} + +void i2c_send_status(void) +{ + struct i2c_ans_sensorboard_status ans; + i2c_flush(); + ans.hdr.cmd = I2C_ANS_SENSORBOARD_STATUS; + ans.status = 0x55; /* XXX */ + ans.opponent_x = beacon.opponent_x; + ans.opponent_y = beacon.opponent_y; + ans.opponent_a = beacon.opponent_angle; + ans.opponent_d = beacon.opponent_dist; + + ans.scan_status = 0; + ans.scan_status |= scan_params.working ? 0 : I2C_SCAN_DONE; + ans.scan_status |= scan_params.max_column_detected ? I2C_SCAN_MAX_COLUMN : 0; + + + ans.dropzone_x = scan_params.dropzone_x; + ans.dropzone_y = scan_params.dropzone_y; + ans.dropzone_h = scan_params.dropzone_h; + + i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, + sizeof(ans), I2C_CTRL_GENERIC); +} + + +void i2c_scanner_calibre_laser(void* dummy) +{ + scanner_calibre_laser(); +} + +void i2c_scanner_end_process(void* dummy) +{ + scanner_end_process(); +} + +void i2c_recvevent(uint8_t * buf, int8_t size) +{ + void *void_cmd = buf; + static uint8_t a=0; + a=!a; + if (a) + LED2_ON(); + else + LED2_OFF(); + + if (size <= 0) { + goto error; + } + + switch (buf[0]) { + + /* Commands (no answer needed) */ + case I2C_CMD_GENERIC_LED_CONTROL: + { + struct i2c_cmd_led_control *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + i2c_led_control(cmd->led_num, cmd->state); + break; + } + + case I2C_CMD_GENERIC_SET_COLOR: + { + struct i2c_cmd_generic_color *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + sensorboard.our_color = cmd->color; + break; + } + + case I2C_CMD_SENSORBOARD_SET_BEACON: + { + struct i2c_cmd_sensorboard_start_beacon *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + + if (cmd->enable) + beacon_start(); + else + beacon_stop(); + + break; + } + + case I2C_CMD_SENSORBOARD_SET_SCANNER: + { + struct i2c_cmd_sensorboard_scanner *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + + scanner_set_mode(cmd->mode); + break; + + } + + + case I2C_CMD_SENSORBOARD_SCANNER_ALGO: + { + struct i2c_cmd_sensorboard_scanner_algo *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + + scan_params.algo = cmd->algo; + + if (cmd->algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE){ + scan_params.drop_zone.working_zone = cmd->drop_zone.working_zone; + scan_params.drop_zone.center_x = cmd->drop_zone.center_x; + scan_params.drop_zone.center_y = cmd->drop_zone.center_y; + } + else if (cmd->algo == I2C_SCANNER_ALGO_CHECK_TEMPLE) { + scan_params.check_temple.level = cmd->check_temple.level; + scan_params.check_temple.temple_x = cmd->check_temple.temple_x; + scan_params.check_temple.temple_y = cmd->check_temple.temple_y; + } + else if (cmd->algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE){ + scan_params.drop_zone.working_zone = cmd->drop_zone.working_zone; + scan_params.drop_zone.center_x = cmd->drop_zone.center_x; + scan_params.drop_zone.center_y = cmd->drop_zone.center_y; + } + else{ + /* new command */ + } + + scan_params.working = 1; + scheduler_add_single_event_priority(i2c_scanner_end_process, NULL, + 1, + CS_PRIO-1); + break; + + } + + case I2C_CMD_SENSORBOARD_CALIB_SCANNER: + { + struct i2c_cmd_sensorboard_calib_scanner *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + + scheduler_add_single_event_priority(i2c_scanner_calibre_laser, NULL, + 1, + CS_PRIO-1); + break; + } + + + + /* Add other commands here ...*/ + + + case I2C_REQ_SENSORBOARD_STATUS: + { + struct i2c_req_sensorboard_status *cmd = void_cmd; + if (size != sizeof (*cmd)) + goto error; + + beacon.robot_x = cmd->x; + beacon.robot_y = cmd->y; + beacon.robot_angle = cmd->a; + + if (cmd->enable_pickup_wheels) + pickup_wheels_on(); + else + pickup_wheels_off(); + + i2c_send_status(); + break; + } + + default: + goto error; + } + + error: + /* log error on a led ? */ + return; +} + +void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c) +{ +} + +void i2c_sendevent(int8_t size) +{ +} + + diff --git a/projects/microb2010/sensorboard/i2c_protocol.h b/projects/microb2010/sensorboard/i2c_protocol.h new file mode 100644 index 0000000..6fcac74 --- /dev/null +++ b/projects/microb2010/sensorboard/i2c_protocol.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation (2007) + * + * 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: i2c_protocol.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + */ + +#include + +void i2c_protocol_init(void); + +void i2c_recvevent(uint8_t * buf, int8_t size); +void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c); +void i2c_sendevent(int8_t size); + +int debug_send(char c, FILE* f); diff --git a/projects/microb2010/sensorboard/img_processing.c b/projects/microb2010/sensorboard/img_processing.c new file mode 100644 index 0000000..d030478 --- /dev/null +++ b/projects/microb2010/sensorboard/img_processing.c @@ -0,0 +1,1619 @@ +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include "img_processing.h" + + +#define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) + +#ifndef HOST_VERSION + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" + +#define IMG_DEBUG(args...) DEBUG(E_USER_IMGPROCESS, args) +#define IMG_NOTICE(args...) NOTICE(E_USER_IMGPROCESS, args) +#define IMG_ERROR(args...) ERROR(E_USER_IMGPROCESS, args) + +#else + +#define IMG_DEBUG(args...) debug_printf(args) +#define IMG_NOTICE(args...) debug_printf(args) +#define IMG_ERROR(args...) debug_printf(args) + +#endif + + + + +#define OBJECT_MINIMUM_DEMI_PERIMETER (2*3) +/* store object in pool if never seen + * returns: + * 1 if new object + * 0 if already known + */ +int store_obj(Object_bb* tab_o, int total, Object_bb*o) +{ + uint8_t i; + + if (o->x_max - o->x_min + o->y_max - o->y_min < OBJECT_MINIMUM_DEMI_PERIMETER) + return 0; + + for (i=0;ix_max = 0; + o->y_max = 0; + o->x_min = x_in; + o->y_min = y_in; + + + while(1){ + if (pos_x == start_x && pos_y == start_y){ + count_stand++; + if (count_stand>4) + break; + if( v.x == vi.x && v.y == vi.y && start) + break; + } + + if (pos_xx_min) + o->x_min = pos_x; + if (pos_yy_min) + o->y_min = pos_y; + if (pos_x>o->x_max) + o->x_max = pos_x; + if (pos_y>o->y_max) + o->y_max = pos_y; + + /* is next pixel is good color */ + if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){ + pos_x = pos_x+v.x; + pos_y = pos_y+v.y; + len++; + vect_rot_retro(&v); + start = 1; + continue; + } + vect_rot_trigo(&v); + } + + o->len = len; +} + + + + + +/* step around object of given color and computes its polygon */ +void object_find_poly(unsigned char* data, + int16_t x_in, int16_t y_in, + int16_t start_x, int16_t start_y, + int16_t color, int16_t color_w, Object_poly* o) +{ + int16_t pos_x = start_x; + int16_t pos_y = start_y; + int16_t start =0; + int16_t len = 0; + int16_t count_stand = 0; + uint16_t pt_step, pt_num; + vect_t v, vi; + + v.x = 1; + v.y = 0; + + vi = v; + + pt_step = o->len/POLY_MAX_PTS + 1; + + pt_num = 0; + + while(1){ + if (pos_x == start_x && pos_y == start_y){ + count_stand++; + if (count_stand>4) + break; + if( v.x == vi.x && v.y == vi.y && start) + break; + } + + /* is next pixel is good color */ + if (data[(pos_y+v.y)*x_in + pos_x+v.x] != color){ + pos_x = pos_x+v.x; + pos_y = pos_y+v.y; + len++; + + if (len >pt_num*pt_step){ + o->pts[pt_num].x = pos_x; + o->pts[pt_num].y = pos_y; + pt_num+=1; + if (pt_num>=POLY_MAX_PTS) + break; + } + + vect_rot_retro(&v); + start = 1; + continue; + } + vect_rot_trigo(&v); + } + + o->pts_num = pt_num; + +} + +#define PT_LEFT 0 +#define PT_RIGHT 2 +#define PT_TOP 1 +#define PT_DOWN 3 + +/* return most left/right/top/down pts indexes of given polygon */ +void object_find_extrem_points_index(Object_poly* o, + unsigned int *pts) +{ + unsigned int i; + for (i=0;i<4;i++) + pts[i] = 0; + + + for (i=1;ipts_num;i++){ + if (o->pts[i].x < o->pts[pts[PT_LEFT]].x) + pts[PT_LEFT] = i; + if (o->pts[i].x > o->pts[pts[PT_RIGHT]].x) + pts[PT_RIGHT] = i; + if (o->pts[i].y < o->pts[pts[PT_TOP]].y) + pts[PT_TOP] = i; + if (o->pts[i].y > o->pts[pts[PT_DOWN]].y) + pts[PT_DOWN] = i; + } + +} + + +#define NUM_CALIPERS 4 +/* for debug purpose: display a vector on image */ +void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in, + vect_t *v, point_t p) +{ + unsigned int i; + float n; + int16_t x, y; + float coef=1.0; + + if (!v->x && !v->y) + return; + + n = vect_norm(v); + coef = 1/n; + for (i=0;i<5;i++){ + x = p.x; + y = p.y; + + x+=(float)((v->x)*(float)i*(float)coef); + y+=(float)((v->y)*(float)i*(float)coef); + if ((x== p.x) && (y == p.y)) + buf[y*x_in+x] = 0x0; + else + buf[y*x_in+x] = 0x0; + } + +} + +#define CAL_X 3 +#define CAL_Y 0 + + +/* compute minimum rectangle area including the given convex polygon */ +void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out, + vect_t *v_out, vect_t *r1, vect_t*r2) +{ + vect_t calipers[NUM_CALIPERS]; + vect_t edges[NUM_CALIPERS]; + + unsigned int i; + unsigned int calipers_pts_index[NUM_CALIPERS]; + float angles[NUM_CALIPERS]; + float min_angle; + float total_rot_angle = 0; + int caliper_result_index; + + vect_t res1, res2; + float ps, n1, n2, caliper_n; + + float aera_tmp; + /* XXX hack sould be max*/ + float aera_min=0x100000; + + object_find_extrem_points_index(o, calipers_pts_index); + + calipers[0].x = 0; + calipers[0].y = 1; + + calipers[1].x = -1; + calipers[1].y = 0; + + calipers[2].x = 0; + calipers[2].y = -1; + + calipers[3].x = 1; + calipers[3].y = 0; + + + while (total_rot_angle <= M_PI/2){ + + for (i=0;ipts[(calipers_pts_index[i] + 1)%o->pts_num].x - + o->pts[calipers_pts_index[i]].x; + edges[i].y = o->pts[(calipers_pts_index[i] + 1)%o->pts_num].y - + o->pts[calipers_pts_index[i]].y; + + /* compute angle between caliper and polygon edge */ + angles[i] = vect_get_angle(&edges[i], &calipers[i]); + } + + /* find min angle */ + min_angle = angles[0]; + caliper_result_index = 0; + for (i=1;ipts_num; i++){ + calipers[(i+1) % NUM_CALIPERS] = calipers[i % NUM_CALIPERS]; + vect_rot_trigo(&calipers[(i+1) % NUM_CALIPERS]); + } + + /* update calipers point */ + for (i=0;ipts_num; + } + + res1.x = o->pts[calipers_pts_index[2]].x - o->pts[calipers_pts_index[0]].x; + res1.y = o->pts[calipers_pts_index[2]].y - o->pts[calipers_pts_index[0]].y; + + res2.x = o->pts[calipers_pts_index[3]].x - o->pts[calipers_pts_index[1]].x; + res2.y = o->pts[calipers_pts_index[3]].y - o->pts[calipers_pts_index[1]].y; + + ps = vect_pscal(&res1, &calipers[CAL_X]); + n1 = vect_norm(&res1); + caliper_n = vect_norm(&calipers[CAL_X]); + caliper_n*=caliper_n; + + res1 = calipers[CAL_X]; + + res1.x *= ps/(caliper_n); + res1.y *= ps/(caliper_n); + + + ps = vect_pscal(&res2, &calipers[CAL_Y]); + n1 = vect_norm(&res2); + + res2 = calipers[CAL_Y]; + + res2.x *= ps/(caliper_n); + res2.y *= ps/(caliper_n); + + n1 = vect_norm(&res1); + n2 = vect_norm(&res2); + + aera_tmp = n1*n2; + + if (aera_min >aera_tmp){ + aera_min = aera_tmp; + for (i=0;ipts[pts_index_out[i]].x; + p1.y = o->pts[pts_index_out[i]].y; + + p2.x = p1.x+COEF_CALIP*caliper_tmp.x; + p2.y = p1.y+COEF_CALIP*caliper_tmp.y; + + pts2line(&p1, &p2, &l1); + + vect_rot_trigo(&caliper_tmp); + + p1.x = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].x; + p1.y = o->pts[pts_index_out[(i+1)%NUM_CALIPERS]].y; + + p2.x = p1.x+COEF_CALIP*caliper_tmp.x; + p2.y = p1.y+COEF_CALIP*caliper_tmp.y; + + pts2line(&p1, &p2, &l2); + + ret = intersect_line(&l1, &l2, &p_int1); + if (ret!=1) + return 0; + //IMG_DEBUG("int1 (%d): %" PRIi32 " %" PRIi32 " ", ret, p_int1.x, p_int1.y); + + mp_x+=p_int1.x; + mp_y+=p_int1.y; + + } + + + + p->x = lround(mp_x/NUM_CALIPERS); + p->y = lround(mp_y/NUM_CALIPERS); + + + return 1; + +} + +#define OBJECT_DIM 5 +/* split zone in many column's sized area */ +int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color) +{ + int n1, n2; + int i, j; + int index=0; + int r1_s, r2_s; + point_t ptmp; + + + n1 = vect_norm(r1); + n2 = vect_norm(r2); + + r1_s = n1/OBJECT_DIM; + r2_s = n2/OBJECT_DIM; + + if (!r1_s || ! r2_s) + return 0; + + ptmp.x = p->x - r1->x/2 - r2->x/2 + (r1->x/(r1_s*2))+(r2->x/(r2_s*2)); + ptmp.y = p->y - r1->y/2 - r2->y/2 + (r1->y/(r1_s*2))+(r2->y/(r2_s*2)); + + for(i=0;ix)/r1_s+(j*r2->x)/r2_s; + zones[index].p.y = ptmp.y + (i*r1->y)/r1_s+(j*r2->y)/r2_s; + zones[index].h = color; + zones[index].valid = 1; + + index++; + if (index>=max_zone) + return index; + + + } + } + + return index; +} + +#define OBJECT_SEMI_DIM (OBJECT_DIM/2) +#define MIN_SURFACE_PERCENT 50 +#define HIGHER_MAX_PIXEL 5 + + +int zone_has_enought_pixels(unsigned char* data, int16_t x_in, int16_t y_in, zone* z) +{ + int x, y; + uint16_t count, total_pix, higher_pixels; + + count = 0; + total_pix=0; + higher_pixels = 0; + + for (x = -OBJECT_SEMI_DIM; + (x <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL); + x++){ + for (y = -OBJECT_SEMI_DIM; + (y <= OBJECT_SEMI_DIM) && (higher_pixels < HIGHER_MAX_PIXEL); + y++){ + total_pix++; + if (data[x_in * (y + z->p.y) + x + z->p.x] == z->h) + count++; + + if (data[x_in * (y + z->p.y) + x + z->p.x] > z->h) + higher_pixels++; + + } + + } + + IMG_DEBUG("has enougth pixel (h: %d x %"PRIi32": y:%"PRIi32") total: %d/%d (tt: %d, hmax: %d)", z->h, z->p.x, z->p.y, + count, (total_pix * MIN_SURFACE_PERCENT) / 100, total_pix, higher_pixels); + + if ((count > (total_pix * MIN_SURFACE_PERCENT) / 100) && + (higher_pixels (CENTER_MIN_DIST+tweak_min_margin) * (CENTER_MIN_DIST+tweak_min_margin)) + continue; + + p[i].valid = 0; + + } + + return zones_num; +} + +#define MAX_DIST_TO_ZONE 2 + +unsigned int zone_filter_zone_rect(unsigned int zones_num, zone* p, int16_t center_x, int16_t center_y , uint8_t working_zone) +{ + int i; + + for (i = 0; i < zones_num; i++){ + + IMG_DEBUG("rct x:%"PRIi32" y:%"PRIi32" (centerx: %d)",p[i].p.x , p[i].p.y, center_x); + + if ((p[i].p.x > center_x - MAX_DIST_TO_ZONE) && (p[i].h > working_zone)) + continue; + + p[i].valid = 0; + } + + return zones_num; +} + + +/* delete point to render polygon convex */ +int object_poly_to_convex(Object_poly *o) +{ + unsigned int i, j; + vect_t v, w; + int16_t z; + unsigned int del_pts_num = 0; + + for (i=0;ipts_num;){ + v.x = o->pts[(i + o->pts_num - 1)%o->pts_num].x - o->pts[i].x; + v.y = o->pts[(i + o->pts_num - 1)%o->pts_num].y - o->pts[i].y; + + w.x = o->pts[(i+1)%o->pts_num].x - o->pts[i].x; + w.y = o->pts[(i+1)%o->pts_num].y - o->pts[i].y; + + z = vect_pvect(&v, &w); + if (z>0){ + i+=1; + continue; + } + + /* found a convex angle (or colinear points) */ + for (j = i; j < o->pts_num-1; j++){ + o->pts[j] = o->pts[j+1]; + } + if (i!=0) + i-=1; + o->pts_num--; + del_pts_num++; + } + + return del_pts_num; +} + + + + + +#define DEFAULT_COLOR 255 +/* scan all image and find objects*/ +unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in, + Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj) +{ + int16_t i, obj_num; + + uint8_t in_color=0; + int16_t etat; + + Object_bb o; + int ret; + + obj_num = 0; + /* + first, delete borders + */ + for (i=0;i= MAX_SISTER_PER_ZONE) + break; + + + if (!zones[i].valid) + continue; + + + if (i == z_n) + continue; + + /* twin tower must have same high */ + if (zones[i].h != zones[z_n].h) + continue; + + IMG_DEBUG("test sisters (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", + zones[z_n].p.x, zones[z_n].p.y, + zones[i].p.x, zones[i].p.y); + + + dist = sqrt( (zones[i].p.x - zones[z_n].p.x) * (zones[i].p.x - zones[z_n].p.x) + + (zones[i].p.y - zones[z_n].p.y) * (zones[i].p.y - zones[z_n].p.y) ); + + + IMG_DEBUG("sister space is %2.2f may be near %d", dist, SPACE_INTER_TWIN_TOWER); + + /* + twin tower must be close/far enought to drop lintel + */ + if (ABS(dist - SPACE_INTER_TWIN_TOWER) > SPACE_INTER_TWIN_TOWER_TOLERENCE) + continue; + + + pts2line(&zones[i].p, &zones[z_n].p, &l); + + /* + test the paralelism of the temple: + zone may be on same distance from center + */ + + + v1.x = zones[z_n].p.x - center_x; + v1.y = zones[z_n].p.y - center_y; + + dist = vect_norm(&v1); + + v2.x = zones[i].p.x - center_x; + v2.y = zones[i].p.y - center_y; + + dist2 = vect_norm(&v2); + + IMG_DEBUG("zone dist %2.2f %2.2f", dist, dist2); + if (ABS(dist-dist2) > 3){ + IMG_DEBUG("bad parallelism %2.2f", ABS(dist-dist2)); + continue; + } + + + + + /* no other aligned tower to avoid dropping on a lintel + * (3 aligned zone may mean lintel) + */ + + good_zone = 1; + + for (j = 0; j < zones_num; j++){ + if (j==i ||j == z_n) + continue; + + /* if third zone, but lower */ + if (zones[j].h <= zones[i].h) + continue; + + /* + check distance from dropping zone to + line formed by twin towers + */ + + proj_pt_line(&zones[j].p, &l, &p); + + + /* test if projected point is in the segement */ + + + v.x = zones[z_n].p.x - zones[i].p.x; + v.y = zones[z_n].p.y - zones[i].p.y; + + v1.x = p.x - zones[i].p.x; + v1.y = p.y - zones[i].p.y; + + n1 = vect_pscal_sign(&v, &v1); + + v.x = -v.x; + v.y = -v.y; + + v1.x = p.x - zones[z_n].p.x; + v1.y = p.y - zones[z_n].p.y; + + + n2 =vect_pscal_sign(&v, &v1); + + v.x = p.x - zones[j].p.x; + v.y = p.y - zones[j].p.y; + + dist = vect_norm(&v); + IMG_DEBUG("dist pt h %d n: (%d %d) (%"PRIi32" %"PRIi32") to line %2.2f", zones[j].h, n1, n2, zones[j].p.x, zones[j].p.y, dist); + + + if ((n1>=0 && n2>=0) && dist < OBJECT_DIM+2.){ + good_zone = 0; + break; + } + + + /* test if zone is far from points*/ + + v1.x = zones[j].p.x - zones[z_n].p.x; + v1.y = zones[j].p.y - zones[z_n].p.y; + + dist = vect_norm(&v1); + IMG_DEBUG("dist pt to z1 %2.2f", dist); + + if (dist < OBJECT_DIM){ + good_zone = 0; + break; + } + + v2.x = zones[j].p.x - zones[i].p.x; + v2.y = zones[j].p.y - zones[i].p.y; + + + dist = vect_norm(&v2); + IMG_DEBUG("dist pt to z2 %2.2f", dist); + + if (dist < OBJECT_DIM){ + good_zone = 0; + break; + } + + + + z1 = i; + z2 = z_n; + z3 = j; + + + + + + /* + XXX may be a lintel on lintel !! + */ + + } + + if (!good_zone) + continue; + + IMG_DEBUG("sisters ok (%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", + zones[z_n].p.x, zones[z_n].p.y, + zones[i].p.x, zones[i].p.y); + + + sisters[z_n][current_sister] = i; + current_sister++; + } + } +} + +/* test if a higher zone is too close */ +int test_close_zone(uint8_t zones_num, zone* zones, unsigned int z_n) +{ + uint8_t i; + vect_t v; + double dist; + + for (i = 0; i < zones_num; i++){ + if (i == z_n) + continue; + if (zones[i].h <= zones[z_n].h) + continue; + + v.x = zones[i].p.x - zones[z_n].p.x; + v.y = zones[i].p.y - zones[z_n].p.y; + + dist = vect_norm(&v); + //IMG_DEBUG("dist pt to pt %2.2f", dist); + + if (dist < OBJECT_DIM){ + return 1; + } + + } + + return 0; +} + +#define MAX_COLUMN 4 +#define MAX_LINTEL 2 + +drop_column_zone drop_c[MAX_COLUMN]; +drop_lintel_zone drop_l[MAX_LINTEL]; + + +void reset_drop_zone(void) +{ + memset(drop_c, 0, sizeof(drop_c)); + memset(drop_l, 0, sizeof(drop_l)); + +} + + +void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones) +{ + unsigned int i; + + for (i=0;i(b)?(a):(b)) + + +#if 0 +#define MAX_DROP_HIGH 8 + + +/* + recursive function to maximize points during object + dropping, given lintel/column number + working zone may be 1, 2 or 3 + */ +unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max, + uint8_t n_columns, uint8_t n_lintels, + uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone) +{ + + uint8_t i, j; + unsigned int points_calc; + //unsigned int points_added = 0; + int sister; + int ret; + + + /* if no more objects, return points */ + if (n_columns == 0 && n_lintels == 0) + return MY_MAX(points, points_max); + + /* start by putting columns if so */ + for (i = 0; i < zones_num; i++){ + if (zones[i].h >= MAX_DROP_HIGH) + continue; + + if (n_columns){ + + ret = test_close_zone(zones_num, zones, i); + if (ret) + continue; + + zones[i].h++; + points_calc = solve_objects_dropping(points + zones[i].h, points_max, + n_columns - 1, n_lintels, + zones_num, zones, sisters, working_zone); + + if (points_calc > points_max){ + points_max = points_calc; + drop_c[n_columns - 1].z = i; + drop_c[n_columns - 1].h = zones[i].h; + drop_c[n_columns - 1].valid = 1; + + } + zones[i].h--; + } + /* we must depose all columns before dropping lintels */ + else if (n_lintels){ + + /* dont drop lintel on ground */ + if (zones[i].h <= working_zone) + continue; + + /* XXX todo need get second zone near selected one */ + //ret = find_twin_tower(zones_num, zones, i, &sister); + + for (j = 0; j < MAX_SISTER_PER_ZONE; j++){ + sister = sisters[i][j]; + if (sister == -1) + break; + if (zones[i].h != zones[sister].h){ + sister = -1; + } + + } + + if (sister == -1) + continue; + //IMG_DEBUG("sister found: %d %d (h=%d %p)", i, sister, zones[i].h, &zones[i].h); + + zones[i].h++; + zones[sister].h++; + + + points_calc = solve_objects_dropping(points + zones[i].h * 3, points_max, + n_columns, n_lintels - 1, + zones_num, zones, sisters, working_zone); + + if (points_calc > points_max){ + points_max = points_calc; + + drop_l[n_lintels - 1].z1 = i; + drop_l[n_lintels - 1].z2 = sister; + drop_l[n_lintels - 1].h = zones[i].h; + drop_l[n_lintels - 1].valid = 1; + + } + + + zones[sister].h--; + zones[i].h--; + } + } + + return MY_MAX(points, points_max); +} +#endif + + +/* */ +int find_column_dropzone(uint8_t zones_num, zone* zones) +{ + uint8_t i; + + uint8_t z_n = 0; + + if (zones_num <= 0) + return -1; + + for (i = 0; i < zones_num; i++){ + if (!zones[i].valid) + continue; + if (zones[i].h > zones[z_n].h) + z_n = i; + } + + + /* + now, chose dropzone closest to robot + meaning little x, big y + so maximise y-x + */ + for (i = 0; i < zones_num; i++){ + if (zones[i].h != zones[z_n].h) + continue; + if (!zones[i].valid) + continue; + if (zones[i].p.y - zones[i].p.x > zones[z_n].p.y - zones[z_n].p.x) + z_n = i; + } + + + + return z_n; +} + + + +uint8_t color2h(uint8_t color) +{ + return (0x100-color)/0x20; +} + +uint8_t h2color(uint8_t color) +{ + return color*0x20; +} + + +#define NUM_ZONE_GENERATE 8 +#define DIST_ZONE_GEN 9 + +/* + remove zone at ground level, and generate zones on + a circle at X cm from center + */ +unsigned int generate_center_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in, + zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y) +{ + double c_a, s_a; + uint8_t i, j; + double px1, py1, px2, py2; + + + /* first del zone at level 2 */ + for (i = 0; i < zones_num; ){ + if (zones[i].h!=2){ + i++; + continue; + } + + for (j = i; j < zones_num-1; j++) + zones[j] = zones[j+1]; + + zones_num--; + + } + + /* generate zones around circle */ + + c_a = cos(2*M_PI/NUM_ZONE_GENERATE); + s_a = sin(2*M_PI/NUM_ZONE_GENERATE); + + px1 = DIST_ZONE_GEN; + py1 = 0; + + for (i = 0; i < NUM_ZONE_GENERATE; i++){ + + zones[zones_num].p.x = center_x + px1; + zones[zones_num].p.y = center_y + py1; + zones[zones_num].h = 2; + zones[zones_num].valid = 1; + + + px2 = px1*c_a + py1*s_a; + py2 = -px1*s_a + py1*c_a; + + px1 = px2; + py1 = py2; + + /* skip zone if it is not in img */ + if (zones[zones_num].p.x < 0 || zones[zones_num].p.y < 0 || + zones[zones_num].p.x >= x_in || zones[zones_num].p.y > y_in) + continue; + + /* skip zone if not enougth pixels */ + if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num])) + continue; + + zones_num++; + if (zones_num >= max_zones) + break; + + } + + return zones_num; + + +} + + +/* + remove zone at ground level, and generate zones on + a line at X cm from robot + */ +unsigned int generate_rectangle_ground_zones(unsigned char* data, int16_t x_in, int16_t y_in, + zone * zones, unsigned int zones_num, uint8_t max_zones, int16_t center_x, int16_t center_y, + uint8_t working_zone) +{ + uint8_t i, j; + uint8_t y; + + /* first del zone at level i */ + for (i = 0; i < zones_num; ){ + if (zones[i].h != working_zone ){ + i++; + continue; + } + + for (j = i; j < zones_num-1; j++) + zones[j] = zones[j+1]; + + zones_num--; + } + + + /* generate zones on a line */ + for (y = OBJECT_DIM; y < y_in; y+=OBJECT_DIM){ + + zones[zones_num].p.x = center_x; + zones[zones_num].p.y = y; + zones[zones_num].h = working_zone ; + zones[zones_num].valid = 1; + + if (!zone_has_enought_pixels(data, x_in, y_in, &zones[zones_num])) + continue; + zones_num++; + + if (zones_num >= max_zones) + break; + + } + return zones_num; + +} + + +#define MAX_DECAL_LINE 5 +#define ENOUGHT_ZONE_PIXEL 2 + +void recal_img_y(unsigned char* buffer, int16_t x_in, int16_t y_in, + uint8_t working_zone) +{ + uint8_t i, j; + uint8_t cpt; + + /* recal img only for central zone */ + if (working_zone !=2) + return; + + for (i = 0; i < MAX_DECAL_LINE; i++){ + cpt = 0; + for (j = 0; j < x_in; j++){ + if (buffer[i*x_in + j] ==2) + cpt++; + } + + if (cpt >= ENOUGHT_ZONE_PIXEL) + break; + } + + memmove(buffer, &buffer[i * x_in], x_in * y_in - i*x_in); + memset(&buffer[x_in * y_in - i * x_in], 0, i*x_in); +} + + +#define MAX_OBJECTS 20 + +#define MAX_ZONES_PER_OBJECT 20 + + +uint8_t g_zone_num; +zone g_all_zones[MAX_ZONES]; + +uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in, + zone * all_zones, uint8_t max_zones) +{ + + int ret; + int i, j; + + zone zones[MAX_ZONES_PER_OBJECT]; + + vect_t caliper; + unsigned int pts_cal[4]; + point_t ptmp; + vect_t r1, r2; + int zone_len; + + + uint8_t zone_num = 0; + + Object_bb sac_obj[MAX_OBJECTS]; + Object_poly sac_obj_poly[MAX_OBJECTS]; + + + + /* + XXX fix: only decal for working zone 2/(1?) + but we dont have info yet + */ + recal_img_y(buffer, x_in, y_in, 2); + + memset(sac_obj, 0, sizeof(sac_obj)); + memset(sac_obj_poly, 0, sizeof(sac_obj_poly)); + + + /* first, find polygons*/ + parcour_img(buffer, x_in, y_in, sac_obj, sac_obj_poly, MAX_OBJECTS); + + /* enclose each poygon in the min area polygon + then, split each rectangle in dropping zone + */ + for (i=0;i 0; i--){ + z.h = i; + z.valid = 1; + ret = zone_has_enought_pixels(buffer, x_in, y_in, &z); + IMG_NOTICE("test z3:(h=%d) %d", i, ret); + + if (ret) + return 1; + } + + return 0; + +} + + +int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, + uint8_t working_zone, int16_t center_x, int16_t center_y, + int16_t * dropzone_x, int16_t * dropzone_y) +{ + uint8_t zone_num; + int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE]; + int8_t z_n = -1; + uint8_t i, j; + + + /* find all drop zone */ + zone_num = filter_zones(buffer, x_in, y_in, + g_all_zones, g_zone_num, MAX_ZONES, + working_zone, center_x, center_y, + -2); + + /* precompute possible twin towers */ + find_twin_tower(zone_num, g_all_zones, sisters, center_x, center_y); + + + for (i=0; i< zone_num; i++){ + IMG_DEBUG("all sisters: %d", i); + for (j=0;j g_all_zones[i].h) + continue; + + z_n = i; + } + + IMG_NOTICE("twin tower found :z_n=%d", z_n); + if (z_n == -1) + return -1; + + IMG_NOTICE("(%"PRIi32" %"PRIi32") (%"PRIi32" %"PRIi32")", + g_all_zones[z_n].p.x, g_all_zones[z_n].p.y, + g_all_zones[sisters[z_n][0]].p.x, g_all_zones[sisters[z_n][0]].p.y); + + + *dropzone_x = ((double)(g_all_zones[z_n].p.x + g_all_zones[sisters[z_n][0]].p.x)*PIXEL2CM ) / 2; + *dropzone_y = ((double)(g_all_zones[z_n].p.y + g_all_zones[sisters[z_n][0]].p.y)*PIXEL2CM ) / 2 + 30; + + + return g_all_zones[z_n].h; + +} + diff --git a/projects/microb2010/sensorboard/img_processing.h b/projects/microb2010/sensorboard/img_processing.h new file mode 100644 index 0000000..4c06237 --- /dev/null +++ b/projects/microb2010/sensorboard/img_processing.h @@ -0,0 +1,111 @@ + + + +typedef struct _Object_bb +{ + uint8_t x_min; + uint8_t x_max; + uint8_t y_min; + uint8_t y_max; + uint16_t len; +}Object_bb; + +#define POLY_MAX_PTS 12 +typedef struct _Object_poly +{ + uint16_t pixels_perim; + uint16_t pts_num; + point_t pts[POLY_MAX_PTS]; + uint16_t len; + uint8_t color; +}Object_poly; + + +typedef struct _zone +{ + point_t p; + uint8_t h:7; + uint8_t valid:1; +}zone; + + +typedef struct _drop_column_zone +{ + uint8_t valid; + uint8_t z; + uint8_t h; +}drop_column_zone; + +typedef struct _drop_lintel_zone +{ + uint8_t valid; + uint8_t z1; + uint8_t h; + uint8_t z2; +}drop_lintel_zone; + + + + +#define MAX_ZONES 30 + +#define MAX_SISTER_PER_ZONE 1 + + + +unsigned char *parcour_img(unsigned char* data, int16_t x_in, int16_t y_in, Object_bb *sac_obj, Object_poly *sac_obj_poly, int16_t max_obj); + +float vect_get_angle(vect_t*v, vect_t* w); + +void object_poly_get_min_ar(Object_poly *o, unsigned int *pts_index_out, vect_t *v_out, vect_t *r1, vect_t*r2); + +void draw_pt_vect(unsigned char *buf, int16_t x_in, int16_t y_in, vect_t *v, point_t p); + +void vect_rot_trigo(vect_t* v); + +int object_poly_caliper_to_rectangle(Object_poly *o, + unsigned int *pts_index_out, vect_t* caliper, + vect_t *r1, vect_t*r2, point_t *p); + +int split_rectangle(point_t *p, vect_t *r1, vect_t* r2, uint8_t max_zone, zone* zones, uint8_t color); + + + +int zone_filtre_min_surface(unsigned char* data, int16_t x_in, int16_t y_in, + uint8_t color, unsigned int num_zone, zone* p); + +void reset_drop_zone(void); +void display_drop_zones(uint8_t n_columns, uint8_t n_lintels, zone* zones); + +unsigned int solve_objects_dropping(unsigned int points, unsigned int points_max, + uint8_t n_columns, uint8_t n_lintels, + uint8_t zones_num, zone* zones, int8_t sisters[MAX_ZONES][MAX_SISTER_PER_ZONE], uint8_t working_zone); + +uint8_t process_img(unsigned char *buffer, int16_t x_in, int16_t y_in, + zone * all_zones, uint8_t max_zones); + + +uint8_t color2h(uint8_t color); +uint8_t h2color(uint8_t color); + +int8_t get_column_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, + uint8_t working_zone, int16_t center_x, int16_t center_y, + int16_t * dropzone_x, int16_t * dropzone_y); + + +uint8_t is_temple_there(unsigned char * buffer, int16_t x_in, int16_t y_in, + uint8_t h, int16_t center_x, int16_t center_y); + + + +int8_t find_temple_dropzone(unsigned char *buffer, int16_t x_in, int16_t y_in, + uint8_t working_zone, int16_t center_x, int16_t center_y, + int16_t * dropzone_x, int16_t * dropzone_y); + +void process_img_to_zone(unsigned char *buffer, int16_t x_in, int16_t y_in); + +#define PIXEL2CM (300./27.) + + +extern uint8_t g_zone_num; +extern zone g_all_zones[MAX_ZONES]; diff --git a/projects/microb2010/sensorboard/main.c b/projects/microb2010/sensorboard/main.c new file mode 100755 index 0000000..82fa2a5 --- /dev/null +++ b/projects/microb2010/sensorboard/main.c @@ -0,0 +1,279 @@ +/* + * 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 + * (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: main.c,v 1.4 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "../common/eeprom_mapping.h" +#include "../common/i2c_commands.h" + +#include "main.h" +#include "ax12_user.h" +#include "cmdline.h" +#include "sensor.h" +#include "actuator.h" +#include "cs.h" +#include "i2c_protocol.h" +#include "beacon.h" +#include "scanner.h" + +/* 0 means "programmed" + * ---- with 16 Mhz quartz + * CKSEL 3-0 : 0111 + * SUT 1-0 : 10 + * CKDIV8 : 1 + * ---- bootloader + * BOOTZ 1-0 : 01 (4K bootloader) + * BOOTRST : 0 (reset on bootloader) + * ---- jtag + * jtagen : 0 + */ + +struct genboard gen; +struct sensorboard sensorboard; + +/***********************/ + +void bootloader(void) +{ +#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; + TIMSK2 = 0; + TIMSK3 = 0; + TIMSK4 = 0; + TIMSK5 = 0; + EIMSK = 0; + UCSR0B = 0; + UCSR1B = 0; + UCSR2B = 0; + UCSR3B = 0; + SPCR = 0; + TWCR = 0; + ACSR = 0; + ADCSRA = 0; + + EIND = 1; + __asm__ __volatile__ ("ldi r31,0xf8\n"); + __asm__ __volatile__ ("ldi r30,0x00\n"); + __asm__ __volatile__ ("eijmp\n"); + + /* never returns */ +} + +void do_led_blink(__attribute__((unused)) void *dummy) +{ +#if 1 /* simple blink */ + static uint8_t a=0; + + if(a) + LED1_ON(); + else + LED1_OFF(); + + a = !a; +#endif +} + +static void main_timer_interrupt(void) +{ + static uint8_t cpt = 0; + cpt++; + sei(); + if ((cpt & 0x3) == 0) + scheduler_interrupt(); +} + +int main(void) +{ + /* brake */ + BRAKE_OFF(); + BRAKE_DDR(); + + /* CPLD reset on PG3 */ + DDRG |= 1<<3; + PORTG &= ~(1<<3); /* implicit */ + + /* LEDS */ + DDRJ |= 0x0c; + DDRL = 0xc0; + LED1_OFF(); + memset(&gen, 0, sizeof(gen)); + memset(&sensorboard, 0, sizeof(sensorboard)); + sensorboard.flags = DO_ENCODERS | DO_CS | DO_POWER; // DO_BD + + /* UART */ + uart_init(); +#if CMDLINE_UART == 3 + fdevopen(uart3_dev_send, uart3_dev_recv); + uart_register_rx_event(3, emergency); +#elif CMDLINE_UART == 1 + fdevopen(uart1_dev_send, uart1_dev_recv); + uart_register_rx_event(1, emergency); +#else +# error not supported +#endif + + //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_SENSORBOARD); + /* check eeprom to avoid to run the bad program */ + if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != + EEPROM_MAGIC_SENSORBOARD) { + sei(); + printf_P(PSTR("Bad eeprom value\r\n")); + while(1); + } + + /* LOGS */ + error_register_emerg(mylog); + error_register_error(mylog); + error_register_warning(mylog); + error_register_notice(mylog); + error_register_debug(mylog); + + /* SPI + ENCODERS */ + encoders_spi_init(); /* this will also init spi hardware */ + + /* I2C */ + i2c_protocol_init(); + i2c_init(I2C_MODE_SLAVE, I2C_SENSORBOARD_ADDR); + i2c_register_recv_event(i2c_recvevent); + + /* TIMER */ + timer_init(); + timer0_register_OV_intr(main_timer_interrupt); + + /* PWM */ + 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, + 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, + &PORTD, 5); + PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, + &PORTD, 6); + PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED, + &PORTD, 7); + + + /* servos */ + 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, + TIMER1_PRESCALER_DIV_256); + PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL, + NULL, 0); + PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL, + 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, + LED_PRIO); + /* all cs management */ + microb_cs_init(); + + /* sensors, will also init hardware adc */ + sensor_init(); + + /* TIME */ + time_init(TIME_PRIO); + + /* ax12 */ + ax12_user_init(); + + /* beacon */ + beacon_init(); + scheduler_add_periodical_event_priority(beacon_calc, NULL, + 20000L / SCHEDULER_UNIT, + BEACON_PRIO); + + + + /* scan */ + + + scanner_init(); + scheduler_add_periodical_event_priority(do_scan, NULL, + (1024L*1L) / SCHEDULER_UNIT, + CS_PRIO-1); + + sei(); + + scan_params.speed = SCAN_DEFAULT_SPEED; + scan_params.debug = 0; + + scanner_calibre_mirror(); + scanner_calibre_laser(); + + beacon_calibre_pos(); + + + printf_P(PSTR("\r\n")); + printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n")); + cmdline_interact(); + + + + return 0; +} diff --git a/projects/microb2010/sensorboard/main.h b/projects/microb2010/sensorboard/main.h new file mode 100755 index 0000000..3a4ff83 --- /dev/null +++ b/projects/microb2010/sensorboard/main.h @@ -0,0 +1,154 @@ +/* + * 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 + * (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: main.h,v 1.4 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +#define LED_TOGGLE(port, bit) do { \ + if (port & _BV(bit)) \ + port &= ~_BV(bit); \ + else \ + port |= _BV(bit); \ + } while(0) + +#define LED1_ON() sbi(PORTJ, 2) +#define LED1_OFF() cbi(PORTJ, 2) +#define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2) + +#define LED2_ON() sbi(PORTL, 7) +#define LED2_OFF() cbi(PORTL, 7) +#define LED2_TOGGLE() LED_TOGGLE(PORTL, 7) + +#define LED3_ON() sbi(PORTJ, 3) +#define LED3_OFF() cbi(PORTJ, 3) +#define LED3_TOGGLE() LED_TOGGLE(PORTJ, 3) + +#define LED4_ON() sbi(PORTL, 6) +#define LED4_OFF() cbi(PORTL, 6) +#define LED4_TOGGLE() LED_TOGGLE(PORTL, 6) + +#define BRAKE_DDR() do { DDRJ |= 0xF0; } while(0) +#define BRAKE_ON() do { PORTJ |= 0xF0; } while(0) +#define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0) + +#define BEACON_ENCODER ((void *)0) +#define SCANNER_ENCODER ((void *)1) + +#define BEACON_PWM ((void *)&gen.pwm1_4A) +#define SCANNER_PWM ((void *)&gen.pwm2_4B) +#define PICKUP_WHEEL_L_PWM ((void *)&gen.pwm3_1A) +#define PICKUP_WHEEL_R_PWM ((void *)&gen.pwm4_1B) + +#define BEACON_POS_SENSOR 2 +#define SCANNER_POS_SENSOR 7 +#define SCANNER_MAXCOLUMN_SENSOR 1 + + + +#define SCANNER_POS_OUT 179 +#define SCANNER_POS_CALIBRE 370 +#define SCANNER_POS_IN 400 + + +/** ERROR NUMS */ +#define E_USER_I2C_PROTO 195 +#define E_USER_SENSOR 196 +#define E_USER_BEACON 197 +#define E_USER_SCANNER 198 +#define E_USER_IMGPROCESS 199 + +#define LED_PRIO 170 +#define TIME_PRIO 160 +#define ADC_PRIO 120 +#define CS_PRIO 100 +#define BEACON_PRIO 80 +#define I2C_POLL_PRIO 20 + +#define CS_PERIOD 5000L + +#define NB_LOGS 4 + +/* generic to all boards */ +struct genboard { + /* command line interface */ + struct rdline rdl; + char prompt[RDLINE_PROMPT_SIZE]; + + /* motors */ + struct pwm_ng pwm1_4A; + struct pwm_ng pwm2_4B; + struct pwm_ng pwm3_1A; + struct pwm_ng pwm4_1B; + + /* servos */ + struct pwm_ng servo1; + struct pwm_ng servo2; + struct pwm_ng servo3; + struct pwm_ng servo4; + + /* ax12 interface */ + AX12 ax12; + + /* log */ + uint8_t logs[NB_LOGS+1]; + uint8_t log_level; + uint8_t debug; +}; + +struct cs_block { + uint8_t on; + struct cs cs; + struct pid_filter pid; + struct quadramp_filter qr; + struct blocking_detection bd; +}; + +/* sensorboard specific */ +struct sensorboard { +#define DO_ENCODERS 1 +#define DO_CS 2 +#define DO_BD 4 +#define DO_POWER 8 + uint8_t flags; /* misc flags */ + + /* control systems */ + struct cs_block beacon; + struct cs_block scanner; + + /* robot status */ + uint8_t our_color; +}; + +extern struct genboard gen; +extern struct sensorboard sensorboard; + +/* start the bootloader */ +void bootloader(void); + +#define wait_cond_or_timeout(cond, timeout) \ +({ \ + microseconds __us = time_get_us2(); \ + uint8_t __ret = 1; \ + while(! (cond)) { \ + if (time_get_us2() - __us > (timeout)*1000L) {\ + __ret = 0; \ + break; \ + } \ + } \ + __ret; \ +}) diff --git a/projects/microb2010/sensorboard/pid_config.h b/projects/microb2010/sensorboard/pid_config.h new file mode 100755 index 0000000..fa95f08 --- /dev/null +++ b/projects/microb2010/sensorboard/pid_config.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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 + * + * + * + */ + +#ifndef PID_CONFIG_H +#define PID_CONFIG_H + +/** the derivate term can be filtered to remove the noise. This value + * is the maxium sample count to keep in memory to do this + * filtering. For an instance of pid, this count is defined o*/ +#define PID_DERIVATE_FILTER_MAX_SIZE 4 + +#endif diff --git a/projects/microb2010/sensorboard/rdline_config.h b/projects/microb2010/sensorboard/rdline_config.h new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/sensorboard/scanner.c b/projects/microb2010/sensorboard/scanner.c new file mode 100644 index 0000000..dfc869e --- /dev/null +++ b/projects/microb2010/sensorboard/scanner.c @@ -0,0 +1,898 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include "sensor.h" + +#include + +#include "main.h" +#include "scanner.h" + +#include "cmdline.h" + + +#include "scan_h_l.h" + + +#include +#include "img_processing.h" + + +#include "../common/i2c_commands.h" + +#define SCANNER_DEBUG(args...) DEBUG(E_USER_SCANNER, args) +#define SCANNER_NOTICE(args...) NOTICE(E_USER_SCANNER, args) +#define SCANNER_ERROR(args...) ERROR(E_USER_SCANNER, args) + + +#define MODULO_TIMER (1023L) + +#define COEFF_TIMER (2) +#define COEFF_MULT (1000L) +#define COEFF_MULT2 (1000L) + +double TELEMETRE_A = TELEMETRE_A_INIT; +double TELEMETRE_B = TELEMETRE_B_INIT; + + + + +struct scan_params scan_params; + +static volatile int32_t scan_delta_pos; +static volatile int32_t scan_tick_cur = 0; +static volatile int32_t scan_tick_prev = 0; +/*static volatile int32_t count = 0; +static volatile int32_t count_diff_rising = 0; +static volatile int32_t count_diff_falling = 0; +*/ +static int32_t scanner_coeff = 0; + +//static volatile int8_t valid_scanner = 0; + +static volatile int32_t scan_pos_cur = 0; + +static int32_t pos_ref = 0; + + +int32_t encoders_spi_get_value_scanner(void *number) +{ + int32_t ret; + + ret = encoders_spi_get_value(number); + return ret*4; +} + + +void encoders_spi_set_value_scanner(void * number, int32_t v) +{ + encoders_spi_set_value(number, v/4); +} + +int32_t encoders_spi_update_scanner(void * number) +{ + int32_t ret; + uint8_t flags; + + IRQ_LOCK(flags); + ret = encoders_spi_get_value_scanner(number); + scan_delta_pos = ret - scan_pos_cur; + scan_pos_cur = ret; + scan_tick_prev = scan_tick_cur; + scan_tick_cur = TCNT3; + + scanner_coeff = (scan_delta_pos * COEFF_MULT) / + ((scan_tick_cur - scan_tick_prev + MODULO_TIMER + 1) & MODULO_TIMER); + + IRQ_UNLOCK(flags); + + + return ret; +} + +int32_t encoders_spi_get_value_scanner_interpolation(void * number) +{ + uint8_t flags; + int32_t pos; + + IRQ_LOCK(flags); + pos = scan_pos_cur; + pos += (scanner_coeff * ((TCNT3 - scan_tick_cur + MODULO_TIMER + 1)& MODULO_TIMER ))/ + COEFF_MULT; + + IRQ_UNLOCK(flags); + + return pos; +} + + +void scanner_reset_pos(void) +{ + pwm_ng_set(SCANNER_PWM, 0); + encoders_spi_set_value_scanner(SCANNER_ENCODER, 0); +} + +void scanner_init(void) +{ + + scan_params.working = 0; + scan_params.must_stop = 0; + + scanner_reset_pos(); + pos_ref = encoders_spi_get_value_scanner(SCANNER_ENCODER); + + //memset(&scanner, 0, sizeof(struct scanner)); + + scan_delta_pos = 0; + + /*for(i=0;i 0){ + time_wait_ms(10); + } + + /* arm in */ + pwm_ng_set(&gen.servo3, SCANNER_POS_IN); + +} + + +/* + * called from IRQ: + * mode can be off/prepare/start, see in i2c_commands.h + */ +void scanner_set_mode(uint8_t mode) +{ + if (mode == I2C_SENSORBOARD_SCANNER_PREPARE){ + /* reset flag max pos */ + scan_params.max_column_detected = 0; + + /* arm out */ + pwm_ng_set(&gen.servo3, SCANNER_POS_OUT); + + } + else if (mode == I2C_SENSORBOARD_SCANNER_STOP){ + /* arm in */ + pwm_ng_set(&gen.servo3, SCANNER_POS_IN); + scan_params.must_stop = 1; + } + else if (mode == I2C_SENSORBOARD_SCANNER_START){ + scan_params.max_column_detected = 0; + scan_params.must_stop = 0; + + + /* start scan in background */ + scanner_do_scan(); + } + + +} + +/* +void scanner_stop(void) +{ + sensorboard.scanner.on = 0; + pwm_ng_set(SCANNER_PWM, 0); +} +*/ + + +/* +int32_t encoders_spi_get_scanner_speed(void * dummy) +{ + return scanner_speed; +} +*/ + + +//uint8_t sample_tab[MAX_SAMPLE]; +//uint16_t sample_i = 0; + + +//#define offset_a (75.*M_PI/180.) +//float offset_a; +//float offset_b; + +//int32_t pos_start_scan; + + +/* get motor angle in radian; return mirror angle in radian, cos a sin a */ +void ang2_a_mirror(float b, float * c_a, float* s_a, float* a) +{ + float x2, y2; + float A, DELTA, B, D; + + b+=scan_params.offset_b; + x2 = X + l1*cos(b); + y2 = Y + l1*sin(b); + + A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3); + + DELTA = -(A*A - x2*x2 - y2*y2); + B = sqrt(DELTA); + + D = x2*x2 + y2*y2; + + *c_a = (x2*A + y2*B)/D; + *s_a = -(x2*B - y2*A)/D; + + *a = atan2(*s_a, *c_a); + + *a += scan_params.offset_a; + // *s_a = sin(*a); + // *c_a = cos(*a); + +} + +/* get telemeter dist , cos a, sin a, a and return H, L of scanned point */ +void ang2_H_L(float l_telemetre, float c_a, float s_a, float a, float *H, float *L) +{ + float d; + d = h_mirror*c_a/s_a; + *H = (l_telemetre - l_mirror - d)*sin(2*a); + *L = l_mirror + d + *H/tan(2*a); + + //*H+= 8*sin(a-scan_params.offset_a); +} + + + +//int32_t last_col_n; +//int32_t last_row_n; +//uint8_t last_sample; + +//uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123}; +//uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123}; + +/* last high is dummy, to deal higher columns */ +uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250}; +#define H_MARGIN 7 + + +#if 0 +void do_scan(void * dummy) +{ + + unsigned int i; + int16_t a; + int32_t row_n; + int32_t col_n; + + + int32_t tour_pos; + int32_t pos, last_pos; + int32_t pos_tmp ; + int32_t mot_pos; + float dist; + uint8_t min_sample; + + float b, c_a, s_a, H, L, m_a; + int32_t H_fin; + + + if (scan_params.sample_i==0) + return; + + mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); + + if (scan_params.sample_i==1){ + SCANNER_DEBUG("dump end enc %ld %d ", mot_pos, PIX_PER_SCAN); + //scanner.flags &= (~CS_ON); + + + + /* stop scan at cur pos + 10 round */ + mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC); + mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ; + + SCANNER_DEBUG("set to %ld ", mot_pos); + + cs_set_consign(&sensorboard.scanner.cs, mot_pos); + //pwm_ng_set(SCANNER_MOT_PWM, 0); + + + } + + mot_pos-= scan_params.pos_start_scan; + + a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 ); + + + //dist = (a-TELEMETRE_B)/TELEMETRE_A; + dist = TELEMETRE_A * a +TELEMETRE_B; + + //SCANNER_DEBUG("enc val = %ld", encoders_microb_get_value((void *)SCANNER_ENC)); + + + //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a; + //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0; + + + row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); +#if 0 + /* separe scan forward/backword */ + if (row_n%2){ + row_n/=2; + } + else{ + row_n/=2; + row_n+=30; + } +#endif + + tour_pos = (mot_pos)%(SCANNER_STEP_TOUR); + + b = (2.*M_PI)*(float)tour_pos/(float)(SCANNER_STEP_TOUR); + + ang2_a_mirror(b, &c_a, &s_a, &m_a); + ang2_H_L(dist, c_a, s_a, m_a, &H, &L); + + + SCANNER_DEBUG("%ld %d", tour_pos, a); + + if (H >0){ + printf("zarb H\n"); + H = 0; + } + + if (dist> SCAN_MAX_DIST){ + H = 0; + L = 0; + } + + H = H;//m_a*180/M_PI; + L = L;//(L-100)*PIX_PER_SCAN; + + //SCANNER_DEBUG("polling : ADC0 = %i %f",a, dist); + //SCANNER_DEBUG("%f %f %2.2f %f", H, L, m_a*180./M_PI, dist); + + + //SCANNER_DEBUG("%f %f", dist, m_a*180/M_PI); + + H=(H+SCAN_H_MAX)*SCAN_H_COEF; + L-=SCAN_L_MIN; + + + /* XXX may never append */ + if (L<0) + L=0; + + + /* first filter => pixel modulo level */ +#define H_BASE 10 +#define H_MUL 14 + H_fin = H;//+SCAN_H_MAX; + //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE; + + if (scan_params.filter){ + H_fin = 11; // default is level 11 + for (i=0;iPIX_PER_SCAN) + printf("BUG!!! RECALC MAX L"); + + //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; + + //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; + //pos= row_n*PIX_PER_SCAN+tour_pos; + //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; + + + + pos= row_n*PIX_PER_SCAN+col_n; + last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; + + //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin); + + //a-=0x100; + a-=200; + //a/=10; + + if (0<= pos && pos 0xff?0xFF:a; + //sample_tab[(int)L] = H ; + scan_params.sample_tab[pos] = H_fin; + nop(); + if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ + /* we have a hole, pad it with minimal hight */ + if (H_fin>scan_params.last_sample) + min_sample = scan_params.last_sample; + else + min_sample = H_fin; + + //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n); + + /* fill grow, avoid erasing curent pos */ + if (pos > last_pos){ + pos_tmp = last_pos; + last_pos = pos; + //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); + + } + else{ + pos_tmp = pos+1; + //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); + } + + + for (;pos_tmp< last_pos;pos_tmp++){ + if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; + + //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; + + /* + if (((pos =DIM_DIST) + j = DIM_DIST-1; + + if (i>=DIM_ANGLE) + i = DIM_ANGLE-1; + + + val.u16 = pgm_read_word_near(&array_h_l[j][i]); + + + //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); + //val.u16 = pgm_read_word_near(&array_h_l[a][tp]); + H = val.h_l.h; + L = val.h_l.l; + /* + val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]); + + H = val.h_l.h; + L = val.h_l.l; + */ + H_fin = H; + L_fin = L; + + + if (L<=0) + L = 0; + + col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN); + if (col_n>=PIX_PER_SCAN) { + //printf("BUG!!! RECALC MAX L"); + col_n = PIX_PER_SCAN-1; + } + + //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN; + + //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS; + //pos= row_n*PIX_PER_SCAN+tour_pos; + //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos; + + row_n = (mot_pos)/(SCANNER_STEP_TOUR/2); + + + pos= row_n*PIX_PER_SCAN+col_n; + last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n; + + //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin); + + //a-=0x100; + a-=200; + //a/=10; + + if (0<= pos && pos 0xff?0xFF:a; + //sample_tab[(int)L] = H ; + scan_params.sample_tab[pos] = H_fin; + nop(); + if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){ + /* we have a hole, pad it with minimal hight */ + if (H_fin>scan_params.last_sample) + min_sample = scan_params.last_sample; + else + min_sample = H_fin; + + //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n); + + /* fill grow, avoid erasing curent pos */ + if (pos > last_pos){ + pos_tmp = last_pos; + last_pos = pos; + //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); + + } + else{ + pos_tmp = pos+1; + //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp); + } + + + for (;pos_tmp< last_pos;pos_tmp++){ + if (0< pos_tmp && pos_tmp 0x1ff?0x1FF:a; + + //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2; + + /* + if (((pos + * + * 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: sensor.c,v 1.3 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "sensor.h" + +/************ ADC */ + +struct adc_infos { + uint16_t config; + int16_t value; + int16_t prev_val; + int16_t (*filter)(struct adc_infos *, int16_t); +}; + +/* reach 90% of the value in 4 samples */ +int16_t rii_light(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + (int32_t)adc->prev_val / 2; + return adc->prev_val / 2; +} + +/* reach 90% of the value in 8 samples */ +int16_t rii_medium(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4; + return adc->prev_val / 4; +} + +/* reach 90% of the value in 16 samples */ +int16_t rii_strong(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8; + return adc->prev_val / 8; +} + + +#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x ) + +/* 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 }, + + /* 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, see init below */ +static void do_adc(void *dummy) +{ + /* launch first conversion */ + adc_launch(adc_infos[0].config); +} + +static void adc_event(int16_t result) +{ + static uint8_t i = 0; + + /* filter value if needed */ + if (adc_infos[i].filter) + adc_infos[i].value = adc_infos[i].filter(&adc_infos[i], + result); + else + adc_infos[i].value = result; + + i ++; + if (i >= ADC_MAX) + i = 0; + else + adc_launch(adc_infos[i].config); +} + +int16_t sensor_get_adc(uint8_t i) +{ + int16_t tmp; + uint8_t flags; + + IRQ_LOCK(flags); + tmp = adc_infos[i].value; + 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; + uint8_t invert; +}; + +/* pullup mapping: + * CAP 5,6,7,8 + * voltage div mapping: + * CAP 1 + */ +static struct sensor_filter sensor_filter[SENSOR_MAX] = { + [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */ + [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */ + [S_CAP3] = { 1, 0, 0, 1, 0, 0 }, /* 2 */ + [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ + [S_CAP5] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ + [S_CAP6] = { 1, 0, 0, 1, 0, 0 }, /* 5 */ + [S_CAP7] = { 1, 0, 0, 1, 0, 0 }, /* 6 */ + [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ + [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ + [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ + [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */ + [S_RESERVED4] = { 1, 0, 0, 1, 0, 0 }, /* 11 */ + [S_RESERVED5] = { 1, 0, 0, 1, 0, 0 }, /* 12 */ + [S_RESERVED6] = { 1, 0, 0, 1, 0, 0 }, /* 13 */ + [S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */ + [S_RESERVED8] = { 1, 0, 0, 1, 0, 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_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 */ +#define BACKGROUND_ADC 0 + +/* called every 10 ms, see init below */ +static void do_sensors(void *dummy) +{ + if (BACKGROUND_ADC) + do_adc(NULL); + do_boolean_sensors(NULL); +} + +void sensor_init(void) +{ + adc_init(); + if (BACKGROUND_ADC) + adc_register_event(adc_event); + /* CS EVENT */ + scheduler_add_periodical_event_priority(do_sensors, NULL, + 10000L / SCHEDULER_UNIT, + ADC_PRIO); + +} + diff --git a/projects/microb2010/sensorboard/sensor.h b/projects/microb2010/sensorboard/sensor.h new file mode 100644 index 0000000..c2b18a7 --- /dev/null +++ b/projects/microb2010/sensorboard/sensor.h @@ -0,0 +1,56 @@ +/* + * Copyright Droids Corporation (2009) + * 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 + * (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: sensor.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + */ + +/* synchronize with sensor.c */ +#define ADC_CSENSE1 0 +#define ADC_CSENSE2 1 +#define ADC_CSENSE3 2 +#define ADC_CSENSE4 3 +#define ADC_MAX 4 + +/* synchronize with sensor.c */ +#define S_CAP1 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); + +/* 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); diff --git a/projects/microb2010/sensorboard/spi_config.h b/projects/microb2010/sensorboard/spi_config.h new file mode 100644 index 0000000..76697c3 --- /dev/null +++ b/projects/microb2010/sensorboard/spi_config.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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 + * + */ + +/* + * Author : Julien LE GUEN - jlg@jleguen.info + */ + + +/* + * Configure HERE your SPI module + */ + + + +/* Number of slave devices in your system + * Each slave have a dedicated SS line that you have to register + * before using the SPI module + */ +#define SPI_MAX_SLAVES 1 + diff --git a/projects/microb2010/sensorboard/time_config.h b/projects/microb2010/sensorboard/time_config.h new file mode 100755 index 0000000..e0f7f2a --- /dev/null +++ b/projects/microb2010/sensorboard/time_config.h @@ -0,0 +1,23 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: time_config.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + */ + +/** precision of the time processor, in us */ +#define TIME_PRECISION 25000l diff --git a/projects/microb2010/sensorboard/timer_config.h b/projects/microb2010/sensorboard/timer_config.h new file mode 100755 index 0000000..810525c --- /dev/null +++ b/projects/microb2010/sensorboard/timer_config.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2006) + * + * 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: timer_config.h,v 1.1 2009-03-29 18:44:54 zer0 Exp $ + * + */ + +#define TIMER0_ENABLED + +/* #define TIMER1_ENABLED */ +/* #define TIMER1A_ENABLED */ +/* #define TIMER1B_ENABLED */ +/* #define TIMER1C_ENABLED */ + +/* #define TIMER2_ENABLED */ + +/* #define TIMER3_ENABLED */ +/* #define TIMER3A_ENABLED */ +/* #define TIMER3B_ENABLED */ +/* #define TIMER3C_ENABLED */ + +#define TIMER0_PRESCALER_DIV 8 diff --git a/projects/microb2010/sensorboard/uart_config.h b/projects/microb2010/sensorboard/uart_config.h new file mode 100644 index 0000000..2a27e07 --- /dev/null +++ b/projects/microb2010/sensorboard/uart_config.h @@ -0,0 +1,102 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: uart_config.h,v 1.3 2009-05-27 20:04:07 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* + * UART1 definitions + */ + +/* compile uart1 fonctions, undefine it to pass compilation */ +#define UART1_COMPILE + +/* enable uart1 if == 1, disable if == 0 */ +#define UART1_ENABLED 1 + +/* enable uart1 interrupts if == 1, disable if == 0 */ +#define UART1_INTERRUPT_ENABLED 1 + +#define UART1_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART1_USE_DOUBLE_SPEED 1 + +#define UART1_RX_FIFO_SIZE 64 +#define UART1_TX_FIFO_SIZE 64 +#define UART1_NBITS 8 + +#define UART1_PARITY UART_PARTITY_NONE + +#define UART1_STOP_BIT UART_STOP_BITS_1 + + +/* + * UART3 definitions + */ + +/* compile uart3 fonctions, undefine it to pass compilation */ +#define UART3_COMPILE + +/* enable uart3 if == 1, disable if == 0 */ +#define UART3_ENABLED 1 + +/* enable uart3 interrupts if == 1, disable if == 0 */ +#define UART3_INTERRUPT_ENABLED 1 + +#define UART3_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART3_USE_DOUBLE_SPEED 1 +//#define UART3_USE_DOUBLE_SPEED 1 + +#define UART3_RX_FIFO_SIZE 64 +#define UART3_TX_FIFO_SIZE 64 +//#define UART3_NBITS 5 +//#define UART3_NBITS 6 +//#define UART3_NBITS 7 +#define UART3_NBITS 8 +//#define UART3_NBITS 9 + +#define UART3_PARITY UART_PARTITY_NONE +//#define UART3_PARITY UART_PARTITY_ODD +//#define UART3_PARITY UART_PARTITY_EVEN + +#define UART3_STOP_BIT UART_STOP_BITS_1 +//#define UART3_STOP_BIT UART_STOP_BITS_2 + + + + +/* .... same for uart 1, 2, 3 ... */ + +#endif + diff --git a/projects/microb2010/tests/static_beacon/.config b/projects/microb2010/tests/static_beacon/.config new file mode 100644 index 0000000..fc8861c --- /dev/null +++ b/projects/microb2010/tests/static_beacon/.config @@ -0,0 +1,251 @@ +# +# Automatically generated by make menuconfig: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +CONFIG_MCU_ATMEGA8=y +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +# CONFIG_MCU_ATMEGA128 is not set +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +# CONFIG_MCU_ATMEGA2560 is not set +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +# CONFIG_MATH_LIB is not set +# CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set +# CONFIG_MINIMAL_PRINTF is not set +CONFIG_STANDARD_PRINTF=y +# CONFIG_FORMAT_IHEX is not set +# CONFIG_FORMAT_SREC is not set +CONFIG_FORMAT_BINARY=y + +# +# Base modules +# +# CONFIG_MODULE_CIRBUF is not set +# CONFIG_MODULE_CIRBUF_LARGE is not set +# CONFIG_MODULE_FIXED_POINT is not set +# CONFIG_MODULE_VECT2 is not set +# CONFIG_MODULE_GEOMETRY is not set +# CONFIG_MODULE_SCHEDULER is not set +# CONFIG_MODULE_SCHEDULER_STATS is not set +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +CONFIG_MODULE_SCHEDULER_TIMER0=y +# CONFIG_MODULE_SCHEDULER_MANUAL is not set +# CONFIG_MODULE_TIME is not set +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# +# CONFIG_MODULE_UART is not set +# CONFIG_MODULE_UART_9BITS is not set +# CONFIG_MODULE_UART_CREATE_CONFIG is not set +# CONFIG_MODULE_SPI is not set +# CONFIG_MODULE_SPI_CREATE_CONFIG is not set +# CONFIG_MODULE_I2C is not set +# CONFIG_MODULE_I2C_MASTER is not set +# CONFIG_MODULE_I2C_MULTIMASTER is not set +# CONFIG_MODULE_I2C_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +# CONFIG_MODULE_TIMER is not set +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +# CONFIG_MODULE_PWM_NG is not set +# CONFIG_MODULE_ADC is not set +# CONFIG_MODULE_ADC_CREATE_CONFIG is not set + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +# CONFIG_MODULE_VT100 is not set +# CONFIG_MODULE_RDLINE is not set +# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set +# CONFIG_MODULE_RDLINE_KILL_BUF is not set +# CONFIG_MODULE_RDLINE_HISTORY is not set +# CONFIG_MODULE_PARSE is not set +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +# CONFIG_MODULE_AX12 is not set +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +# CONFIG_MODULE_ENCODERS_MICROB is not set +# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_SPI is not set +# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set + +# +# Robot specific modules +# +# CONFIG_MODULE_ROBOT_SYSTEM is not set +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +# CONFIG_MODULE_POSITION_MANAGER is not set +# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set +# CONFIG_MODULE_TRAJECTORY_MANAGER is not set +# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set + +# +# Control system modules +# +# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set +# CONFIG_MODULE_PID is not set +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +# CONFIG_MODULE_QUADRAMP is not set +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# +# CONFIG_MODULE_DIAGNOSTIC is not set +# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set +# CONFIG_MODULE_ERROR is not set +# CONFIG_MODULE_ERROR_CREATE_CONFIG is not set + +# +# Programmer options +# +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/tests/static_beacon/.config.old b/projects/microb2010/tests/static_beacon/.config.old new file mode 100644 index 0000000..dd4bd20 --- /dev/null +++ b/projects/microb2010/tests/static_beacon/.config.old @@ -0,0 +1,251 @@ +# +# Automatically generated by make menuconfig: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +CONFIG_MCU_ATMEGA8=y +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +# CONFIG_MCU_ATMEGA128 is not set +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +# CONFIG_MCU_ATMEGA2560 is not set +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +# CONFIG_MATH_LIB is not set +# CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set +# CONFIG_MINIMAL_PRINTF is not set +CONFIG_STANDARD_PRINTF=y +# CONFIG_FORMAT_IHEX is not set +# CONFIG_FORMAT_SREC is not set +CONFIG_FORMAT_BINARY=y + +# +# Base modules +# +# CONFIG_MODULE_CIRBUF is not set +# CONFIG_MODULE_CIRBUF_LARGE is not set +# CONFIG_MODULE_FIXED_POINT is not set +# CONFIG_MODULE_VECT2 is not set +# CONFIG_MODULE_GEOMETRY is not set +# CONFIG_MODULE_SCHEDULER is not set +# CONFIG_MODULE_SCHEDULER_STATS is not set +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +CONFIG_MODULE_SCHEDULER_TIMER0=y +# CONFIG_MODULE_SCHEDULER_MANUAL is not set +# CONFIG_MODULE_TIME is not set +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# +# CONFIG_MODULE_UART is not set +# CONFIG_MODULE_UART_9BITS is not set +# CONFIG_MODULE_UART_CREATE_CONFIG is not set +# CONFIG_MODULE_SPI is not set +# CONFIG_MODULE_SPI_CREATE_CONFIG is not set +# CONFIG_MODULE_I2C is not set +# CONFIG_MODULE_I2C_MASTER is not set +# CONFIG_MODULE_I2C_MULTIMASTER is not set +# CONFIG_MODULE_I2C_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +# CONFIG_MODULE_TIMER is not set +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +# CONFIG_MODULE_PWM_NG is not set +# CONFIG_MODULE_ADC is not set +# CONFIG_MODULE_ADC_CREATE_CONFIG is not set + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +# CONFIG_MODULE_VT100 is not set +# CONFIG_MODULE_RDLINE is not set +# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set +# CONFIG_MODULE_RDLINE_KILL_BUF is not set +# CONFIG_MODULE_RDLINE_HISTORY is not set +# CONFIG_MODULE_PARSE is not set +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +# CONFIG_MODULE_AX12 is not set +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +# CONFIG_MODULE_ENCODERS_MICROB is not set +# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_SPI is not set +# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set + +# +# Robot specific modules +# +# CONFIG_MODULE_ROBOT_SYSTEM is not set +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +# CONFIG_MODULE_POSITION_MANAGER is not set +# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set +# CONFIG_MODULE_TRAJECTORY_MANAGER is not set +# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set + +# +# Control system modules +# +# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set +# CONFIG_MODULE_PID is not set +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +# CONFIG_MODULE_QUADRAMP is not set +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# +# CONFIG_MODULE_DIAGNOSTIC is not set +# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set +# CONFIG_MODULE_ERROR is not set +# CONFIG_MODULE_ERROR_CREATE_CONFIG is not set + +# +# Programmer options +# +# CONFIG_AVRDUDE is not set +CONFIG_AVARICE=y + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/tests/static_beacon/Makefile b/projects/microb2010/tests/static_beacon/Makefile new file mode 100755 index 0000000..ddbfbda --- /dev/null +++ b/projects/microb2010/tests/static_beacon/Makefile @@ -0,0 +1,24 @@ +TARGET = static_beacon + +AVERSIVE_DIR = ../../../.. + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + +AVRDUDE_DELAY=30 + +CFLAGS += -W + +######################################## + +-include .aversive_conf +include $(AVERSIVE_DIR)/mk/aversive_project.mk diff --git a/projects/microb2010/tests/static_beacon/coin.c b/projects/microb2010/tests/static_beacon/coin.c new file mode 100644 index 0000000..88a80a3 --- /dev/null +++ b/projects/microb2010/tests/static_beacon/coin.c @@ -0,0 +1,472 @@ +# 1 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" +# 1 "/home/zer0/zavr/projects/microb2010/tests/static_beacon//" +# 1 "" +# 1 "" +# 1 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" +# 23 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" +# 1 "/home/zer0/zavr/include/aversive.h" 1 +# 31 "/home/zer0/zavr/include/aversive.h" +# 1 "./autoconf.h" 1 +# 32 "/home/zer0/zavr/include/aversive.h" 2 + + +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/interrupt.h" 1 3 +# 38 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/interrupt.h" 3 +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 1 3 +# 94 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 3 +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/sfr_defs.h" 1 3 +# 126 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/sfr_defs.h" 3 +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/inttypes.h" 1 3 +# 37 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/inttypes.h" 3 +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 1 3 +# 121 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3 +typedef int int8_t __attribute__((__mode__(__QI__))); +typedef unsigned int uint8_t __attribute__((__mode__(__QI__))); +typedef int int16_t __attribute__ ((__mode__ (__HI__))); +typedef unsigned int uint16_t __attribute__ ((__mode__ (__HI__))); +typedef int int32_t __attribute__ ((__mode__ (__SI__))); +typedef unsigned int uint32_t __attribute__ ((__mode__ (__SI__))); + +typedef int int64_t __attribute__((__mode__(__DI__))); +typedef unsigned int uint64_t __attribute__((__mode__(__DI__))); +# 142 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3 +typedef int16_t intptr_t; + + + + +typedef uint16_t uintptr_t; +# 159 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3 +typedef int8_t int_least8_t; + + + + +typedef uint8_t uint_least8_t; + + + + +typedef int16_t int_least16_t; + + + + +typedef uint16_t uint_least16_t; + + + + +typedef int32_t int_least32_t; + + + + +typedef uint32_t uint_least32_t; + + + + + + + +typedef int64_t int_least64_t; + + + + + + +typedef uint64_t uint_least64_t; +# 213 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3 +typedef int8_t int_fast8_t; + + + + +typedef uint8_t uint_fast8_t; + + + + +typedef int16_t int_fast16_t; + + + + +typedef uint16_t uint_fast16_t; + + + + +typedef int32_t int_fast32_t; + + + + +typedef uint32_t uint_fast32_t; + + + + + + + +typedef int64_t int_fast64_t; + + + + + + +typedef uint64_t uint_fast64_t; +# 273 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/stdint.h" 3 +typedef int64_t intmax_t; + + + + +typedef uint64_t uintmax_t; +# 38 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/inttypes.h" 2 3 +# 77 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/inttypes.h" 3 +typedef int32_t int_farptr_t; + + + +typedef uint32_t uint_farptr_t; +# 127 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/sfr_defs.h" 2 3 +# 95 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3 +# 229 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 3 +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/iom8.h" 1 3 +# 230 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3 +# 317 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 3 +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/portpins.h" 1 3 +# 318 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3 + +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/common.h" 1 3 +# 320 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3 + +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/version.h" 1 3 +# 322 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3 + + +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/fuse.h" 1 3 +# 223 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/fuse.h" 3 +typedef struct +{ + unsigned char low; + unsigned char high; +} __fuse_t; +# 325 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3 + + +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/lock.h" 1 3 +# 328 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/io.h" 2 3 +# 39 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/avr/interrupt.h" 2 3 +# 35 "/home/zer0/zavr/include/aversive.h" 2 + + + +# 1 "/home/zer0/zavr/include/aversive/types.h" 1 +# 47 "/home/zer0/zavr/include/aversive/types.h" +typedef uint8_t u08; +typedef uint16_t u16; +typedef uint32_t u32; +typedef uint64_t u64; +typedef int8_t s08; +typedef int16_t s16; +typedef int32_t s32; +typedef int64_t s64; +# 39 "/home/zer0/zavr/include/aversive.h" 2 +# 1 "/home/zer0/zavr/include/aversive/errno.h" 1 +# 33 "/home/zer0/zavr/include/aversive/errno.h" +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/errno.h" 1 3 +# 55 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/errno.h" 3 +extern int errno; +# 34 "/home/zer0/zavr/include/aversive/errno.h" 2 +# 40 "/home/zer0/zavr/include/aversive.h" 2 +# 1 "/home/zer0/zavr/include/aversive/irq_lock.h" 1 +# 41 "/home/zer0/zavr/include/aversive.h" 2 +# 125 "/home/zer0/zavr/include/aversive.h" +struct extract32 { + union { + struct { + + uint8_t u8_0; + uint8_t u8_1; + uint8_t u8_2; + uint8_t u8_3; + + + + + + + } __attribute__ ((packed)) u8; + struct { + + uint16_t u16_0; + uint16_t u16_1; + + + + + } __attribute__ ((packed)) u16; + struct { + + uint8_t u8_0; + uint16_t u16_mid; + uint8_t u8_3; + + + + + + } __attribute__ ((packed)) u16_b; + uint32_t u32; + } __attribute__ ((packed)) u; +} __attribute__ ((packed)); +# 174 "/home/zer0/zavr/include/aversive.h" +struct extract16 { + union { + struct { + + uint8_t u8_0; + uint8_t u8_1; + + + + + } __attribute__ ((packed)) u8; + uint16_t u16; + } __attribute__ ((packed)) u; +} __attribute__ ((packed)); +# 24 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" 2 +# 1 "/home/zer0/zavr/include/aversive/wait.h" 1 +# 45 "/home/zer0/zavr/include/aversive/wait.h" +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 1 3 +# 39 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 3 +# 1 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay_basic.h" 1 3 +# 65 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay_basic.h" 3 +static inline void _delay_loop_1(uint8_t __count) __attribute__((always_inline)); +static inline void _delay_loop_2(uint16_t __count) __attribute__((always_inline)); +# 80 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay_basic.h" 3 +void +_delay_loop_1(uint8_t __count) +{ + __asm__ volatile ( + "1: dec %0" "\n\t" + "brne 1b" + : "=r" (__count) + : "0" (__count) + ); +} +# 102 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay_basic.h" 3 +void +_delay_loop_2(uint16_t __count) +{ + __asm__ volatile ( + "1: sbiw %0,1" "\n\t" + "brne 1b" + : "=w" (__count) + : "0" (__count) + ); +} +# 40 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 2 3 +# 79 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 3 +static inline void _delay_us(double __us) __attribute__((always_inline)); +static inline void _delay_ms(double __ms) __attribute__((always_inline)); +# 107 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 3 +void +_delay_us(double __us) +{ + uint8_t __ticks; + double __tmp = ((((unsigned long)(16000000))) / 3e6) * __us; + if (__tmp < 1.0) + __ticks = 1; + else if (__tmp > 255) + { + _delay_ms(__us / 1000.0); + return; + } + else + __ticks = (uint8_t)__tmp; + _delay_loop_1(__ticks); +} +# 141 "/usr/lib/gcc/avr/4.3.2/../../../avr/include/util/delay.h" 3 +void +_delay_ms(double __ms) +{ + uint16_t __ticks; + double __tmp = ((((unsigned long)(16000000))) / 4e3) * __ms; + if (__tmp < 1.0) + __ticks = 1; + else if (__tmp > 65535) + { + + __ticks = (uint16_t) (__ms * 10.0); + while(__ticks) + { + + _delay_loop_2(((((unsigned long)(16000000))) / 4e3) / 10); + __ticks --; + } + return; + } + else + __ticks = (uint16_t)__tmp; + _delay_loop_2(__ticks); +} +# 46 "/home/zer0/zavr/include/aversive/wait.h" 2 +# 59 "/home/zer0/zavr/include/aversive/wait.h" +static inline void wait_ms(uint16_t n) +{ + while ( n -- ) + _delay_loop_2(((unsigned long)(16000000))/4000); +} +# 25 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" 2 +# 100 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" +static inline void xmit_0(void) +{ + uint8_t t = (((17 + 17) * 10) / 3); + (*(volatile uint8_t *)((0x2E) + 0x20)) = 0; + (*(volatile uint8_t *)((0x2F) + 0x20)) = 0; + _delay_loop_1(t); +} + +static inline void xmit_1(void) +{ + uint8_t t = (((17 + 17) * 10) / 3); + (*(volatile uint8_t *)((0x2E) + 0x20)) = (1 << (4)) | (1 << (3)); + (*(volatile uint16_t *)((0x2C) + 0x20)) = (17 + 17)-1; + (*(volatile uint8_t *)((0x2F) + 0x20)) = (1 << (7)) | (1 << (1)); + (*(volatile uint8_t *)((0x2E) + 0x20)) = (1 << (4)) | (1 << (3)) | (1 << (0)); + _delay_loop_1(t); +} + + + +static inline void xmit_manchester_0(void) +{ + xmit_0(); + xmit_1(); +} + +static inline void xmit_manchester_1(void) +{ + xmit_1(); + xmit_0(); +} + + + +static void xmit_bits(uint8_t *buf, uint8_t nbit) +{ + uint8_t i; + uint8_t byte = *buf; + + for (i=0; i>= 1; + + + if (((i & 0x07) == 0) && (i != 0)) + byte = *(++buf); + } + xmit_0(); +} + + +static inline int8_t wait_laser(void) +{ + uint8_t photos; + uint8_t time; + + + + while (((*(volatile uint8_t *)((0x13) + 0x20)) & ((1 << (0)) | (1 << (1)))) != ((1 << (0)) | (1 << (1)))); + + + while ((photos = ((*(volatile uint8_t *)((0x13) + 0x20)) & ((1 << (0)) | (1 << (1))))) == ((1 << (0)) | (1 << (1)))); + + if (photos != ((1 << (1)))) + return -1; + time = (*(volatile uint8_t *)((0x32) + 0x20)); + ( (*(volatile uint8_t *)((0x12) + 0x20)) |= (1 << (5))); + + + while ((photos = ((*(volatile uint8_t *)((0x13) + 0x20)) & ((1 << (0)) | (1 << (1))))) == ((1 << (1)))) { + if ((*(volatile uint8_t *)((0x32) + 0x20)) - time > ((uint8_t)25)) + return -1; + } + if (photos != ((1 << (0)) | (1 << (1)))) + return -1; + + + + while ((photos = ((*(volatile uint8_t *)((0x13) + 0x20)) & ((1 << (0)) | (1 << (1))))) == ((1 << (0)) | (1 << (1)))) { + if ((*(volatile uint8_t *)((0x32) + 0x20)) - time > ((uint8_t)250)) + return -1; + } + ( (*(volatile uint8_t *)((0x12) + 0x20)) |= (1 << (6))); + if ((*(volatile uint8_t *)((0x32) + 0x20)) - time < ((uint8_t)0)) + return -1; + if (photos != ((1 << (0)))) + return -1; +# 206 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" + return 0; +} + + + +int main(void) +{ + + uint8_t frame = 0x0B; + int8_t ret; + + + (*(volatile uint8_t *)((0x11) + 0x20)) = (1 << (5)) | (1 << (6)) | (1 << (7)); + (*(volatile uint8_t *)((0x17) + 0x20)) |= (1 << (1)); +# 240 "/home/zer0/zavr/projects/microb2010/tests/static_beacon/static_beacon.c" + (*(volatile uint16_t *)((0x26) + 0x20)) = (17 + 17); + (*(volatile uint16_t *)((0x2A) + 0x20)) = 17; + (*(volatile uint8_t *)((0x2F) + 0x20)) = (1 << (7)) | (1 << (1)); + (*(volatile uint8_t *)((0x2E) + 0x20)) = (1 << (4)) | (1 << (3)); + + + (*(volatile uint8_t *)((0x33) + 0x20)) = (1 << (1)) | (1 << (0)); + + while (1) { + + + ret = wait_laser(); + + ( (*(volatile uint8_t *)((0x12) + 0x20)) &= ~ (1 << (5))); + ( (*(volatile uint8_t *)((0x12) + 0x20)) &= ~ (1 << (6))); + + if (ret) + continue; + + + + ( (*(volatile uint8_t *)((0x12) + 0x20)) |= (1 << (7))); + + xmit_bits(&frame, 4); + + + wait_ms(10); + ( (*(volatile uint8_t *)((0x12) + 0x20)) &= ~ (1 << (7))); + + + + + + } + return 0; +} diff --git a/projects/microb2010/tests/static_beacon/hfuse b/projects/microb2010/tests/static_beacon/hfuse new file mode 100644 index 0000000..06e504c --- /dev/null +++ b/projects/microb2010/tests/static_beacon/hfuse @@ -0,0 +1 @@ +0xD9 diff --git a/projects/microb2010/tests/static_beacon/hfuse_new b/projects/microb2010/tests/static_beacon/hfuse_new new file mode 100644 index 0000000..174d84c --- /dev/null +++ b/projects/microb2010/tests/static_beacon/hfuse_new @@ -0,0 +1,2 @@ +:01000000C936 +:00000001FF diff --git a/projects/microb2010/tests/static_beacon/lfuse b/projects/microb2010/tests/static_beacon/lfuse new file mode 100644 index 0000000..8541fd9 --- /dev/null +++ b/projects/microb2010/tests/static_beacon/lfuse @@ -0,0 +1 @@ +0xE1 diff --git a/projects/microb2010/tests/static_beacon/lfuse_new b/projects/microb2010/tests/static_beacon/lfuse_new new file mode 100644 index 0000000..4a40b3f --- /dev/null +++ b/projects/microb2010/tests/static_beacon/lfuse_new @@ -0,0 +1,2 @@ +:01000000FF00 +:00000001FF diff --git a/projects/microb2010/tests/static_beacon/static_beacon.bin b/projects/microb2010/tests/static_beacon/static_beacon.bin new file mode 100755 index 0000000..096876d --- /dev/null +++ b/projects/microb2010/tests/static_beacon/static_beacon.bin @@ -0,0 +1,2 @@ +À,À+À*À)À(À'À&À%À$À#À"À!À ÀÀÀÀÀÀ$¾ÏåÔàÞ¿Í¿à æ°àäéñàÀ ’ 6±Ù÷à æ°àÀ’ 6±á÷КÀÑÏߒï’ÿ’““ß“Ï“’Í·Þ·‹à‰ƒ€î»¹š‚âà—½†½áà›½Š½‚菽ˆáŽ½ƒàƒ¿Ž_OQç(áÒ.aâpà’èé.‰áø. ê¿àƒ³ƒpƒ0á÷ƒ³ƒpƒ0áó‚0ùô"·•šÀ‚·‚Š1Àôƒ³ƒp‚0Áóƒ0‘ôÀ‚·‚‹?hô“³“p“0Á󖚂·‚Œ0 ð‘0ô€àÀï•˜–˜ˆ#‘ö—šKàø à0à@ÿ ÀÞ¼}½l½ï¼þ¼…/Š•ñ÷¼¼ +À¼¼…/Š•ñ÷Þ¼}½l½ï¼þ¼…/Š•ñ÷F•É‡pp‰+!ô"#ð1–@/_?O$01¹ö¼¼…/Š•ñ÷ŠààÀý1—ñ÷—/ï?’Á÷—˜’Ïø”ÿÏ \ No newline at end of file diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c new file mode 100755 index 0000000..b8a2adb --- /dev/null +++ b/projects/microb2010/tests/static_beacon/static_beacon.c @@ -0,0 +1,280 @@ +/* + * Copyright Droids Corporation (2009) + * 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 + * (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: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include + +/* + * Leds: PD5 -> PD7 + * Photodiodes: PC0 PC1 + * IR: PB1, OC1A + */ + +/* + * hfuse: RSTDISBL=1 WTDON=1 SPIEN=0 CKOPT=0 EESAVE=1 BOOTSZ1=0 BOOTSZ0=0 BOOTRST=1 + * lfuse: BODLEVEL=1 BODEN=1 SUT1=1 SUT0=1 CKSEL3=1 CKSEL2=1 CKSEL1=1 CKSEL0=1 + */ + +#define N_PERIODS 10 +#define N_CYCLES_0 17 +#define N_CYCLES_1 17 +#define N_CYCLES_PERIOD (N_CYCLES_0 + N_CYCLES_1) + +#define LED_PORT PORTD +#define LED_DDR DDRD +#define LED1_BIT 5 +#define LED2_BIT 6 +#define LED3_BIT 7 + +#define LED_TOGGLE(port, bit) do { \ + if (port & _BV(bit)) \ + port &= ~_BV(bit); \ + else \ + port |= _BV(bit); \ + } while(0) + +#define LED1_ON() sbi(LED_PORT, LED1_BIT) +#define LED1_OFF() cbi(LED_PORT, LED1_BIT) +#define LED1_TOGGLE() LED_TOGGLE(LED_PORT, LED1_BIT) +#define LED2_ON() sbi(LED_PORT, LED2_BIT) +#define LED2_OFF() cbi(LED_PORT, LED2_BIT) +#define LED2_TOGGLE() LED_TOGGLE(LED_PORT, LED2_BIT) +#define LED3_ON() sbi(LED_PORT, LED3_BIT) +#define LED3_OFF() cbi(LED_PORT, LED3_BIT) +#define LED3_TOGGLE() LED_TOGGLE(LED_PORT, LED3_BIT) + +#define IR_PORT PORTB +#define IR_DDR DDRB +#define IR_BIT 1 + +/* FRAME must be odd */ +#define FRAME 0x0B /* in little endian 1-1-0-1 */ +#define FRAME_LEN 4 + +/* pin returns 1 when nothing, and 0 when laser is on photodiode */ +#define PHOTO_PIN PINC +#define PHOTO1_BIT 0 +#define PHOTO2_BIT 1 +#define READ_PHOTOS() (PHOTO_PIN & (_BV(PHOTO1_BIT) | _BV(PHOTO2_BIT))) +#define READ_PHOTO1() (PHOTO_PIN & _BV(PHOTO1_BIT)) +#define READ_PHOTO2() (PHOTO_PIN & _BV(PHOTO2_BIT)) + +#define PHOTOS_ALL_OFF (_BV(PHOTO1_BIT) | _BV(PHOTO2_BIT)) +#define PHOTOS_ALL_ON (0) +#define PHOTOS_1ON_2OFF (_BV(PHOTO2_BIT)) +#define PHOTOS_1OFF_2ON (_BV(PHOTO1_BIT)) + + +/* in cycles/64 (unit is 4 us at 16Mhz) */ +#define MAX_PHOTO_TIME ((uint8_t)25) /* t=100us len=5mm rps=40Hz dist=20cm */ + +#define MIN_INTER_TIME ((uint8_t)12) /* t=50us len=50mm rps=40Hz dist=350cm */ +#define MAX_INTER_TIME ((uint8_t)250) /* t=1000us len=50mm rps=40Hz dist=20cm */ + +/* in ms */ +#define INTER_LASER_TIME 10 + +#define WAIT_LASER + +/* basic functions to transmit on IR */ + +static inline void xmit_0(void) +{ + uint8_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 3); + TCCR1B = 0; + TCCR1A = 0; + _delay_loop_1(t); /* 3 cycles per loop */ +} + +static inline void xmit_1(void) +{ + uint8_t t = ((N_CYCLES_PERIOD * N_PERIODS) / 3); + TCCR1B = _BV(WGM13) | _BV(WGM12); + TCNT1 = N_CYCLES_PERIOD-1; + TCCR1A = _BV(COM1A1) | _BV(WGM11); + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10); + _delay_loop_1(t); /* 3 cycles per loop */ +} + +/* transmit in manchester code */ + +static inline void xmit_manchester_0(void) +{ + xmit_0(); + xmit_1(); +} + +static inline void xmit_manchester_1(void) +{ + xmit_1(); + xmit_0(); +} + +/* transmit a full frame. Each byte is lsb first. */ + +static void xmit_bits(uint8_t *buf, uint8_t nbit) +{ + uint8_t i; + uint8_t byte = *buf; + + for (i=0; i>= 1; + + /* next byte */ + if (((i & 0x07) == 0) && (i != 0)) + byte = *(++buf); + } + xmit_0(); +} + +/* */ +static inline int8_t wait_laser(void) +{ + uint8_t photos; + uint8_t time; + uint8_t diff; + + /* wait until all is off */ + while (READ_PHOTOS() != PHOTOS_ALL_OFF); + + /* wait an event, it must be photo1 on */ + while ((photos = READ_PHOTOS()) == PHOTOS_ALL_OFF); + + if (photos != PHOTOS_1ON_2OFF) + return -1; + time = TCNT0; + LED1_ON(); + + /* wait an event, it must be photo1 off */ + while ((photos = READ_PHOTOS()) == PHOTOS_1ON_2OFF) { + diff = TCNT0 - time; + if (diff > MAX_PHOTO_TIME) + return -1; + } + if (photos != PHOTOS_ALL_OFF) + return -1; + //time = TCNT0; + + /* wait an event, it must be photo2 on */ + while ((photos = READ_PHOTOS()) == PHOTOS_ALL_OFF) { + diff = TCNT0 - time; + if (diff > MAX_INTER_TIME) + return -1; + } + LED2_ON(); + + diff = TCNT0 - time; + if (diff < MIN_INTER_TIME) + return -1; + if (photos != PHOTOS_1OFF_2ON) + return -1; + +#if 0 + time = TCNT0; + + /* wait an event, it must be photo2 off */ + while ((photos = READ_PHOTOS()) == PHOTOS_1OFF_2ON) { + diff = TCNT0 - time; + if (diff > MAX_PHOTO_TIME) + return -1; + } + if (photos != PHOTOS_ALL_OFF) + return -1; +#endif + + /* laser ok */ + return 0; +} + +/* */ + +int main(void) +{ + /* must be odd */ + uint8_t frame = FRAME; + int8_t ret; + + /* LEDS */ + LED_DDR = _BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT); + IR_DDR |= _BV(IR_BIT); + +#if 0 + while (1) { + LED1_ON(); + wait_ms(500); + LED1_OFF(); + wait_ms(500); + } +#endif + +#if 0 + while (1) { + if (READ_PHOTO() & _BV(PHOTO1_BIT)) + LED1_ON(); + else + LED1_OFF(); + } +#endif + + /* configure PWM */ + ICR1 = N_CYCLES_PERIOD; + OCR1A = N_CYCLES_1; + TCCR1A = _BV(COM1A1) | _BV(WGM11); + TCCR1B = _BV(WGM13) | _BV(WGM12); + + /* configure timer 0, prescaler = 64 */ + TCCR0 = _BV(CS01) | _BV(CS00); + + while (1) { + +#ifdef WAIT_LASER + ret = wait_laser(); + + LED1_OFF(); + LED2_OFF(); + + if (ret) + continue; +#endif + +#if 1 + LED3_ON(); + /* ok, transmit frame */ + xmit_bits(&frame, FRAME_LEN); + + /* don't watch a laser during this time */ + wait_ms(INTER_LASER_TIME); + LED3_OFF(); +#else + LED1_ON(); + wait_ms(1); + LED1_OFF(); +#endif + } + return 0; +} diff --git a/projects/microb2010/tests/test_board2008/.config b/projects/microb2010/tests/test_board2008/.config new file mode 100644 index 0000000..f1528d4 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/.config @@ -0,0 +1,280 @@ +# +# Automatically generated make config: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +# CONFIG_MCU_ATMEGA8 is not set +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +CONFIG_MCU_ATMEGA128=y +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +# CONFIG_MCU_ATMEGA2560 is not set +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +CONFIG_MATH_LIB=y +# CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set +# CONFIG_MINIMAL_PRINTF is not set +# CONFIG_STANDARD_PRINTF is not set +CONFIG_ADVANCED_PRINTF=y +# CONFIG_FORMAT_IHEX is not set +# CONFIG_FORMAT_SREC is not set +CONFIG_FORMAT_BINARY=y + +# +# Base modules +# + +# +# Enable math library in generation options to see all modules +# +CONFIG_MODULE_CIRBUF=y +# CONFIG_MODULE_CIRBUF_LARGE is not set +CONFIG_MODULE_FIXED_POINT=y +CONFIG_MODULE_VECT2=y +CONFIG_MODULE_GEOMETRY=y +CONFIG_MODULE_SCHEDULER=y +CONFIG_MODULE_SCHEDULER_STATS=y +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +# CONFIG_MODULE_SCHEDULER_TIMER0 is not set +CONFIG_MODULE_SCHEDULER_MANUAL=y +CONFIG_MODULE_TIME=y +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# + +# +# uart needs circular buffer, mf2 client may need scheduler +# +CONFIG_MODULE_UART=y +# CONFIG_MODULE_UART_9BITS is not set +CONFIG_MODULE_UART_CREATE_CONFIG=y +# CONFIG_MODULE_SPI is not set +# CONFIG_MODULE_SPI_CREATE_CONFIG is not set +# CONFIG_MODULE_I2C is not set +# CONFIG_MODULE_I2C_MASTER is not set +# CONFIG_MODULE_I2C_MULTIMASTER is not set +# CONFIG_MODULE_I2C_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +CONFIG_MODULE_TIMER=y +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +CONFIG_MODULE_PWM_NG=y +CONFIG_MODULE_ADC=y +CONFIG_MODULE_ADC_CREATE_CONFIG=y + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +CONFIG_MODULE_VT100=y +CONFIG_MODULE_RDLINE=y +CONFIG_MODULE_RDLINE_CREATE_CONFIG=y +CONFIG_MODULE_RDLINE_KILL_BUF=y +CONFIG_MODULE_RDLINE_HISTORY=y +CONFIG_MODULE_PARSE=y +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +# CONFIG_MODULE_AX12 is not set +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +CONFIG_MODULE_ENCODERS_MICROB=y +CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG=y +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_SPI is not set +# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set + +# +# Robot specific modules +# +CONFIG_MODULE_ROBOT_SYSTEM=y +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +CONFIG_MODULE_POSITION_MANAGER=y +CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y +CONFIG_MODULE_TRAJECTORY_MANAGER=y +CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y +# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set + +# +# Control system modules +# +CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y + +# +# Filters +# +CONFIG_MODULE_PID=y +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +CONFIG_MODULE_QUADRAMP=y +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# + +# +# Some radio devices require SPI to be activated +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# + +# +# Crypto modules depend on utils module +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# + +# +# Encoding modules depend on utils module +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# + +# +# Debug modules depend on utils module +# +CONFIG_MODULE_DIAGNOSTIC=y +CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y +CONFIG_MODULE_ERROR=y +CONFIG_MODULE_ERROR_CREATE_CONFIG=y + +# +# Programmer options +# +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/tests/test_board2008/.config.old b/projects/microb2010/tests/test_board2008/.config.old new file mode 100644 index 0000000..f1528d4 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/.config.old @@ -0,0 +1,280 @@ +# +# Automatically generated make config: don't edit +# + +# +# Hardware +# +# CONFIG_MCU_AT90S2313 is not set +# CONFIG_MCU_AT90S2323 is not set +# CONFIG_MCU_AT90S3333 is not set +# CONFIG_MCU_AT90S2343 is not set +# CONFIG_MCU_ATTINY22 is not set +# CONFIG_MCU_ATTINY26 is not set +# CONFIG_MCU_AT90S4414 is not set +# CONFIG_MCU_AT90S4433 is not set +# CONFIG_MCU_AT90S4434 is not set +# CONFIG_MCU_AT90S8515 is not set +# CONFIG_MCU_AT90S8534 is not set +# CONFIG_MCU_AT90S8535 is not set +# CONFIG_MCU_AT86RF401 is not set +# CONFIG_MCU_ATMEGA103 is not set +# CONFIG_MCU_ATMEGA603 is not set +# CONFIG_MCU_AT43USB320 is not set +# CONFIG_MCU_AT43USB355 is not set +# CONFIG_MCU_AT76C711 is not set +# CONFIG_MCU_ATMEGA8 is not set +# CONFIG_MCU_ATMEGA48 is not set +# CONFIG_MCU_ATMEGA88 is not set +# CONFIG_MCU_ATMEGA8515 is not set +# CONFIG_MCU_ATMEGA8535 is not set +# CONFIG_MCU_ATTINY13 is not set +# CONFIG_MCU_ATTINY2313 is not set +# CONFIG_MCU_ATMEGA16 is not set +# CONFIG_MCU_ATMEGA161 is not set +# CONFIG_MCU_ATMEGA162 is not set +# CONFIG_MCU_ATMEGA163 is not set +# CONFIG_MCU_ATMEGA165 is not set +# CONFIG_MCU_ATMEGA168 is not set +# CONFIG_MCU_ATMEGA169 is not set +# CONFIG_MCU_ATMEGA32 is not set +# CONFIG_MCU_ATMEGA323 is not set +# CONFIG_MCU_ATMEGA325 is not set +# CONFIG_MCU_ATMEGA3250 is not set +# CONFIG_MCU_ATMEGA64 is not set +# CONFIG_MCU_ATMEGA645 is not set +# CONFIG_MCU_ATMEGA6450 is not set +CONFIG_MCU_ATMEGA128=y +# CONFIG_MCU_ATMEGA1281 is not set +# CONFIG_MCU_AT90CAN128 is not set +# CONFIG_MCU_AT94K is not set +# CONFIG_MCU_AT90S1200 is not set +# CONFIG_MCU_ATMEGA2560 is not set +# CONFIG_MCU_ATMEGA256 is not set +CONFIG_QUARTZ=16000000 + +# +# Generation options +# +# CONFIG_OPTM_0 is not set +# CONFIG_OPTM_1 is not set +# CONFIG_OPTM_2 is not set +# CONFIG_OPTM_3 is not set +CONFIG_OPTM_S=y +CONFIG_MATH_LIB=y +# CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set +# CONFIG_MINIMAL_PRINTF is not set +# CONFIG_STANDARD_PRINTF is not set +CONFIG_ADVANCED_PRINTF=y +# CONFIG_FORMAT_IHEX is not set +# CONFIG_FORMAT_SREC is not set +CONFIG_FORMAT_BINARY=y + +# +# Base modules +# + +# +# Enable math library in generation options to see all modules +# +CONFIG_MODULE_CIRBUF=y +# CONFIG_MODULE_CIRBUF_LARGE is not set +CONFIG_MODULE_FIXED_POINT=y +CONFIG_MODULE_VECT2=y +CONFIG_MODULE_GEOMETRY=y +CONFIG_MODULE_SCHEDULER=y +CONFIG_MODULE_SCHEDULER_STATS=y +# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set +# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set +# CONFIG_MODULE_SCHEDULER_TIMER0 is not set +CONFIG_MODULE_SCHEDULER_MANUAL=y +CONFIG_MODULE_TIME=y +# CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set + +# +# Communication modules +# + +# +# uart needs circular buffer, mf2 client may need scheduler +# +CONFIG_MODULE_UART=y +# CONFIG_MODULE_UART_9BITS is not set +CONFIG_MODULE_UART_CREATE_CONFIG=y +# CONFIG_MODULE_SPI is not set +# CONFIG_MODULE_SPI_CREATE_CONFIG is not set +# CONFIG_MODULE_I2C is not set +# CONFIG_MODULE_I2C_MASTER is not set +# CONFIG_MODULE_I2C_MULTIMASTER is not set +# CONFIG_MODULE_I2C_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_CLIENT is not set +# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set +# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set +# CONFIG_MODULE_MF2_SERVER is not set +# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set + +# +# Hardware modules +# +CONFIG_MODULE_TIMER=y +# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set +# CONFIG_MODULE_TIMER_DYNAMIC is not set +# CONFIG_MODULE_PWM is not set +# CONFIG_MODULE_PWM_CREATE_CONFIG is not set +CONFIG_MODULE_PWM_NG=y +CONFIG_MODULE_ADC=y +CONFIG_MODULE_ADC_CREATE_CONFIG=y + +# +# IHM modules +# +# CONFIG_MODULE_MENU is not set +CONFIG_MODULE_VT100=y +CONFIG_MODULE_RDLINE=y +CONFIG_MODULE_RDLINE_CREATE_CONFIG=y +CONFIG_MODULE_RDLINE_KILL_BUF=y +CONFIG_MODULE_RDLINE_HISTORY=y +CONFIG_MODULE_PARSE=y +# CONFIG_MODULE_PARSE_NO_FLOAT is not set + +# +# External devices modules +# +# CONFIG_MODULE_LCD is not set +# CONFIG_MODULE_LCD_CREATE_CONFIG is not set +# CONFIG_MODULE_MULTISERVO is not set +# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +# CONFIG_MODULE_AX12 is not set +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set + +# +# Brushless motor drivers (you should enable pwm modules to see all) +# +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set +# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set + +# +# Encoders (you need comm/spi for encoders_spi) +# +CONFIG_MODULE_ENCODERS_MICROB=y +CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG=y +# CONFIG_MODULE_ENCODERS_EIRBOT is not set +# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_SPI is not set +# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set + +# +# Robot specific modules +# +CONFIG_MODULE_ROBOT_SYSTEM=y +# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set +CONFIG_MODULE_POSITION_MANAGER=y +CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y +CONFIG_MODULE_TRAJECTORY_MANAGER=y +CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y +# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set + +# +# Control system modules +# +CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y + +# +# Filters +# +CONFIG_MODULE_PID=y +# CONFIG_MODULE_PID_CREATE_CONFIG is not set +# CONFIG_MODULE_RAMP is not set +CONFIG_MODULE_QUADRAMP=y +# CONFIG_MODULE_QUADRAMP_DERIVATE is not set +# CONFIG_MODULE_BIQUAD is not set + +# +# Radio devices +# + +# +# Some radio devices require SPI to be activated +# +# CONFIG_MODULE_CC2420 is not set +# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set + +# +# Crypto modules +# + +# +# Crypto modules depend on utils module +# +# CONFIG_MODULE_AES is not set +# CONFIG_MODULE_AES_CTR is not set +# CONFIG_MODULE_MD5 is not set +# CONFIG_MODULE_MD5_HMAC is not set +# CONFIG_MODULE_RC4 is not set + +# +# Encodings modules +# + +# +# Encoding modules depend on utils module +# +# CONFIG_MODULE_BASE64 is not set +# CONFIG_MODULE_HAMMING is not set + +# +# Debug modules +# + +# +# Debug modules depend on utils module +# +CONFIG_MODULE_DIAGNOSTIC=y +CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y +CONFIG_MODULE_ERROR=y +CONFIG_MODULE_ERROR_CREATE_CONFIG=y + +# +# Programmer options +# +CONFIG_AVRDUDE=y +# CONFIG_AVARICE is not set + +# +# Avrdude +# +# CONFIG_AVRDUDE_PROG_FUTURELEC is not set +# CONFIG_AVRDUDE_PROG_ABCMINI is not set +# CONFIG_AVRDUDE_PROG_PICOWEB is not set +# CONFIG_AVRDUDE_PROG_SP12 is not set +# CONFIG_AVRDUDE_PROG_ALF is not set +# CONFIG_AVRDUDE_PROG_BASCOM is not set +# CONFIG_AVRDUDE_PROG_DT006 is not set +# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set +CONFIG_AVRDUDE_PROG_STK200=y +# CONFIG_AVRDUDE_PROG_PAVR is not set +# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set +# CONFIG_AVRDUDE_PROG_AVR910 is not set +# CONFIG_AVRDUDE_PROG_STK500 is not set +# CONFIG_AVRDUDE_PROG_AVRISP is not set +# CONFIG_AVRDUDE_PROG_BSD is not set +# CONFIG_AVRDUDE_PROG_DAPA is not set +# CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set +CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 + +# +# Avarice +# +CONFIG_AVARICE_PORT="/dev/ttyS0" +CONFIG_AVARICE_DEBUG_PORT=1234 +CONFIG_AVARICE_PROG_MKI=y +# CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/projects/microb2010/tests/test_board2008/Makefile b/projects/microb2010/tests/test_board2008/Makefile new file mode 100755 index 0000000..5dde5d5 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/Makefile @@ -0,0 +1,27 @@ +TARGET = main + +# repertoire des modules +AVERSIVE_DIR = ../../../.. + +SRC = $(TARGET).c cmdline.c commands_gen.c +SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c +SRC += sensor.c actuator.c cs.c strat_base.c strat_utils.c + +ASRC = + +CFLAGS += -Wall -Werror +#CFLAGS += -DHOMOLOGATION +#CFLAGS += -DTEST_BEACON +#LDFLAGS = -T ../common/avr6.x + +######################################## + +-include .aversive_conf +include $(AVERSIVE_DIR)/mk/aversive_project.mk + +AVRDUDE_DELAY=50 + +program_noerase: $(TARGET).$(FORMAT_EXTENSION) $(TARGET).eep + echo $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + $(AVRDUDE) -D -V $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) ;\ + diff --git a/projects/microb2010/tests/test_board2008/action.c b/projects/microb2010/tests/test_board2008/action.c new file mode 100644 index 0000000..2b3917e --- /dev/null +++ b/projects/microb2010/tests/test_board2008/action.c @@ -0,0 +1,46 @@ +/* + * Copyright Droids Corporation, Microb Technology (2008) + * + * 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: action.c,v 1.5 2008-03-31 22:22:52 zer0 Exp $ + * + * Olivier MATZ + */ + +#include + +#include + +#include "../common/i2c_commands.h" +#include "main.h" +#include "action.h" + +void action_init(void) +{ +} + +/* + * Protocol: + * 0000 XXXX select servo XXXX + * 01XX XXXX set LSB of servo + * 10XX XXXX set MSB of servo and validate value + */ +void action_servo_set(uint8_t num, uint16_t us) +{ + uart1_send(num); + uart1_send(0x40 | (us&0x3F)); + uart1_send(0x80 | ((us>>6)&0x3F)); +} diff --git a/projects/microb2010/tests/test_board2008/action.h b/projects/microb2010/tests/test_board2008/action.h new file mode 100644 index 0000000..7dfcdb9 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/action.h @@ -0,0 +1,37 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: action.h,v 1.3 2008-03-31 22:22:52 zer0 Exp $ + * + */ + +#ifndef _ACTION_H_ +#define _ACTION_H_ + +#define BRAKE_PORT PORTG +//#define BRAKE_DDR DDRG +#define BRAKE_PIN PING +#define BRAKE_MASK 0x03 + +//#define BRAKE_ON() do { BRAKE_PORT = BRAKE_PIN | BRAKE_MASK; } while(0) +//#define BRAKE_OFF() do { BRAKE_PORT = BRAKE_PIN & (~BRAKE_MASK); } while(0) +#define BRAKE_IS_ON ((BRAKE_PIN & BRAKE_MASK) == BRAKE_MASK) + +void action_init(void); +void action_servo_set(uint8_t num, uint16_t us); + +#endif diff --git a/projects/microb2010/tests/test_board2008/actuator.c b/projects/microb2010/tests/test_board2008/actuator.c new file mode 100644 index 0000000..5e2f39a --- /dev/null +++ b/projects/microb2010/tests/test_board2008/actuator.c @@ -0,0 +1,366 @@ +/* + * 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 + * (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.3 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "sensor.h" +#include "main.h" + +#define OFF 0 +#define WAIT_SENSOR 1 +#define SENSOR_OK 2 +#define WAIT_DOWN 3 + +static volatile uint8_t fessor_state = OFF; +static volatile uint32_t fessor_pos_up = 35000; +static volatile uint32_t fessor_pos_down = 63500; +static volatile uint32_t fessor_delay_up = 500; +static volatile uint32_t fessor_delay_down = 2000; +static volatile uint32_t delay = 0; +static volatile int32_t fessor_k1 = 500, fessor_k2 = 20; +static volatile int32_t fessor_cmd = 0; + + +static volatile uint8_t elevator_state = OFF; +static volatile uint32_t elevator_pos_up = -8000; +static volatile uint32_t elevator_pos_down = 0; +static volatile uint32_t elevator_delay_up = 500; +static volatile uint32_t elevator_delay_down = 2000; +//static volatile uint32_t delay = 0; +static volatile int32_t elevator_k1 = 500, elevator_k2 = 20; +static volatile int32_t elevator_cmd = 0; + +void pwm_set_and_save(void *pwm, int32_t val) +{ + /* we need to do the saturation here, before saving the + * value */ + if (val > 4095) + val = 4095; + if (val < -4095) + val = -4095; + + if (pwm == LEFT_PWM) + mainboard.pwm_l = val; + else if (pwm == RIGHT_PWM) + mainboard.pwm_r = val; + pwm_ng_set(pwm, val); +} + +void pickup_wheels_on(void) +{ + mainboard.enable_pickup_wheels = 1; +} + +void pickup_wheels_off(void) +{ + mainboard.enable_pickup_wheels = 0; +} + +/* init fessor position at beginning */ +void fessor_autopos(void) +{ + pwm_ng_set(FESSOR_PWM, -500); + wait_ms(3000); + pwm_ng_set(FESSOR_PWM, 0); +} + +/* set CS command for fessor */ +void fessor_set(void *dummy, int32_t cmd) +{ + fessor_cmd = cmd; +} + +void fessor_set_coefs(uint32_t k1, uint32_t k2) +{ + fessor_k1 = k1; + fessor_k2 = k2; +} + +void fessor_set_delays(uint32_t delay_up, uint32_t delay_down) +{ + fessor_delay_up = delay_up; + fessor_delay_down = delay_down; +} + +void fessor_set_pos(uint32_t pos_up, uint32_t pos_down) +{ + fessor_pos_up = pos_up; + fessor_pos_down = pos_down; +} + +void fessor_dump_params(void) +{ + printf_P(PSTR("coef %ld %ld\r\n"), fessor_k1, fessor_k2); + printf_P(PSTR("pos %ld %ld\r\n"), fessor_pos_up, fessor_pos_down); + printf_P(PSTR("delay %ld %ld\r\n"), fessor_delay_up, fessor_delay_down); +} + +/* called by CS perdiodically (current limit) */ +void fessor_manage(void) +{ + static int32_t oldpos = 0; + int32_t pos, maxcmd, speed, cmd; + + cmd = fessor_cmd; + pos = encoders_microb_get_value(FESSOR_ENC); + speed = pos - oldpos; + if (speed > 0 && cmd < 0) + maxcmd = fessor_k1; + else if (speed < 0 && cmd > 0) + maxcmd = fessor_k1; + else { + speed = ABS(speed); + maxcmd = fessor_k1 + fessor_k2 * speed; + } + if (cmd > maxcmd) + cmd = maxcmd; + else if (cmd < -maxcmd) + cmd = -maxcmd; + + pwm_ng_set(FESSOR_PWM, cmd); + + oldpos = pos; +} + +void fessor_up(void) +{ + fessor_state = 0; + cs_set_consign(&mainboard.fessor.cs, fessor_pos_up); +} + +void fessor_down(void) +{ + fessor_state = 0; + cs_set_consign(&mainboard.fessor.cs, fessor_pos_down); +} + +void fessor_stop(void) +{ + fessor_state = 0; +} + +void fessor_auto(void) +{ + fessor_state = WAIT_SENSOR; + cs_set_consign(&mainboard.fessor.cs, fessor_pos_up); +} + +/* for fessor auto mode */ +static void fessor_cb(void *dummy) +{ + static uint8_t prev = 0; + uint8_t val; + + val = !sensor_get(0); + + switch (fessor_state) { + case OFF: + break; + case WAIT_SENSOR: + if (val && !prev) { + delay = fessor_delay_up; + fessor_state = SENSOR_OK; + } + break; + case SENSOR_OK: + if (delay-- == 0) { + cs_set_consign(&mainboard.fessor.cs, fessor_pos_down); + fessor_state = WAIT_DOWN; + delay = fessor_delay_down; + } + break; + case WAIT_DOWN: + if (delay-- == 0) { + cs_set_consign(&mainboard.fessor.cs, fessor_pos_up); + fessor_state = WAIT_SENSOR; + } + break; + default: + break; + } + prev = val; +} + +void fessor_init(void) +{ + scheduler_add_periodical_event_priority(fessor_cb, NULL, + 1000L / SCHEDULER_UNIT, + FESSOR_PRIO); +} + + + + + + + + +/* init elevator position at beginning */ +void elevator_autopos(void) +{ + pwm_ng_set(ELEVATOR_PWM, 300); + wait_ms(4000); + pwm_ng_set(ELEVATOR_PWM, 0); +} + +/* set CS command for elevator */ +void elevator_set(void *dummy, int32_t cmd) +{ + elevator_cmd = cmd; +} + +void elevator_set_coefs(uint32_t k1, uint32_t k2) +{ + elevator_k1 = k1; + elevator_k2 = k2; +} + +void elevator_set_delays(uint32_t delay_up, uint32_t delay_down) +{ + elevator_delay_up = delay_up; + elevator_delay_down = delay_down; +} + +void elevator_set_pos(uint32_t pos_up, uint32_t pos_down) +{ + elevator_pos_up = pos_up; + elevator_pos_down = pos_down; +} + +void elevator_dump_params(void) +{ + printf_P(PSTR("coef %ld %ld\r\n"), elevator_k1, elevator_k2); + printf_P(PSTR("pos %ld %ld\r\n"), elevator_pos_up, elevator_pos_down); + printf_P(PSTR("delay %ld %ld\r\n"), elevator_delay_up, elevator_delay_down); +} + +/* called by CS perdiodically (current limit) */ +void elevator_manage(void) +{ + static int32_t oldpos = 0; + int32_t pos, maxcmd, speed, cmd; + + cmd = elevator_cmd; + pos = encoders_microb_get_value(ELEVATOR_ENC); + speed = pos - oldpos; + if (speed > 0 && cmd < 0) + maxcmd = elevator_k1; + else if (speed < 0 && cmd > 0) + maxcmd = elevator_k1; + else { + speed = ABS(speed); + maxcmd = elevator_k1 + elevator_k2 * speed; + } + if (cmd > maxcmd) + cmd = maxcmd; + else if (cmd < -maxcmd) + cmd = -maxcmd; + + pwm_ng_set(ELEVATOR_PWM, cmd); + + oldpos = pos; +} + +void elevator_up(void) +{ + elevator_state = 0; + cs_set_consign(&mainboard.elevator.cs, elevator_pos_up); +} + +void elevator_down(void) +{ + elevator_state = 0; + cs_set_consign(&mainboard.elevator.cs, elevator_pos_down); +} + +void elevator_stop(void) +{ + elevator_state = 0; +} + +void elevator_auto(void) +{ + elevator_state = WAIT_SENSOR; + cs_set_consign(&mainboard.elevator.cs, elevator_pos_down); +} + +/* for elevator auto mode */ +static void elevator_cb(void *dummy) +{ + static uint8_t prev = 0; + uint8_t val; + + val = !sensor_get(0); + + switch (elevator_state) { + case OFF: + break; + case WAIT_SENSOR: + if (val && !prev) { + delay = elevator_delay_up; + elevator_state = SENSOR_OK; + } + break; + case SENSOR_OK: + if (delay-- == 0) { + cs_set_consign(&mainboard.elevator.cs, elevator_pos_up); + elevator_state = WAIT_DOWN; + delay = elevator_delay_down; + } + break; + case WAIT_DOWN: + if (delay-- == 0) { + cs_set_consign(&mainboard.elevator.cs, elevator_pos_down); + elevator_state = WAIT_SENSOR; + } + break; + default: + break; + } + prev = val; +} + +void elevator_init(void) +{ + scheduler_add_periodical_event_priority(elevator_cb, NULL, + 1000L / SCHEDULER_UNIT, + ELEVATOR_PRIO); +} diff --git a/projects/microb2010/tests/test_board2008/actuator.h b/projects/microb2010/tests/test_board2008/actuator.h new file mode 100644 index 0000000..0da5415 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/actuator.h @@ -0,0 +1,53 @@ +/* + * 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 + * (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.h,v 1.2 2009-04-24 19:30:41 zer0 Exp $ + * + */ + +void pwm_set_and_save(void *pwm, int32_t val); +void pickup_wheels_on(void); +void pickup_wheels_off(void); + +void fessor_set(void *dummy, int32_t cmd); +void fessor_manage(void); +void fessor_up(void); +void fessor_down(void); +void fessor_stop(void); +void fessor_auto(void); +void fessor_init(void); +void fessor_autopos(void); +void fessor_dump_params(void); + +void fessor_set_coefs(uint32_t k1, uint32_t k2); +void fessor_set_delays(uint32_t delay_up, uint32_t delay_down); +void fessor_set_pos(uint32_t pos_up, uint32_t pos_down); + + +void elevator_set(void *dummy, int32_t cmd); +void elevator_manage(void); +void elevator_up(void); +void elevator_down(void); +void elevator_stop(void); +void elevator_auto(void); +void elevator_init(void); +void elevator_autopos(void); +void elevator_dump_params(void); + +void elevator_set_coefs(uint32_t k1, uint32_t k2); +void elevator_set_delays(uint32_t delay_up, uint32_t delay_down); +void elevator_set_pos(uint32_t pos_up, uint32_t pos_down); diff --git a/projects/microb2010/tests/test_board2008/adc_config.h b/projects/microb2010/tests/test_board2008/adc_config.h new file mode 100644 index 0000000..5bce59c --- /dev/null +++ b/projects/microb2010/tests/test_board2008/adc_config.h @@ -0,0 +1,7 @@ +#ifndef _ADC_CONFIG_H_ +#define _ADC_CONFIG_H_ + + + + +#endif // _ADC_CONFIG_H_ diff --git a/projects/microb2010/tests/test_board2008/cmdline.c b/projects/microb2010/tests/test_board2008/cmdline.c new file mode 100644 index 0000000..c584222 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/cmdline.c @@ -0,0 +1,167 @@ +/* + * 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 + * (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: cmdline.c,v 1.5 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "main.h" +#include "strat_base.h" +#include "cmdline.h" + + +/******** See in commands.c for the list of commands. */ +extern parse_pgm_ctx_t main_ctx[]; + +static void write_char(char c) +{ + uart_send(CMDLINE_UART, c); +} + +static void +valid_buffer(const char *buf, uint8_t size) +{ + int8_t ret; + + /* reset CTRL-C for trajectory interruption each time we + * receive a new command */ + interrupt_traj_reset(); + + ret = parse(main_ctx, buf); + if (ret == PARSE_AMBIGUOUS) + printf_P(PSTR("Ambiguous command\r\n")); + else if (ret == PARSE_NOMATCH) + printf_P(PSTR("Command not found\r\n")); + else if (ret == PARSE_BAD_ARGS) + printf_P(PSTR("Bad arguments\r\n")); +} + +static int8_t +complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize, + int16_t *state) +{ + return complete(main_ctx, buf, state, dstbuf, dstsize); +} + + +/* sending "pop" on cmdline uart resets the robot */ +void emergency(char c) +{ + static uint8_t i = 0; + + /* interrupt traj here */ + if (c == '\003') + interrupt_traj(); + + if ((i == 0 && c == 'p') || + (i == 1 && c == 'o') || + (i == 2 && c == 'p')) + i++; + else if ( !(i == 1 && c == 'p') ) + i = 0; + if (i == 3) + reset(); +} + +/* log function, add a command to configure + * it dynamically */ +void mylog(struct error * e, ...) +{ + va_list ap; + u16 stream_flags = stdout->flags; + uint8_t i; + time_h tv; + + if (e->severity > ERROR_SEVERITY_ERROR) { + if (gen.log_level < e->severity) + return; + + for (i=0; ierr_num) + break; + if (i == NB_LOGS+1) + return; + } + + va_start(ap, e); + tv = time_get_time(); + printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL)); + + printf_P(PSTR("(%d,%d,%d) "), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + + vfprintf_P(stdout, e->text, ap); + printf_P(PSTR("\r\n")); + va_end(ap); + stdout->flags = stream_flags; +} + +int cmdline_interact(void) +{ + const char *history, *buffer; + int8_t ret, same = 0; + int16_t c; + + rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer); + snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > "); + rdline_newline(&gen.rdl, gen.prompt); + + while (1) { + c = uart_recv_nowait(CMDLINE_UART); + if (c == -1) + continue; + ret = rdline_char_in(&gen.rdl, c); + if (ret != 2 && ret != 0) { + buffer = rdline_get_buffer(&gen.rdl); + history = rdline_get_history_item(&gen.rdl, 0); + if (history) { + same = !memcmp(buffer, history, strlen(history)) && + buffer[strlen(history)] == '\n'; + } + else + same = 0; + if (strlen(buffer) > 1 && !same) + rdline_add_history(&gen.rdl, buffer); + rdline_newline(&gen.rdl, gen.prompt); + } + } + + return 0; +} diff --git a/projects/microb2010/tests/test_board2008/cmdline.h b/projects/microb2010/tests/test_board2008/cmdline.h new file mode 100644 index 0000000..a9a2830 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/cmdline.h @@ -0,0 +1,44 @@ +/* + * 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 + * (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: cmdline.h,v 1.2 2009-02-27 22:23:37 zer0 Exp $ + * + */ + +#define CMDLINE_UART 0 + +/* uart rx callback for reset() */ +void emergency(char c); + +/* log function */ +void mylog(struct error * e, ...); + +/* launch cmdline */ +int cmdline_interact(void); + +static inline uint8_t cmdline_keypressed(void) { + return (uart_recv_nowait(CMDLINE_UART) != -1); +} + +static inline int16_t cmdline_getchar(void) { + return uart_recv_nowait(CMDLINE_UART); +} + +static inline uint8_t cmdline_getchar_wait(void) { + return uart_recv(CMDLINE_UART); +} diff --git a/projects/microb2010/tests/test_board2008/commands.c b/projects/microb2010/tests/test_board2008/commands.c new file mode 100644 index 0000000..ce905e5 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/commands.c @@ -0,0 +1,192 @@ +/* + * 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 + * (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: commands.c,v 1.7 2009-04-24 19:30:41 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include +#include + +/* commands_gen.c */ +extern parse_pgm_inst_t cmd_reset; +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_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; +extern parse_pgm_inst_t cmd_stack_space; +extern parse_pgm_inst_t cmd_scheduler; + +/* commands_ax12.c */ +extern parse_pgm_inst_t cmd_baudrate; +extern parse_pgm_inst_t cmd_uint16_read; +extern parse_pgm_inst_t cmd_uint16_write; +extern parse_pgm_inst_t cmd_uint8_read; +extern parse_pgm_inst_t cmd_uint8_write; + +/* commands_cs.c */ +extern parse_pgm_inst_t cmd_gain; +extern parse_pgm_inst_t cmd_gain_show; +extern parse_pgm_inst_t cmd_speed; +extern parse_pgm_inst_t cmd_speed_show; +extern parse_pgm_inst_t cmd_derivate_filter; +extern parse_pgm_inst_t cmd_derivate_filter_show; +extern parse_pgm_inst_t cmd_consign; +extern parse_pgm_inst_t cmd_maximum; +extern parse_pgm_inst_t cmd_maximum_show; +extern parse_pgm_inst_t cmd_quadramp; +extern parse_pgm_inst_t cmd_quadramp_show; +extern parse_pgm_inst_t cmd_cs_status; +extern parse_pgm_inst_t cmd_blocking_i; +extern parse_pgm_inst_t cmd_blocking_i_show; + +/* commands_mainboard.c */ +extern parse_pgm_inst_t cmd_event; +extern parse_pgm_inst_t cmd_spi_test; +extern parse_pgm_inst_t cmd_opponent; +extern parse_pgm_inst_t cmd_opponent_set; +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_rs; +extern parse_pgm_inst_t cmd_i2cdebug; +extern parse_pgm_inst_t cmd_mechboard_show; +extern parse_pgm_inst_t cmd_mechboard_setmode1; +extern parse_pgm_inst_t cmd_mechboard_setmode2; +extern parse_pgm_inst_t cmd_mechboard_setmode3; +extern parse_pgm_inst_t cmd_mechboard_setmode4; +extern parse_pgm_inst_t cmd_mechboard_setmode5; +extern parse_pgm_inst_t cmd_pickup_wheels; +extern parse_pgm_inst_t cmd_beacon_start; +extern parse_pgm_inst_t cmd_pump_current; +extern parse_pgm_inst_t cmd_build_test; +extern parse_pgm_inst_t cmd_column_test; +extern parse_pgm_inst_t cmd_column_test2; +extern parse_pgm_inst_t cmd_lintel_test; +extern parse_pgm_inst_t cmd_pickup_test; +extern parse_pgm_inst_t cmd_scan_test; +extern parse_pgm_inst_t cmd_scan_test2; +extern parse_pgm_inst_t cmd_time_monitor; +extern parse_pgm_inst_t cmd_scanner; +extern parse_pgm_inst_t cmd_build_z1; +#ifdef TEST_BEACON +extern parse_pgm_inst_t cmd_beacon_opp_dump; +#endif +extern parse_pgm_inst_t cmd_test; +extern parse_pgm_inst_t cmd_fessor; +extern parse_pgm_inst_t cmd_fessor_params; +extern parse_pgm_inst_t cmd_fessor_params_show; + + +extern parse_pgm_inst_t cmd_elevator; +extern parse_pgm_inst_t cmd_elevator_params; +extern parse_pgm_inst_t cmd_elevator_params_show; + +/* commands_traj.c */ +extern parse_pgm_inst_t cmd_traj_speed; +extern parse_pgm_inst_t cmd_traj_speed_show; +extern parse_pgm_inst_t cmd_trajectory; +extern parse_pgm_inst_t cmd_trajectory_show; +extern parse_pgm_inst_t cmd_rs_gains; +extern parse_pgm_inst_t cmd_rs_gains_show; +extern parse_pgm_inst_t cmd_track; +extern parse_pgm_inst_t cmd_track_show; +extern parse_pgm_inst_t cmd_pt_list; +extern parse_pgm_inst_t cmd_pt_list_append; +extern parse_pgm_inst_t cmd_pt_list_del; +extern parse_pgm_inst_t cmd_pt_list_show; +extern parse_pgm_inst_t cmd_goto1; +extern parse_pgm_inst_t cmd_goto2; +extern parse_pgm_inst_t cmd_goto3; +extern parse_pgm_inst_t cmd_position; +extern parse_pgm_inst_t cmd_position_set; +extern parse_pgm_inst_t cmd_strat_infos; +extern parse_pgm_inst_t cmd_strat_conf; +extern parse_pgm_inst_t cmd_strat_conf2; +extern parse_pgm_inst_t cmd_strat_conf3; +extern parse_pgm_inst_t cmd_strat_conf4; +extern parse_pgm_inst_t cmd_subtraj; + +/* in progmem */ +parse_pgm_ctx_t main_ctx[] = { + + /* commands_gen.c */ + (parse_pgm_inst_t *)&cmd_reset, + (parse_pgm_inst_t *)&cmd_bootloader, + (parse_pgm_inst_t *)&cmd_encoders, + (parse_pgm_inst_t *)&cmd_pwm, + (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, + (parse_pgm_inst_t *)&cmd_stack_space, + (parse_pgm_inst_t *)&cmd_scheduler, + + /* commands_cs.c */ + (parse_pgm_inst_t *)&cmd_gain, + (parse_pgm_inst_t *)&cmd_gain_show, + (parse_pgm_inst_t *)&cmd_speed, + (parse_pgm_inst_t *)&cmd_speed_show, + (parse_pgm_inst_t *)&cmd_consign, + (parse_pgm_inst_t *)&cmd_derivate_filter, + (parse_pgm_inst_t *)&cmd_derivate_filter_show, + (parse_pgm_inst_t *)&cmd_maximum, + (parse_pgm_inst_t *)&cmd_maximum_show, + (parse_pgm_inst_t *)&cmd_quadramp, + (parse_pgm_inst_t *)&cmd_quadramp_show, + (parse_pgm_inst_t *)&cmd_cs_status, + (parse_pgm_inst_t *)&cmd_blocking_i, + (parse_pgm_inst_t *)&cmd_blocking_i_show, + + /* commands_mainboard.c */ + (parse_pgm_inst_t *)&cmd_event, + (parse_pgm_inst_t *)&cmd_interact, + (parse_pgm_inst_t *)&cmd_rs, + (parse_pgm_inst_t *)&cmd_fessor, + (parse_pgm_inst_t *)&cmd_fessor_params, + (parse_pgm_inst_t *)&cmd_fessor_params_show, + (parse_pgm_inst_t *)&cmd_elevator, + (parse_pgm_inst_t *)&cmd_elevator_params, + (parse_pgm_inst_t *)&cmd_elevator_params_show, + (parse_pgm_inst_t *)&cmd_test, + + /* commands_traj.c */ + (parse_pgm_inst_t *)&cmd_traj_speed, + (parse_pgm_inst_t *)&cmd_traj_speed_show, + (parse_pgm_inst_t *)&cmd_trajectory, + (parse_pgm_inst_t *)&cmd_trajectory_show, + (parse_pgm_inst_t *)&cmd_rs_gains, + (parse_pgm_inst_t *)&cmd_rs_gains_show, + (parse_pgm_inst_t *)&cmd_track, + (parse_pgm_inst_t *)&cmd_track_show, + (parse_pgm_inst_t *)&cmd_pt_list, + (parse_pgm_inst_t *)&cmd_pt_list_append, + (parse_pgm_inst_t *)&cmd_pt_list_del, + (parse_pgm_inst_t *)&cmd_pt_list_show, + (parse_pgm_inst_t *)&cmd_goto1, + (parse_pgm_inst_t *)&cmd_goto2, + (parse_pgm_inst_t *)&cmd_position, + (parse_pgm_inst_t *)&cmd_position_set, + NULL, +}; diff --git a/projects/microb2010/tests/test_board2008/commands_ax12.c b/projects/microb2010/tests/test_board2008/commands_ax12.c new file mode 100644 index 0000000..804e084 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/commands_ax12.c @@ -0,0 +1,375 @@ +/* + * 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 + * (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: commands_ax12.c,v 1.3 2009-05-02 10:08:09 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" + +uint8_t addr_from_string(const char *s) +{ + /* 16 bits */ + if (!strcmp_P(s, PSTR("cw_angle_limit"))) + return AA_CW_ANGLE_LIMIT_L; + if (!strcmp_P(s, PSTR("ccw_angle_limit"))) + return AA_CCW_ANGLE_LIMIT_L; + if (!strcmp_P(s, PSTR("max_torque"))) + return AA_MAX_TORQUE_L; + if (!strcmp_P(s, PSTR("down_calibration"))) + return AA_DOWN_CALIBRATION_L; + if (!strcmp_P(s, PSTR("up_calibration"))) + return AA_UP_CALIBRATION_L; + if (!strcmp_P(s, PSTR("torque_limit"))) + return AA_TORQUE_LIMIT_L; + if (!strcmp_P(s, PSTR("position"))) + return AA_PRESENT_POSITION_L; + if (!strcmp_P(s, PSTR("speed"))) + return AA_PRESENT_SPEED_L; + if (!strcmp_P(s, PSTR("load"))) + return AA_PRESENT_LOAD_L; + if (!strcmp_P(s, PSTR("moving_speed"))) + return AA_MOVING_SPEED_L; + if (!strcmp_P(s, PSTR("model"))) + return AA_MODEL_NUMBER_L; + if (!strcmp_P(s, PSTR("goal_pos"))) + return AA_GOAL_POSITION_L; + if (!strcmp_P(s, PSTR("punch"))) + return AA_PUNCH_L; + + /* 8 bits */ + if (!strcmp_P(s, PSTR("firmware"))) + return AA_FIRMWARE; + if (!strcmp_P(s, PSTR("id"))) + return AA_ID; + if (!strcmp_P(s, PSTR("baudrate"))) + return AA_BAUD_RATE; + if (!strcmp_P(s, PSTR("delay"))) + return AA_DELAY_TIME; + if (!strcmp_P(s, PSTR("high_lim_temp"))) + return AA_HIGHEST_LIMIT_TEMP; + if (!strcmp_P(s, PSTR("low_lim_volt"))) + return AA_LOWEST_LIMIT_VOLTAGE; + if (!strcmp_P(s, PSTR("high_lim_volt"))) + return AA_HIGHEST_LIMIT_VOLTAGE; + if (!strcmp_P(s, PSTR("status_return"))) + return AA_STATUS_RETURN_LEVEL; + if (!strcmp_P(s, PSTR("alarm_led"))) + return AA_ALARM_LED; + if (!strcmp_P(s, PSTR("alarm_shutdown"))) + return AA_ALARM_SHUTDOWN; + if (!strcmp_P(s, PSTR("torque_enable"))) + return AA_TORQUE_ENABLE; + if (!strcmp_P(s, PSTR("led"))) + return AA_LED; + if (!strcmp_P(s, PSTR("cw_comp_margin"))) + return AA_CW_COMPLIANCE_MARGIN; + if (!strcmp_P(s, PSTR("ccw_comp_margin"))) + return AA_CCW_COMPLIANCE_MARGIN; + if (!strcmp_P(s, PSTR("cw_comp_slope"))) + return AA_CW_COMPLIANCE_SLOPE; + if (!strcmp_P(s, PSTR("ccw_comp_slope"))) + return AA_CCW_COMPLIANCE_SLOPE; + if (!strcmp_P(s, PSTR("voltage"))) + return AA_PRESENT_VOLTAGE; + if (!strcmp_P(s, PSTR("temp"))) + return AA_PRESENT_TEMP; + if (!strcmp_P(s, PSTR("reginst"))) + return AA_PRESENT_REGINST; + if (!strcmp_P(s, PSTR("moving"))) + return AA_MOVING; + if (!strcmp_P(s, PSTR("lock"))) + return AA_LOCK; + + return 0; +} + +/**********************************************************/ +/* Ax12_Stress */ + +/* this structure is filled when cmd_ax12_stress is parsed successfully */ +struct cmd_ax12_stress_result { + fixed_string_t arg0; + uint8_t id; +}; + +/* function called when cmd_ax12_stress is parsed successfully */ +static void cmd_ax12_stress_parsed(void *parsed_result, void *data) +{ + struct cmd_ax12_stress_result *res = parsed_result; + int i, nb_errs = 0; + uint8_t val; + microseconds t = time_get_us2(); + + for (i=0; i<1000; i++) { + if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0) + nb_errs ++; + } + + printf_P(PSTR("%d errors / 1000\r\n"), nb_errs); + t = (time_get_us2() - t) / 1000; + printf_P(PSTR("Test done in %d ms\r\n"), (int)t); +} + +prog_char str_ax12_stress_arg0[] = "ax12_stress"; +parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0); +parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8); + +prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands"; +parse_pgm_inst_t cmd_ax12_stress = { + .f = cmd_ax12_stress_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ax12_stress, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ax12_stress_arg0, + (prog_void *)&cmd_ax12_stress_id, + NULL, + }, +}; + +/**********************************************************/ + +/* this structure is filled when cmd_baudrate is parsed successfully */ +struct cmd_baudrate_result { + fixed_string_t arg0; + uint32_t arg1; +}; + +/* function called when cmd_baudrate is parsed successfully */ +static void cmd_baudrate_parsed(void * parsed_result, void * data) +{ + struct cmd_baudrate_result *res = parsed_result; + struct uart_config c; + + printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L); + uart_getconf(1, &c); + c.baudrate = res->arg1; + uart_setconf(1, &c); + printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L); +} + +prog_char str_baudrate_arg0[] = "baudrate"; +parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0); +parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32); + +prog_char help_baudrate[] = "Change ax12 baudrate"; +parse_pgm_inst_t cmd_baudrate = { + .f = cmd_baudrate_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_baudrate, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_baudrate_arg0, + (prog_void *)&cmd_baudrate_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Uint16 */ + + +/* this structure is filled when cmd_uint16_read is parsed successfully */ +struct cmd_uint16_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t num; + uint16_t val; +}; + +/* function called when cmd_uint16_read is parsed successfully */ +static void cmd_uint16_read_parsed(void * parsed_result, void * data) +{ + struct cmd_uint16_result *res = parsed_result; + uint8_t ret; + uint16_t val; + uint8_t addr = addr_from_string(res->arg1); + ret = AX12_read_int(&gen.ax12, res->num, addr, &val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); + printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val); +} + +prog_char str_uint16_arg0[] = "read"; +parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0); +prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#" + "max_torque#down_calibration#up_calibration#torque_limit#" + "position#speed#load#punch"; +parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1); +parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8); + +prog_char help_uint16_read[] = "Read uint16 value (type, num)"; +parse_pgm_inst_t cmd_uint16_read = { + .f = cmd_uint16_read_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint16_read, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint16_arg0, + (prog_void *)&cmd_uint16_arg1, + (prog_void *)&cmd_uint16_num, + NULL, + }, +}; + +/* function called when cmd_uint16_write is parsed successfully */ +static void cmd_uint16_write_parsed(void * parsed_result, void * data) +{ + struct cmd_uint16_result *res = parsed_result; + uint8_t ret; + uint8_t addr = addr_from_string(res->arg1); + printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1, + res->val, res->val); + ret = AX12_write_int(&gen.ax12, res->num, addr, res->val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); +} + +prog_char str_uint16_arg0_w[] = "write"; +parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w); +prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#" + "max_torque#torque_limit#punch"; +parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w); +parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16); + +prog_char help_uint16_write[] = "Write uint16 value (write, num, val)"; +parse_pgm_inst_t cmd_uint16_write = { + .f = cmd_uint16_write_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint16_write, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint16_arg0_w, + (prog_void *)&cmd_uint16_arg1_w, + (prog_void *)&cmd_uint16_num, + (prog_void *)&cmd_uint16_val, + NULL, + }, +}; + +/**********************************************************/ +/* Uint8 */ + + +/* this structure is filled when cmd_uint8_read is parsed successfully */ +struct cmd_uint8_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t num; + uint8_t val; +}; + +/* function called when cmd_uint8_read is parsed successfully */ +static void cmd_uint8_read_parsed(void * parsed_result, void * data) +{ + struct cmd_uint8_result *res = parsed_result; + uint8_t ret; + uint8_t val; + uint8_t addr = addr_from_string(res->arg1); + + ret = AX12_read_byte(&gen.ax12, res->num, addr, &val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); + printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val); +} + +prog_char str_uint8_arg0[] = "read"; +parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0); +prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#" + "low_lim_volt#high_lim_volt#status_return#alarm_led#" + "alarm_shutdown#torque_enable#led#cw_comp_margin#" + "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#" + "voltage#temp#reginst#moving#lock"; +parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1); +parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8); + +prog_char help_uint8_read[] = "Read uint8 value (type, num)"; +parse_pgm_inst_t cmd_uint8_read = { + .f = cmd_uint8_read_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint8_read, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint8_arg0, + (prog_void *)&cmd_uint8_arg1, + (prog_void *)&cmd_uint8_num, + NULL, + }, +}; + +/* function called when cmd_uint8_write is parsed successfully */ +static void cmd_uint8_write_parsed(void * parsed_result, void * data) +{ + struct cmd_uint8_result *res = parsed_result; + uint8_t addr = addr_from_string(res->arg1); + uint8_t ret; + printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1, + res->val, res->val); + ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val); + if (ret) + printf_P(PSTR("AX12 error %.2x!\r\n"), ret); +} + +prog_char str_uint8_arg0_w[] = "write"; +parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w); +prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#" + "low_lim_volt#high_lim_volt#status_return#alarm_led#" + "alarm_shutdown#torque_enable#led#cw_comp_margin#" + "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#" + "reginst#lock"; +parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w); +parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8); + +prog_char help_uint8_write[] = "Write uint8 value (write, num, val)"; +parse_pgm_inst_t cmd_uint8_write = { + .f = cmd_uint8_write_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_uint8_write, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_uint8_arg0_w, + (prog_void *)&cmd_uint8_arg1_w, + (prog_void *)&cmd_uint8_num, + (prog_void *)&cmd_uint8_val, + NULL, + }, +}; diff --git a/projects/microb2010/tests/test_board2008/commands_cs.c b/projects/microb2010/tests/test_board2008/commands_cs.c new file mode 100644 index 0000000..7af6205 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/commands_cs.c @@ -0,0 +1,679 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: commands_cs.c,v 1.4 2009-05-02 10:08:09 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cs.h" +#include "cmdline.h" + +struct csb_list { + const prog_char *name; + struct cs_block *csb; +}; + +prog_char csb_angle_str[] = "angle"; +prog_char csb_distance_str[] = "distance"; +prog_char csb_fessor_str[] = "fessor"; +prog_char csb_wheel_str[] = "wheel"; +prog_char csb_elevator_str[] = "elevator"; + +struct csb_list csb_list[] = { + { .name = csb_angle_str, .csb = &mainboard.angle }, + { .name = csb_distance_str, .csb = &mainboard.distance }, + { .name = csb_fessor_str, .csb = &mainboard.fessor }, + { .name = csb_wheel_str, .csb = &mainboard.wheel }, + { .name = csb_elevator_str, .csb = &mainboard.elevator }, +}; + +struct cmd_cs_result { + fixed_string_t cmdname; + fixed_string_t csname; +}; + +/* token to be used for all cs-related commands */ +prog_char str_csb_name[] = "angle#distance#fessor#wheel#elevator"; +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) +{ + int i; + + for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) { + if (!strcmp_P(name, csb_list[i].name)) + return csb_list[i].csb; + } + return NULL; +} + +/**********************************************************/ +/* Gains for control system */ + +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_result { + struct cmd_cs_result cs; + int16_t p; + int16_t i; + int16_t d; +}; + +/* function called when cmd_gain is parsed successfully */ +static void cmd_gain_parsed(void * parsed_result, void *show) +{ + struct cmd_gain_result *res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_gains(&csb->pid, res->p, res->i, res->d); + + printf_P(PSTR("%s %s %d %d %d\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_gain_P(&csb->pid), + pid_get_gain_I(&csb->pid), + pid_get_gain_D(&csb->pid)); +} + +prog_char str_gain_arg0[] = "gain"; +parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0); +parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16); +parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16); +parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16); + +prog_char help_gain[] = "Set gain values for PID"; +parse_pgm_inst_t cmd_gain = { + .f = cmd_gain_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_gain, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_p, + (prog_void *)&cmd_gain_i, + (prog_void *)&cmd_gain_d, + NULL, + }, +}; + +/* show */ +/* this structure is filled when cmd_gain is parsed successfully */ +struct cmd_gain_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_gain_show_arg[] = "show"; +parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg); + +prog_char help_gain_show[] = "Show gain values for PID"; +parse_pgm_inst_t cmd_gain_show = { + .f = cmd_gain_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_gain_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_gain_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_gain_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Speeds for control system */ + +/* this structure is filled when cmd_speed is parsed successfully */ +struct cmd_speed_result { + struct cmd_cs_result cs; + uint16_t s; +}; + +/* function called when cmd_speed is parsed successfully */ +static void cmd_speed_parsed(void *parsed_result, void *show) +{ + struct cmd_speed_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + +#if notyet + if (!show) + ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */ + + printf_P(PSTR("%s %lu\r\n"), + res->cs.csname, + ext.r_b.var_pos); +#else + printf_P(PSTR("TODO\r\n")); +#endif +} + +prog_char str_speed_arg0[] = "speed"; +parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0); +parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16); + +prog_char help_speed[] = "Set speed values for ramp filter"; +parse_pgm_inst_t cmd_speed = { + .f = cmd_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_s, + NULL, + }, +}; + +/* show */ +struct cmd_speed_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg); + +prog_char help_speed_show[] = "Show speed values for ramp filter"; +parse_pgm_inst_t cmd_speed_show = { + .f = cmd_speed_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_speed_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Derivate_Filters for control system */ + +/* this structure is filled when cmd_derivate_filter is parsed successfully */ +struct cmd_derivate_filter_result { + struct cmd_cs_result cs; + uint8_t size; +}; + +/* function called when cmd_derivate_filter is parsed successfully */ +static void cmd_derivate_filter_parsed(void *parsed_result, void *show) +{ + struct cmd_derivate_filter_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_derivate_filter(&csb->pid, res->size); + + printf_P(PSTR("%s %s %u\r\n"), + res->cs.cmdname, + res->cs.csname, + pid_get_derivate_filter(&csb->pid)); +} + +prog_char str_derivate_filter_arg0[] = "derivate_filter"; +parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0); +parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32); + +prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)"; +parse_pgm_inst_t cmd_derivate_filter = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_derivate_filter, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_size, + NULL, + }, +}; + +/* show */ + +struct cmd_derivate_filter_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_derivate_filter_show_arg[] = "show"; +parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg); + +prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID"; +parse_pgm_inst_t cmd_derivate_filter_show = { + .f = cmd_derivate_filter_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_derivate_filter_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_derivate_filter_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_derivate_filter_show_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Consign for control system */ + +/* this structure is filled when cmd_consign is parsed successfully */ +struct cmd_consign_result { + struct cmd_cs_result cs; + int32_t p; +}; + +/* function called when cmd_consign is parsed successfully */ +static void cmd_consign_parsed(void * parsed_result, void *data) +{ + struct cmd_consign_result * res = parsed_result; + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + cs_set_consign(&csb->cs, res->p); +} + +prog_char str_consign_arg0[] = "consign"; +parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0); +parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32); + +prog_char help_consign[] = "Set consign value"; +parse_pgm_inst_t cmd_consign = { + .f = cmd_consign_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_consign, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_consign_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_consign_p, + NULL, + }, +}; + + +/**********************************************************/ +/* Maximums for control system */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_result { + struct cmd_cs_result cs; + uint32_t in; + uint32_t i; + uint32_t out; +}; + +/* function called when cmd_maximum is parsed successfully */ +static void cmd_maximum_parsed(void *parsed_result, void *show) +{ + struct cmd_maximum_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + pid_set_maximums(&csb->pid, res->in, res->i, res->out); + + printf_P(PSTR("maximum %s %lu %lu %lu\r\n"), + res->cs.csname, + pid_get_max_in(&csb->pid), + pid_get_max_I(&csb->pid), + pid_get_max_out(&csb->pid)); +} + +prog_char str_maximum_arg0[] = "maximum"; +parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0); +parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32); +parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32); +parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32); + +prog_char help_maximum[] = "Set maximum values for PID (in, I, out)"; +parse_pgm_inst_t cmd_maximum = { + .f = cmd_maximum_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_maximum, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_in, + (prog_void *)&cmd_maximum_i, + (prog_void *)&cmd_maximum_out, + NULL, + }, +}; + +/* show */ + +/* this structure is filled when cmd_maximum is parsed successfully */ +struct cmd_maximum_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; +prog_char str_maximum_show_arg[] = "show"; +parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg); + +prog_char help_maximum_show[] = "Show maximum values for PID"; +parse_pgm_inst_t cmd_maximum_show = { + .f = cmd_maximum_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_maximum_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_maximum_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_maximum_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* Quadramp for control system */ + +/* this structure is filled when cmd_quadramp is parsed successfully */ +struct cmd_quadramp_result { + struct cmd_cs_result cs; + uint32_t ap; + uint32_t an; + uint32_t sp; + uint32_t sn; +}; + +/* function called when cmd_quadramp is parsed successfully */ +static void cmd_quadramp_parsed(void *parsed_result, void *show) +{ + struct cmd_quadramp_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) { + quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn); + quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an); + } + + printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"), + res->cs.csname, + csb->qr.var_2nd_ord_pos, + csb->qr.var_2nd_ord_neg, + csb->qr.var_1st_ord_pos, + csb->qr.var_1st_ord_neg); +} + +prog_char str_quadramp_arg0[] = "quadramp"; +parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0); +parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32); +parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32); +parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32); +parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32); + +prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)"; +parse_pgm_inst_t cmd_quadramp = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_quadramp, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_ap, + (prog_void *)&cmd_quadramp_an, + (prog_void *)&cmd_quadramp_sp, + (prog_void *)&cmd_quadramp_sn, + + NULL, + }, +}; + +/* show */ + +struct cmd_quadramp_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_quadramp_show_arg[] = "show"; +parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg); + +prog_char help_quadramp_show[] = "Get quadramp values for control system"; +parse_pgm_inst_t cmd_quadramp_show = { + .f = cmd_quadramp_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_quadramp_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_quadramp_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_quadramp_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* cs_status show for control system */ + +/* this structure is filled when cmd_cs_status is parsed successfully */ +struct cmd_cs_status_result { + struct cmd_cs_result cs; + fixed_string_t arg; +}; + +/* function called when cmd_cs_status is parsed successfully */ +static void cmd_cs_status_parsed(void *parsed_result, void *data) +{ + 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); + return; + } + else if (strcmp_P(res->arg, PSTR("off")) == 0) { + csb->on = 0; + printf_P(PSTR("%s is off\r\n"), res->cs.csname); + return; + } + 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 { + 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[] = "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"; +parse_pgm_inst_t cmd_cs_status = { + .f = cmd_cs_status_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_cs_status, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_cs_status_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_cs_status_arg, + NULL, + }, +}; + + +/**********************************************************/ +/* Blocking_I for control system */ + +/* this structure is filled when cmd_blocking_i is parsed successfully */ +struct cmd_blocking_i_result { + struct cmd_cs_result cs; + int32_t k1; + int32_t k2; + uint32_t i; + uint16_t cpt; +}; + +/* function called when cmd_blocking_i is parsed successfully */ +static void cmd_blocking_i_parsed(void *parsed_result, void *show) +{ + struct cmd_blocking_i_result * res = parsed_result; + + struct cs_block *csb; + + csb = cs_from_name(res->cs.csname); + if (csb == NULL) { + printf_P(PSTR("null csb\r\n")); + return; + } + + if (!show) + bd_set_current_thresholds(&csb->bd, res->k1, res->k2, + res->i, res->cpt); + + printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"), + res->cs.cmdname, + res->cs.csname, + csb->bd.k1, + csb->bd.k2, + csb->bd.i_thres, + csb->bd.cpt_thres); +} + +prog_char str_blocking_i_arg0[] = "blocking"; +parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0); +parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32); +parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32); +parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32); +parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16); + +prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)"; +parse_pgm_inst_t cmd_blocking_i = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_blocking_i, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_k1, + (prog_void *)&cmd_blocking_i_k2, + (prog_void *)&cmd_blocking_i_i, + (prog_void *)&cmd_blocking_i_cpt, + NULL, + }, +}; + +/* show */ + +struct cmd_blocking_i_show_result { + struct cmd_cs_result cs; + fixed_string_t show; +}; + +prog_char str_blocking_i_show_arg[] = "show"; +parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg); + +prog_char help_blocking_i_show[] = "Show blocking detection values"; +parse_pgm_inst_t cmd_blocking_i_show = { + .f = cmd_blocking_i_parsed, /* function to call */ + .data = (void *)1, /* 2nd arg of func */ + .help_str = help_blocking_i_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_blocking_i_arg0, + (prog_void *)&cmd_csb_name_tok, + (prog_void *)&cmd_blocking_i_show_arg, + NULL, + }, +}; + + diff --git a/projects/microb2010/tests/test_board2008/commands_gen.c b/projects/microb2010/tests/test_board2008/commands_gen.c new file mode 100644 index 0000000..084d2cb --- /dev/null +++ b/projects/microb2010/tests/test_board2008/commands_gen.c @@ -0,0 +1,612 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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: commands_gen.c,v 1.6 2009-05-02 10:08:09 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "main.h" +#include "cmdline.h" +#include "sensor.h" + +#include "action.h" + +/**********************************************************/ +/* Reset */ + +/* this structure is filled when cmd_reset is parsed successfully */ +struct cmd_reset_result { + fixed_string_t arg0; +}; + +/* function called when cmd_reset is parsed successfully */ +static void cmd_reset_parsed(void * parsed_result, void * data) +{ + reset(); +} + +prog_char str_reset_arg0[] = "reset"; +parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0); + +prog_char help_reset[] = "Reset the board"; +parse_pgm_inst_t cmd_reset = { + .f = cmd_reset_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_reset, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_reset_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Bootloader */ + +/* this structure is filled when cmd_bootloader is parsed successfully */ +struct cmd_bootloader_result { + fixed_string_t arg0; +}; + +/* function called when cmd_bootloader is parsed successfully */ +static void cmd_bootloader_parsed(void *parsed_result, void *data) +{ + //bootloader(); +} + +prog_char str_bootloader_arg0[] = "bootloader"; +parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0); + +prog_char help_bootloader[] = "Launch the bootloader"; +parse_pgm_inst_t cmd_bootloader = { + .f = cmd_bootloader_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_bootloader, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_bootloader_arg0, + NULL, + }, +}; + +/**********************************************************/ +/* Encoders tests */ + +/* this structure is filled when cmd_encoders is parsed successfully */ +struct cmd_encoders_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_encoders is parsed successfully */ +static void cmd_encoders_parsed(void *parsed_result, void *data) +{ + struct cmd_encoders_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + encoders_microb_set_value((void *)0, 0); + encoders_microb_set_value((void *)1, 0); + encoders_microb_set_value((void *)2, 0); + encoders_microb_set_value((void *)3, 0); + return; + } + + /* show */ + while(!cmdline_keypressed()) { + printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"), + encoders_microb_get_value((void *)0), + encoders_microb_get_value((void *)1), + encoders_microb_get_value((void *)2), + encoders_microb_get_value((void *)3)); + wait_ms(100); + } +} + +prog_char str_encoders_arg0[] = "encoders"; +parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0); +prog_char str_encoders_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1); + +prog_char help_encoders[] = "Show encoders values"; +parse_pgm_inst_t cmd_encoders = { + .f = cmd_encoders_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_encoders, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_encoders_arg0, + (prog_void *)&cmd_encoders_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Scheduler show */ + +/* this structure is filled when cmd_scheduler is parsed successfully */ +struct cmd_scheduler_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_scheduler is parsed successfully */ +static void cmd_scheduler_parsed(void *parsed_result, void *data) +{ + scheduler_dump_events(); + scheduler_stats_dump(); +} + +prog_char str_scheduler_arg0[] = "scheduler"; +parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0); +prog_char str_scheduler_arg1[] = "show"; +parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1); + +prog_char help_scheduler[] = "Show scheduler events"; +parse_pgm_inst_t cmd_scheduler = { + .f = cmd_scheduler_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_scheduler, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_scheduler_arg0, + (prog_void *)&cmd_scheduler_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Pwms tests */ + +/* this structure is filled when cmd_pwm is parsed successfully */ +struct cmd_pwm_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; +}; + +/* function called when cmd_pwm is parsed successfully */ +static void cmd_pwm_parsed(void * parsed_result, void * data) +{ + void * pwm_ptr = NULL; + struct cmd_pwm_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("1(1A)"))) + pwm_ptr = &gen.pwm1_1A; + else if (!strcmp_P(res->arg1, PSTR("2(1B)"))) + pwm_ptr = &gen.pwm2_1B; + else if (!strcmp_P(res->arg1, PSTR("3(3A)"))) + pwm_ptr = &gen.pwm3_3A; + else if (!strcmp_P(res->arg1, PSTR("4(3B)"))) + pwm_ptr = &gen.pwm4_3B; + + if (pwm_ptr) + pwm_ng_set(pwm_ptr, res->arg2); + + printf_P(PSTR("done %p\r\n"), pwm_ptr); +} + +prog_char str_pwm_arg0[] = "pwm"; +parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0); +prog_char str_pwm_arg1[] = "1(1A)#2(1B)#3(3A)#4(3B)"; +parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1); +parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16); + +prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]"; +parse_pgm_inst_t cmd_pwm = { + .f = cmd_pwm_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pwm, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pwm_arg0, + (prog_void *)&cmd_pwm_arg1, + (prog_void *)&cmd_pwm_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Adcs tests */ + +/* this structure is filled when cmd_adc is parsed successfully */ +struct cmd_adc_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_adc is parsed successfully */ +static void cmd_adc_parsed(void *parsed_result, void *data) +{ + struct cmd_adc_result *res = parsed_result; + uint8_t i, loop = 0; + + if (!strcmp_P(res->arg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("ADC values: ")); + for (i=0; iarg1, PSTR("loop_show"))) + loop = 1; + + do { + printf_P(PSTR("SENSOR values: ")); + for (i=0; iarg1, PSTR("level"))) { + gen.log_level = res->arg2; + } + + /* else it is a show */ + cmd_log_do_show(); +} + +prog_char str_log_arg0[] = "log"; +parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0); +prog_char str_log_arg1[] = "level"; +parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1); +parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32); + +prog_char help_log[] = "Set log options: level (0 -> 5)"; +parse_pgm_inst_t cmd_log = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1, + (prog_void *)&cmd_log_arg2, + NULL, + }, +}; + +prog_char str_log_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show); + +prog_char help_log_show[] = "Show configured logs"; +parse_pgm_inst_t cmd_log_show = { + .f = cmd_log_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_log_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_log_arg0, + (prog_void *)&cmd_log_arg1_show, + NULL, + }, +}; + +/* this structure is filled when cmd_log is parsed successfully */ +struct cmd_log_type_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; + fixed_string_t arg3; +}; + +/* function called when cmd_log is parsed successfully */ +static void cmd_log_type_parsed(void * parsed_result, void * data) +{ + struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result; + uint8_t lognum; + uint8_t i; + + lognum = log_name2num(res->arg2); + if (lognum == 0) { + printf_P(PSTR("Cannot find log num\r\n")); + return; + } + + if (!strcmp_P(res->arg3, PSTR("on"))) { + for (i=0; iarg3, PSTR("off"))) { + for (i=0; inum, res->us); +} + +prog_char str_servo_arg0[] = "servo"; +parse_pgm_token_string_t cmd_servo_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg0, str_servo_arg0); +parse_pgm_token_num_t cmd_servo_num = TOKEN_NUM_INITIALIZER(struct cmd_servo_result, num, UINT8); +parse_pgm_token_num_t cmd_servo_us = TOKEN_NUM_INITIALIZER(struct cmd_servo_result, us, UINT16); + +prog_char help_servo[] = "Set servo position/speed"; +parse_pgm_inst_t cmd_servo = { + .f = cmd_servo_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_servo, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_servo_arg0, + (prog_void *)&cmd_servo_num, + (prog_void *)&cmd_servo_us, + NULL, + }, +}; +#endif diff --git a/projects/microb2010/tests/test_board2008/commands_mainboard.c b/projects/microb2010/tests/test_board2008/commands_mainboard.c new file mode 100644 index 0000000..de3e936 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/commands_mainboard.c @@ -0,0 +1,653 @@ +/* + * 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 + * (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: commands_mainboard.c,v 1.7 2009-05-02 10:08:09 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "sensor.h" +#include "cmdline.h" +#include "actuator.h" +#include "strat_base.h" + +struct cmd_event_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + + +/* function called when cmd_event is parsed successfully */ +static void cmd_event_parsed(void *parsed_result, void *data) +{ + u08 bit=0; + + 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"))) { + strat_hardstop(); + bit = DO_CS; + } + else if (!strcmp_P(res->arg1, PSTR("rs"))) + bit = DO_RS; + else if (!strcmp_P(res->arg1, PSTR("pos"))) + bit = DO_POS; + else if (!strcmp_P(res->arg1, PSTR("bd"))) + bit = DO_BD; + else if (!strcmp_P(res->arg1, PSTR("timer"))) { + time_reset(); + bit = DO_TIMER; + } + else if (!strcmp_P(res->arg1, PSTR("power"))) + bit = DO_POWER; + + if (!strcmp_P(res->arg2, PSTR("on"))) + mainboard.flags |= bit; + else if (!strcmp_P(res->arg2, PSTR("off"))) { + if (!strcmp_P(res->arg1, PSTR("cs"))) { + pwm_ng_set(LEFT_PWM, 0); + pwm_ng_set(RIGHT_PWM, 0); + } + mainboard.flags &= (~bit); + } + printf_P(PSTR("%s is %s\r\n"), res->arg1, + (bit & mainboard.flags) ? "on":"off"); +} + +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[] = "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); + +prog_char help_event[] = "Enable/disable events"; +parse_pgm_inst_t cmd_event = { + .f = cmd_event_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_event, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Interact */ + +/* this structure is filled when cmd_interact is parsed successfully */ +struct cmd_interact_result { + fixed_string_t arg0; +}; + +static void print_cs(void) +{ + printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld " + "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"), + cs_get_consign(&mainboard.distance.cs), + cs_get_consign(&mainboard.angle.cs), + cs_get_filtered_consign(&mainboard.distance.cs), + cs_get_filtered_consign(&mainboard.angle.cs), + cs_get_error(&mainboard.distance.cs), + cs_get_error(&mainboard.angle.cs), + cs_get_out(&mainboard.distance.cs), + cs_get_out(&mainboard.angle.cs)); +} + +static void print_pos(void) +{ + printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); +} + +static void print_time(void) +{ + printf_P(PSTR("time %d\r\n"),time_get_s()); +} + + +static void print_sensors(void) +{ +#ifdef notyet + if (sensor_start_switch()) + printf_P(PSTR("Start switch | ")); + else + printf_P(PSTR(" | ")); + + if (IR_DISP_SENSOR()) + printf_P(PSTR("IR disp | ")); + else + printf_P(PSTR(" | ")); + + printf_P(PSTR("\r\n")); +#endif +} + +static void print_pid(void) +{ + printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | " + "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid), + pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid), + pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid), + pid_get_value_out(&mainboard.distance.pid), + pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid), + pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid), + pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid), + pid_get_value_out(&mainboard.angle.pid)); +} + +#define PRINT_POS (1<<0) +#define PRINT_PID (1<<1) +#define PRINT_CS (1<<2) +#define PRINT_SENSORS (1<<3) +#define PRINT_TIME (1<<4) +#define PRINT_BLOCKING (1<<5) + +static void cmd_interact_parsed(void * parsed_result, void * data) +{ + int c; + int8_t cmd; + uint8_t print = 0; + struct vt100 vt100; + + vt100_init(&vt100); + + printf_P(PSTR("Display debugs:\r\n" + " 1:pos\r\n" + " 2:pid\r\n" + " 3:cs\r\n" + " 4:sensors\r\n" + " 5:time\r\n" + /* " 6:blocking\r\n" */ + "Commands:\r\n" + " arrows:move\r\n" + " space:stop\r\n" + " q:quit\r\n")); + + /* stop motors */ + mainboard.flags &= (~DO_CS); + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + + while(1) { + if (print & PRINT_POS) { + print_pos(); + } + + if (print & PRINT_PID) { + print_pid(); + } + + if (print & PRINT_CS) { + print_cs(); + } + + if (print & PRINT_SENSORS) { + print_sensors(); + } + + if (print & PRINT_TIME) { + print_time(); + } +/* if (print & PRINT_BLOCKING) { */ +/* printf_P(PSTR("%s %s blocking=%d\r\n"), */ +/* mainboard.blocking ? "BLOCK1":" ", */ +/* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */ +/* rs_get_blocking(&mainboard.rs)); */ +/* } */ + + c = cmdline_getchar(); + if (c == -1) { + wait_ms(10); + continue; + } + cmd = vt100_parser(&vt100, c); + if (cmd == -2) { + wait_ms(10); + continue; + } + + if (cmd == -1) { + switch(c) { + case '1': print ^= PRINT_POS; break; + case '2': print ^= PRINT_PID; break; + case '3': print ^= PRINT_CS; break; + case '4': print ^= PRINT_SENSORS; break; + case '5': print ^= PRINT_TIME; break; + case '6': print ^= PRINT_BLOCKING; break; + + case 'q': +/* if (mainboard.flags & DO_CS) */ +/* strat_hardstop(); */ + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + return; + case ' ': + pwm_set_and_save(LEFT_PWM, 0); + pwm_set_and_save(RIGHT_PWM, 0); + break; + default: + break; + } + } + else { + switch(cmd) { + case KEY_UP_ARR: + pwm_set_and_save(LEFT_PWM, 1200); + pwm_set_and_save(RIGHT_PWM, 1200); + break; + case KEY_LEFT_ARR: + pwm_set_and_save(LEFT_PWM, -1200); + pwm_set_and_save(RIGHT_PWM, 1200); + break; + case KEY_DOWN_ARR: + pwm_set_and_save(LEFT_PWM, -1200); + pwm_set_and_save(RIGHT_PWM, -1200); + break; + case KEY_RIGHT_ARR: + pwm_set_and_save(LEFT_PWM, 1200); + pwm_set_and_save(RIGHT_PWM, -1200); + break; + } + } + wait_ms(10); + } +} + +prog_char str_interact_arg0[] = "interact"; +parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0); + +prog_char help_interact[] = "Interactive mode"; +parse_pgm_inst_t cmd_interact = { + .f = cmd_interact_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_interact, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_interact_arg0, + NULL, + }, +}; + + +/**********************************************************/ +/* Rs tests */ + +/* 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_rs is parsed successfully */ +static void cmd_rs_parsed(void *parsed_result, void *data) +{ + // 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_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_rs, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_arg0, + (prog_void *)&cmd_rs_arg1, + NULL, + }, +}; + + +/**********************************************************/ +/* Fessors tests */ + +/* this structure is filled when cmd_fessor is parsed successfully */ +struct cmd_fessor_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_fessor is parsed successfully */ +static void cmd_fessor_parsed(void * parsed_result, void * data) +{ + struct cmd_fessor_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("up"))) { + fessor_up(); + } + else if (!strcmp_P(res->arg1, PSTR("down"))) { + fessor_down(); + } + else if (!strcmp_P(res->arg1, PSTR("stop"))) { + fessor_stop(); + } + else if (!strcmp_P(res->arg1, PSTR("auto"))) { + fessor_auto(); + } + + printf_P(PSTR("done\r\n")); +} + +prog_char str_fessor_arg0[] = "fessor"; +parse_pgm_token_string_t cmd_fessor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_result, arg0, str_fessor_arg0); +prog_char str_fessor_arg1[] = "auto#up#down#stop"; +parse_pgm_token_string_t cmd_fessor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_result, arg1, str_fessor_arg1); + +prog_char help_fessor[] = "fessor auto mode: fessor auto delay_up delay_down"; +parse_pgm_inst_t cmd_fessor = { + .f = cmd_fessor_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_fessor, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_fessor_arg0, + (prog_void *)&cmd_fessor_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Fessor_Paramss tests */ + +/* this structure is filled when cmd_fessor_params is parsed successfully */ +struct cmd_fessor_params_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; +}; + +/* function called when cmd_fessor_params is parsed successfully */ +static void cmd_fessor_params_parsed(void * parsed_result, void * data) +{ + struct cmd_fessor_params_result * res = parsed_result; + + + if (!strcmp_P(res->arg1, PSTR("delay"))) { + fessor_set_delays(res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("coef"))) { + fessor_set_coefs(res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("pos"))) { + fessor_set_pos(res->arg2, res->arg3); + } + + /* else show */ + fessor_dump_params(); +} + +prog_char str_fessor_params_arg0[] = "fessor_params"; +parse_pgm_token_string_t cmd_fessor_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg0, str_fessor_params_arg0); +prog_char str_fessor_params_arg1[] = "delay#pos#coef"; +parse_pgm_token_string_t cmd_fessor_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg1, str_fessor_params_arg1); +parse_pgm_token_num_t cmd_fessor_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_fessor_params_result, arg2, INT32); +parse_pgm_token_num_t cmd_fessor_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_fessor_params_result, arg3, INT32); + +prog_char help_fessor_params[] = "Set fessor_params values"; +parse_pgm_inst_t cmd_fessor_params = { + .f = cmd_fessor_params_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_fessor_params, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_fessor_params_arg0, + (prog_void *)&cmd_fessor_params_arg1, + (prog_void *)&cmd_fessor_params_arg2, + (prog_void *)&cmd_fessor_params_arg3, + NULL, + }, +}; + +prog_char str_fessor_params_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_fessor_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg1, str_fessor_params_arg1_show); + +prog_char help_fessor_params_show[] = "show fessor params"; +parse_pgm_inst_t cmd_fessor_params_show = { + .f = cmd_fessor_params_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_fessor_params_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_fessor_params_arg0, + (prog_void *)&cmd_fessor_params_arg1_show, + NULL, + }, +}; + + + + +/**********************************************************/ +/* Elevators tests */ + +/* this structure is filled when cmd_elevator is parsed successfully */ +struct cmd_elevator_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_elevator is parsed successfully */ +static void cmd_elevator_parsed(void * parsed_result, void * data) +{ + struct cmd_elevator_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("up"))) { + elevator_up(); + } + else if (!strcmp_P(res->arg1, PSTR("down"))) { + elevator_down(); + } + else if (!strcmp_P(res->arg1, PSTR("stop"))) { + elevator_stop(); + } + else if (!strcmp_P(res->arg1, PSTR("auto"))) { + elevator_auto(); + } + + printf_P(PSTR("done\r\n")); +} + +prog_char str_elevator_arg0[] = "elevator"; +parse_pgm_token_string_t cmd_elevator_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_result, arg0, str_elevator_arg0); +prog_char str_elevator_arg1[] = "auto#up#down#stop"; +parse_pgm_token_string_t cmd_elevator_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_result, arg1, str_elevator_arg1); + +prog_char help_elevator[] = "elevator auto mode: elevator auto delay_up delay_down"; +parse_pgm_inst_t cmd_elevator = { + .f = cmd_elevator_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_elevator, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_elevator_arg0, + (prog_void *)&cmd_elevator_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Elevator_Paramss tests */ + +/* this structure is filled when cmd_elevator_params is parsed successfully */ +struct cmd_elevator_params_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; +}; + +/* function called when cmd_elevator_params is parsed successfully */ +static void cmd_elevator_params_parsed(void * parsed_result, void * data) +{ + struct cmd_elevator_params_result * res = parsed_result; + + + if (!strcmp_P(res->arg1, PSTR("delay"))) { + elevator_set_delays(res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("coef"))) { + elevator_set_coefs(res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("pos"))) { + elevator_set_pos(res->arg2, res->arg3); + } + + /* else show */ + elevator_dump_params(); +} + +prog_char str_elevator_params_arg0[] = "elevator_params"; +parse_pgm_token_string_t cmd_elevator_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg0, str_elevator_params_arg0); +prog_char str_elevator_params_arg1[] = "delay#pos#coef"; +parse_pgm_token_string_t cmd_elevator_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg1, str_elevator_params_arg1); +parse_pgm_token_num_t cmd_elevator_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_elevator_params_result, arg2, INT32); +parse_pgm_token_num_t cmd_elevator_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_elevator_params_result, arg3, INT32); + +prog_char help_elevator_params[] = "Set elevator_params values"; +parse_pgm_inst_t cmd_elevator_params = { + .f = cmd_elevator_params_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_elevator_params, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_elevator_params_arg0, + (prog_void *)&cmd_elevator_params_arg1, + (prog_void *)&cmd_elevator_params_arg2, + (prog_void *)&cmd_elevator_params_arg3, + NULL, + }, +}; + +prog_char str_elevator_params_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_elevator_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg1, str_elevator_params_arg1_show); + +prog_char help_elevator_params_show[] = "show elevator params"; +parse_pgm_inst_t cmd_elevator_params_show = { + .f = cmd_elevator_params_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_elevator_params_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_elevator_params_arg0, + (prog_void *)&cmd_elevator_params_arg1_show, + NULL, + }, +}; + + + + + + +/**********************************************************/ +/* Test */ + +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { + fixed_string_t arg0; +}; + +/* function called when cmd_test is parsed successfully */ +static void cmd_test_parsed(void *parsed_result, void *data) +{ +} + +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); + +prog_char help_test[] = "Test function"; +parse_pgm_inst_t cmd_test = { + .f = cmd_test_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_test, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_test_arg0, + NULL, + }, +}; diff --git a/projects/microb2010/tests/test_board2008/commands_traj.c b/projects/microb2010/tests/test_board2008/commands_traj.c new file mode 100644 index 0000000..691b8c0 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/commands_traj.c @@ -0,0 +1,720 @@ +/* + * 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 + * (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: commands_traj.c,v 1.6 2009-05-02 10:08:09 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "cs.h" +#include "cmdline.h" + +/**********************************************************/ +/* Traj_Speeds for trajectory_manager */ + +/* this structure is filled when cmd_traj_speed is parsed successfully */ +struct cmd_traj_speed_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t s; +}; + +/* function called when cmd_traj_speed is parsed successfully */ +static void cmd_traj_speed_parsed(void *parsed_result, void *data) +{ + struct cmd_traj_speed_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("angle"))) { + trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s); + } + else if (!strcmp_P(res->arg1, PSTR("distance"))) { + trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed); + } + /* else it is a "show" */ + + printf_P(PSTR("angle %u, distance %u\r\n"), + mainboard.traj.a_speed, + mainboard.traj.d_speed); +} + +prog_char str_traj_speed_arg0[] = "traj_speed"; +parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0); +prog_char str_traj_speed_arg1[] = "angle#distance"; +parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1); +parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16); + +prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager"; +parse_pgm_inst_t cmd_traj_speed = { + .f = cmd_traj_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg1, + (prog_void *)&cmd_traj_speed_s, + NULL, + }, +}; + +/* show */ + +prog_char str_traj_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg); + +prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager"; +parse_pgm_inst_t cmd_traj_speed_show = { + .f = cmd_traj_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* trajectory window configuration */ + +/* this structure is filled when cmd_trajectory is parsed successfully */ +struct cmd_trajectory_result { + fixed_string_t arg0; + fixed_string_t arg1; + float d_win; + float a_win; + float a_start; +}; + + +/* function called when cmd_trajectory is parsed successfully */ +static void cmd_trajectory_parsed(void * parsed_result, void * data) +{ + struct cmd_trajectory_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + trajectory_set_windows(&mainboard.traj, res->d_win, + res->a_win, res->a_start); + } + + printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win, + DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad)); +} + +prog_char str_trajectory_arg0[] = "trajectory"; +parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0); +prog_char str_trajectory_arg1[] = "set"; +parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1); +parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT); +parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT); +parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT); + +prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)"; +parse_pgm_inst_t cmd_trajectory = { + .f = cmd_trajectory_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_trajectory, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg1, + (prog_void *)&cmd_trajectory_d, + (prog_void *)&cmd_trajectory_a, + (prog_void *)&cmd_trajectory_as, + NULL, + }, +}; + +/* show */ + +prog_char str_trajectory_show_arg[] = "show"; +parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg); + +prog_char help_trajectory_show[] = "Show trajectory window configuration"; +parse_pgm_inst_t cmd_trajectory_show = { + .f = cmd_trajectory_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_trajectory_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* rs_gains configuration */ + +/* this structure is filled when cmd_rs_gains is parsed successfully */ +struct cmd_rs_gains_result { + fixed_string_t arg0; + fixed_string_t arg1; + float left; + float right; +}; + +/* function called when cmd_rs_gains is parsed successfully */ +static void cmd_rs_gains_parsed(void * parsed_result, void * data) +{ + struct cmd_rs_gains_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value, + LEFT_ENCODER, res->left); // en augmentant on tourne à gauche + rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value, + RIGHT_ENCODER, res->right); //en augmentant on tourne à droite + } + printf_P(PSTR("rs_gains set %f %f"), + mainboard.rs.left_ext_gain, + mainboard.rs.right_ext_gain); +/* f64_print(mainboard.rs.left_ext_gain); */ +/* printf_P(PSTR(" ")); */ +/* f64_print(mainboard.rs.right_ext_gain); */ + printf_P(PSTR("\r\n")); +} + +prog_char str_rs_gains_arg0[] = "rs_gains"; +parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0); +prog_char str_rs_gains_arg1[] = "set"; +parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1); +parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT); +parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT); + +prog_char help_rs_gains[] = "Set rs_gains (left, right)"; +parse_pgm_inst_t cmd_rs_gains = { + .f = cmd_rs_gains_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs_gains, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg1, + (prog_void *)&cmd_rs_gains_l, + (prog_void *)&cmd_rs_gains_r, + NULL, + }, +}; + +/* show */ + +prog_char str_rs_gains_show_arg[] = "show"; +parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg); + +prog_char help_rs_gains_show[] = "Show rs_gains"; +parse_pgm_inst_t cmd_rs_gains_show = { + .f = cmd_rs_gains_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs_gains_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* track configuration */ + +/* this structure is filled when cmd_track is parsed successfully */ +struct cmd_track_result { + fixed_string_t arg0; + fixed_string_t arg1; + float val; +}; + +/* function called when cmd_track is parsed successfully */ +static void cmd_track_parsed(void * parsed_result, void * data) +{ + struct cmd_track_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM); + } + printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm); +} + +prog_char str_track_arg0[] = "track"; +parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0); +prog_char str_track_arg1[] = "set"; +parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1); +parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT); + +prog_char help_track[] = "Set track in mm"; +parse_pgm_inst_t cmd_track = { + .f = cmd_track_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_track, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg1, + (prog_void *)&cmd_track_val, + NULL, + }, +}; + +/* show */ + +prog_char str_track_show_arg[] = "show"; +parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg); + +prog_char help_track_show[] = "Show track"; +parse_pgm_inst_t cmd_track_show = { + .f = cmd_track_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_track_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* Pt_Lists for testing traj */ + +#define PT_LIST_SIZE 10 +static struct xy_point pt_list[PT_LIST_SIZE]; +static uint16_t pt_list_len = 0; + +/* this structure is filled when cmd_pt_list is parsed successfully */ +struct cmd_pt_list_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t arg2; + int16_t arg3; + int16_t arg4; +}; + +/* function called when cmd_pt_list is parsed successfully */ +static void cmd_pt_list_parsed(void * parsed_result, void * data) +{ + struct cmd_pt_list_result * res = parsed_result; + uint8_t i, why=0; + + if (!strcmp_P(res->arg1, PSTR("append"))) { + res->arg2 = pt_list_len; + } + + if (!strcmp_P(res->arg1, PSTR("insert")) || + !strcmp_P(res->arg1, PSTR("append"))) { + if (res->arg2 > pt_list_len) { + printf_P(PSTR("Index too large\r\n")); + return; + } + if (pt_list_len >= PT_LIST_SIZE) { + printf_P(PSTR("List is too large\r\n")); + return; + } + memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], + PT_LIST_SIZE-1-res->arg2); + pt_list[res->arg2].x = res->arg3; + pt_list[res->arg2].y = res->arg4; + pt_list_len++; + } + else if (!strcmp_P(res->arg1, PSTR("del"))) { + if (pt_list_len <= 0) { + printf_P(PSTR("Error: list empty\r\n")); + return; + } + if (res->arg2 > pt_list_len) { + printf_P(PSTR("Index too large\r\n")); + return; + } + memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], + (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point)); + pt_list_len--; + } + else if (!strcmp_P(res->arg1, PSTR("reset"))) { + pt_list_len = 0; + } + + /* else it is a "show" or a "start" */ + if (pt_list_len == 0) { + printf_P(PSTR("List empty\r\n")); + return; + } + for (i=0 ; iarg1, PSTR("start"))) { + trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y); + why = wait_traj_end(0xFF); /* all */ + } + else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { + while (1) { + //why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF); + printf("next point\r\n"); + if (why != END_OBSTACLE) + break; + } + } + if (why & (~(END_TRAJ | END_NEAR))) + trajectory_stop(&mainboard.traj); + } +} + +prog_char str_pt_list_arg0[] = "pt_list"; +parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0); +prog_char str_pt_list_arg1[] = "insert"; +parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1); +parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16); +parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16); +parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16); + +prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)"; +parse_pgm_inst_t cmd_pt_list = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1, + (prog_void *)&cmd_pt_list_arg2, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, + NULL, + }, +}; + +/* append */ + +prog_char str_pt_list_arg1_append[] = "append"; +parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append); + +prog_char help_pt_list_append[] = "Append point in pt_list (x,y)"; +parse_pgm_inst_t cmd_pt_list_append = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_append, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1_append, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, + NULL, + }, +}; + +/* del */ + +prog_char str_pt_list_del_arg[] = "del"; +parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg); + +prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)"; +parse_pgm_inst_t cmd_pt_list_del = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_del, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_del_arg, + (prog_void *)&cmd_pt_list_arg2, + NULL, + }, +}; +/* show */ + +prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start"; +parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg); + +prog_char help_pt_list_show[] = "Show, start or reset pt_list"; +parse_pgm_inst_t cmd_pt_list_show = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* Goto function */ + +/* this structure is filled when cmd_goto is parsed successfully */ +struct cmd_goto_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; +}; + +/* function called when cmd_goto is parsed successfully */ +static void cmd_goto_parsed(void * parsed_result, void * data) +{ + struct cmd_goto_result * res = parsed_result; + uint8_t err; + microseconds t1, t2; + + interrupt_traj_reset(); + if (!strcmp_P(res->arg1, PSTR("a_rel"))) { + trajectory_a_rel(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("d_rel"))) { + trajectory_d_rel(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("a_abs"))) { + trajectory_a_abs(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) { + trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) { + trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) { + trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) { + trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("avoid"))) { + //err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF); + err=0; + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + } + else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) { + //err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF); + err=0; + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) { + trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) { + trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("da_rel"))) { + trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3); + } + t1 = time_get_us2(); + while ((err = test_traj_end(0xFF)) == 0) { + t2 = time_get_us2(); + if (t2 - t1 > 200000) { + dump_cs_debug("angle", &mainboard.angle.cs); + dump_cs_debug("distance", &mainboard.distance.cs); + t1 = t2; + } + } + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + printf_P(PSTR("returned %s\r\n"), get_err(err)); +} + +prog_char str_goto_arg0[] = "goto"; +parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0); +prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs"; +parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a); +parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32); + +/* 1 params */ +prog_char help_goto1[] = "Change orientation of the mainboard"; +parse_pgm_inst_t cmd_goto1 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_a, + (prog_void *)&cmd_goto_arg2, + NULL, + }, +}; + +prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#xy_abs_fow#xy_abs_back#da_rel#a_to_xy#avoid#avoid_bw#a_behind_xy"; +parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b); +parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32); + +/* 2 params */ +prog_char help_goto2[] = "Go to a (x,y) or (d,a) position"; +parse_pgm_inst_t cmd_goto2 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_b, + (prog_void *)&cmd_goto_arg2, + (prog_void *)&cmd_goto_arg3, + NULL, + }, +}; + +/**********************************************************/ +/* Position tests */ + +/* this structure is filled when cmd_position is parsed successfully */ +struct cmd_position_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; +}; + +#define AUTOPOS_SPEED_FAST 200 +static void auto_position(void) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + + interrupt_traj_reset(); + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); + + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + if (err == END_INTR) + goto intr; + wait_ms(100); + strat_reset_pos(ROBOT_LENGTH/2, 0, 0); + + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + + trajectory_a_rel(&mainboard.traj, 90); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + if (err == END_INTR) + goto intr; + wait_ms(100); + strat_reset_pos(DO_NOT_SET_POS, ROBOT_LENGTH/2, + 90); + + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + wait_ms(100); + + trajectory_a_rel(&mainboard.traj, -40); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + wait_ms(100); + + strat_set_speed(old_spdd, old_spda); + return; + +intr: + strat_hardstop(); + strat_set_speed(old_spdd, old_spda); +} + +/* function called when cmd_position is parsed successfully */ +static void cmd_position_parsed(void * parsed_result, void * data) +{ + struct cmd_position_result * res = parsed_result; + + /* display raw position values */ + if (!strcmp_P(res->arg1, PSTR("reset"))) { + position_set(&mainboard.pos, 0, 0, 0); + } + else if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) { + auto_position(); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { + auto_position(); + } + + /* else it's just a "show" */ + printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), + position_get_x_double(&mainboard.pos), + position_get_y_double(&mainboard.pos), + DEG(position_get_a_rad_double(&mainboard.pos))); +} + +prog_char str_position_arg0[] = "position"; +parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0); +prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red"; +parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1); + +prog_char help_position[] = "Show/reset (x,y,a) position"; +parse_pgm_inst_t cmd_position = { + .f = cmd_position_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_position, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1, + NULL, + }, +}; + + +prog_char str_position_arg1_set[] = "set"; +parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set); +parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32); +parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32); +parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32); + +prog_char help_position_set[] = "Set (x,y,a) position"; +parse_pgm_inst_t cmd_position_set = { + .f = cmd_position_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_position_set, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1_set, + (prog_void *)&cmd_position_arg2, + (prog_void *)&cmd_position_arg3, + (prog_void *)&cmd_position_arg4, + NULL, + }, +}; + diff --git a/projects/microb2010/tests/test_board2008/cs.c b/projects/microb2010/tests/test_board2008/cs.c new file mode 100644 index 0000000..9736646 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/cs.c @@ -0,0 +1,315 @@ +/* + * 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 + * (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: cs.c,v 1.7 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "actuator.h" + +static int32_t wheel_speed = 0; + +static int32_t wheel_get_value(void * dummy) +{ + return wheel_speed; +} + +static void wheel_set(void *dummy, int32_t val) +{ + if (val < 0) + val = 0; + pwm_ng_set(WHEEL_PWM, val); +} + +static void wheel_update_value(void) +{ + static int32_t prev = 0; + int32_t val; + + val = encoders_microb_get_value(WHEEL_ENC); + wheel_speed = val - prev; + prev = val; +} + +/* called every 5 ms */ +static void do_cs(void *dummy) +{ + static uint16_t cpt = 0; + static int32_t old_a = 0, old_d = 0; + + wheel_update_value(); + + /* robot system, conversion to angle,distance */ + if (mainboard.flags & DO_RS) { + int16_t a,d; + rs_update(&mainboard.rs); /* takes about 0.5 ms */ + /* process and store current speed */ + a = rs_get_angle(&mainboard.rs); + d = rs_get_distance(&mainboard.rs); + mainboard.speed_a = a - old_a; + mainboard.speed_d = d - old_d; + old_a = a; + old_d = d; + } + + /* control system */ + if (mainboard.flags & DO_CS) { + if (mainboard.angle.on) + cs_manage(&mainboard.angle.cs); + if (mainboard.distance.on) + cs_manage(&mainboard.distance.cs); + if (mainboard.fessor.on) + cs_manage(&mainboard.fessor.cs); + if (mainboard.wheel.on) + cs_manage(&mainboard.wheel.cs); + if (mainboard.elevator.on) + cs_manage(&mainboard.elevator.cs); + + fessor_manage(); + elevator_manage(); + } + if ((cpt & 1) && (mainboard.flags & DO_POS)) { + /* about 1.5ms (worst case without centrifugal force + * compensation) */ + position_manage(&mainboard.pos); + } + if (mainboard.flags & DO_BD) { + bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); + bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); + } + if (mainboard.flags & DO_TIMER) { + uint8_t second; + /* the robot should stop correctly in the strat, but + * in some cases, we must force the stop from an + * interrupt */ + second = time_get_s(); + if (second >= MATCH_TIME + 2) { + pwm_ng_set(LEFT_PWM, 0); + pwm_ng_set(RIGHT_PWM, 0); + printf_P(PSTR("END OF TIME\r\n")); + while(1); + } + } + /* brakes */ + if (mainboard.flags & DO_POWER) + BRAKE_OFF(); + else + BRAKE_ON(); + cpt++; +} + +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", + 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_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 */ + rs_init(&mainboard.rs); + rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM); + 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_microb_get_value, + LEFT_ENCODER, IMP_COEF * -1.0000); + rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value, + RIGHT_ENCODER, IMP_COEF * 1.0000); + /* rs will use external encoders */ + rs_set_flags(&mainboard.rs, RS_USE_EXT); + + /* POSITION MANAGER */ + position_init(&mainboard.pos); + position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM); + position_set_related_robot_system(&mainboard.pos, &mainboard.rs); + //position_set_centrifugal_coef(&mainboard.pos, 0.000016); + position_use_ext(&mainboard.pos); + + /* TRAJECTORY MANAGER */ + trajectory_init(&mainboard.traj); + 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, 1500, 1500); /* d, a */ + /* distance window, angle window, angle start */ + trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.); + + /* ---- CS angle */ + /* PID */ + pid_init(&mainboard.angle.pid); + pid_set_gains(&mainboard.angle.pid, 500, 10, 7000); + pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095); + pid_set_out_shift(&mainboard.angle.pid, 10); + pid_set_derivate_filter(&mainboard.angle.pid, 4); + + /* QUADRAMP */ + quadramp_init(&mainboard.angle.qr); + quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */ + + /* CS */ + cs_init(&mainboard.angle.cs); + cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr); + cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid); + cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs); + cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs); + cs_set_consign(&mainboard.angle.cs, 0); + + /* Blocking detection */ + bd_init(&mainboard.angle.bd); + bd_set_speed_threshold(&mainboard.angle.bd, 80); + bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50); + + /* ---- CS distance */ + /* PID */ + pid_init(&mainboard.distance.pid); + pid_set_gains(&mainboard.distance.pid, 500, 10, 7000); + pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095); + pid_set_out_shift(&mainboard.distance.pid, 10); + pid_set_derivate_filter(&mainboard.distance.pid, 6); + + /* QUADRAMP */ + quadramp_init(&mainboard.distance.qr); + quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */ + quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */ + + /* CS */ + cs_init(&mainboard.distance.cs); + cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr); + cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid); + cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs); + cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs); + cs_set_consign(&mainboard.distance.cs, 0); + + /* Blocking detection */ + bd_init(&mainboard.distance.bd); + bd_set_speed_threshold(&mainboard.distance.bd, 60); + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50); + + /* ---- CS fessor */ + + fessor_autopos(); + /* PID */ + pid_init(&mainboard.fessor.pid); + pid_set_gains(&mainboard.fessor.pid, 300, 10, 150); + pid_set_maximums(&mainboard.fessor.pid, 0, 10000, 4095); + pid_set_out_shift(&mainboard.fessor.pid, 10); + pid_set_derivate_filter(&mainboard.fessor.pid, 4); + + /* CS */ + cs_init(&mainboard.fessor.cs); + cs_set_correct_filter(&mainboard.fessor.cs, pid_do_filter, &mainboard.fessor.pid); + cs_set_process_in(&mainboard.fessor.cs, fessor_set, NULL); + cs_set_process_out(&mainboard.fessor.cs, encoders_microb_get_value, FESSOR_ENC); + fessor_up(); + + + + /* ---- CS elevator */ + + elevator_autopos(); + /* PID */ + pid_init(&mainboard.elevator.pid); + pid_set_gains(&mainboard.elevator.pid, 300, 10, 150); + pid_set_maximums(&mainboard.elevator.pid, 0, 10000, 4095); + pid_set_out_shift(&mainboard.elevator.pid, 10); + pid_set_derivate_filter(&mainboard.elevator.pid, 4); + + /* CS */ + cs_init(&mainboard.elevator.cs); + cs_set_correct_filter(&mainboard.elevator.cs, pid_do_filter, &mainboard.elevator.pid); + cs_set_process_in(&mainboard.elevator.cs, elevator_set, NULL); + cs_set_process_out(&mainboard.elevator.cs, encoders_microb_get_value, ELEVATOR_ENC); + elevator_down(); + + /* ---- CS wheel */ + + /* PID */ + pid_init(&mainboard.wheel.pid); + pid_set_gains(&mainboard.wheel.pid, 100, 100, 0); + pid_set_maximums(&mainboard.wheel.pid, 0, 30000, 4095); + pid_set_out_shift(&mainboard.wheel.pid, 5); + pid_set_derivate_filter(&mainboard.wheel.pid, 4); + + /* CS */ + cs_init(&mainboard.wheel.cs); + cs_set_correct_filter(&mainboard.wheel.cs, pid_do_filter, &mainboard.wheel.pid); + cs_set_process_in(&mainboard.wheel.cs, wheel_set, NULL); + cs_set_process_out(&mainboard.wheel.cs, wheel_get_value, NULL); + cs_set_consign(&mainboard.wheel.cs, 1000); + + /* set them on !! */ + mainboard.angle.on = 0; + mainboard.distance.on = 0; + mainboard.fessor.on = 1; + mainboard.elevator.on = 1; + mainboard.wheel.on = 1; + mainboard.flags |= DO_CS; + + scheduler_add_periodical_event_priority(do_cs, NULL, + 5000L / SCHEDULER_UNIT, + CS_PRIO); +} diff --git a/projects/microb2010/tests/test_board2008/cs.h b/projects/microb2010/tests/test_board2008/cs.h new file mode 100644 index 0000000..d3d1fa9 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/cs.h @@ -0,0 +1,26 @@ +/* + * 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 + * (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: cs.h,v 1.3 2009-03-29 18:42:41 zer0 Exp $ + * + */ + +void microb_cs_init(void); +void dump_cs(const char *name, struct cs *cs); +void dump_cs_debug(const char *name, struct cs *cs); +void dump_pid(const char *name, struct pid_filter *pid); diff --git a/projects/microb2010/tests/test_board2008/diagnostic_config.h b/projects/microb2010/tests/test_board2008/diagnostic_config.h new file mode 100644 index 0000000..9d9c3a5 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/diagnostic_config.h @@ -0,0 +1,44 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: diagnostic_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $ + * + */ + +#ifndef _DEBUG_CONFIG_ +#define _DEBUG_CONFIG_ 1.0 // version + + +/** port line definition for the show_int_loop() function */ +/* undefine it to disable this functionnality */ +#define INTERRUPT_SHOW_PORT PORTA +#define INTERRUPT_SHOW_BIT 3 + + + +/** memory mark for the min_stack_space_available() function + the ram is filled with this value after a reset ! */ +#define MARK 0x55 + +/** the mark is inserted in whole RAM if this is enabled + (could lead to problems if you need to hold values through a reset...) + so it's better to disable it. + stack counting is not affected */ +//#define DIAG_FILL_ENTIRE_RAM + + +#endif //_DEBUG_CONFIG_ diff --git a/projects/microb2010/tests/test_board2008/encoders_microb_config.h b/projects/microb2010/tests/test_board2008/encoders_microb_config.h new file mode 100644 index 0000000..9c8b9e0 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/encoders_microb_config.h @@ -0,0 +1,45 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: encoders_microb_config.h,v 1.3.4.1 2006-11-26 21:06:04 zer0 Exp $ + * + */ +#ifndef _ENCODERS_MICROB_CONFIG_H_ +#define _ENCODERS_MICROB_CONFIG_H_ + +/** number of the LAST encoders used + 1 */ +#define ENCODERS_MICROB_NUMBER 4 + +#define ENCODERS_MICROB_SEL_PORT PORTB +#define ENCODERS_MICROB_SEL_BIT 0 + +#define ENCODERS_MICROB0_ENABLED +#define ENCODERS_MICROB0_PIN PINA + +#define ENCODERS_MICROB1_ENABLED +#define ENCODERS_MICROB1_PIN PINA + +#define ENCODERS_MICROB2_ENABLED +#define ENCODERS_MICROB2_PIN PINC + +#define ENCODERS_MICROB3_ENABLED +#define ENCODERS_MICROB3_PIN PINC + + +// ... until 7 + +#endif diff --git a/projects/microb2010/tests/test_board2008/encoders_spi_config.h b/projects/microb2010/tests/test_board2008/encoders_spi_config.h new file mode 100644 index 0000000..6528244 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/encoders_spi_config.h @@ -0,0 +1,33 @@ +/* + * 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 + * (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: encoders_spi_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $ + * + */ +#ifndef _ENCODERS_SPI_CONFIG_H_ +#define _ENCODERS_SPI_CONFIG_H_ + +#define ENCODERS_SPI_NUMBER 4 +#define ENCODERS_SPI_SS_PORT SS_PORT /* PORTB on atmega2560 */ +#define ENCODERS_SPI_SS_BIT SS_BIT /* 0 on atmega2560 */ + +/* see spi configuration */ +#define ENCODERS_SPI_CLK_RATE SPI_CLK_RATE_16 +#define ENCODERS_SPI_FORMAT SPI_FORMAT_3 +#define ENCODERS_SPI_DATA_ORDER SPI_LSB_FIRST + +#endif diff --git a/projects/microb2010/tests/test_board2008/error_config.h b/projects/microb2010/tests/test_board2008/error_config.h new file mode 100644 index 0000000..7aad86a --- /dev/null +++ b/projects/microb2010/tests/test_board2008/error_config.h @@ -0,0 +1,31 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: error_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $ + * + */ + +#ifndef _ERROR_CONFIG_ +#define _ERROR_CONFIG_ + +/** enable the dump of the comment */ +#define ERROR_DUMP_TEXTLOG + +/** enable the dump of filename and line number */ +#define ERROR_DUMP_FILE_LINE + +#endif diff --git a/projects/microb2010/tests/test_board2008/main. b/projects/microb2010/tests/test_board2008/main. new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/tests/test_board2008/main.bin b/projects/microb2010/tests/test_board2008/main.bin new file mode 100755 index 0000000000000000000000000000000000000000..253898008afc991e16a3e58d0888d4e3c8562e64 GIT binary patch literal 68622 zcmce92|!cV((t`RAP9z_fPyHoprS$oZdJR8*0yT3V71mA!lIxE2)MOca}yw}l1l9U!avtmZj%oQ`1Ja%Gc*7TI=%ccj-l+OI=zWYwm>A8oG+^kkTkd_m*{p0z% z2mbW{b!>~zCueIvn)M*N>Ww!(p(si-+4c2VC;##NkUqUB_%~IZq)=zASi3qoJtrHUAIez0TAiMxNYBVo zEYDb*o&LsBv zXy`UMM*;mQ5;D|TNmQ*bMLkWgr70>QBO@m*L!FeIg*8p|EcM#-#1vaqdr>{qf0^D1 z^^?;RGoW%d*8YsnPRUpwBKD*BmYxgG3bBe}ojPr8ayFITpQ38$lk~r#d3It-a?;u~ zp*M*Owg*UxzAmJty;PCB4(ULpLen6ZJ}&)TC@ORPYV7num#DC)DI-Egheb^t5i%xh z45Y_~jhQ+E;XLZn>=NB;E{qjw+xk~5ju;y`alGO_1^gMelDd{CXeWDUd)77}Qb}w+5t?6i z`QD`-nx|!~z<#|XX)x3@#NAHPlCgX_q~pJk zh}aU~#eNj^E%c_9R7*}Gayjb6RdLyw>O}0AYLQG!&CX0yZ&YL@uU4m~r>3t^kSYqH ziW@Yidi@Cv)oEGD>ZFYd=msPlJ4??{tj@?vR%B<j38TR!GP4bm}2>I+-V_Fkz5* z>1(kTG{Of5Q6LX95Lw00xr)?mMFs-EJb}uPGZZJ9z!OLeO;S*EAAWpp%o8>q!nAV8 z2v$&zg(_2~J=9ZqQbsy-lnK=Y0VTpp5y7R15K}`H$_Z&nik|*JD`8?GyC|4&$S4YU zLuIIew7@8QAdC`aF(HWpCaKd`q$N{HsX*Iwp#1XWY?RaWDajya$uKf?4m^@dA!4Nv zp;8E`6k@Lg(o*cY0$7FYd(Rc8R4t(ViR8R0nDF2R6t|PENuehq*zIG>RC>a_%Ziy*g8|JT)x`6v;0G zC~BNrxZAs~QErazlH5plf69uopFI-E5cIK z!xb~a6&Y)DMo_&6($qn>w*jF8P6%BU5V|uUXiHHYr0pNj7DP8Ybp;`Rl)I=MY?%TI zRAV4TO?98>{;unT?i96Jy&-k=TB0WtumPyUY4Dj&#$YSFBks!q+%xVg0bGWAw)=Vj zX;U%L@3|9#_6&qt9!OJ1NV^ZA&6>69BxDhCv7r+l%`USDE@BN4nJ{%?;;3*1d`E-} z?1m@8yoeFhmjM*zF1-RswY&f6-r+8hI?$RWeH7T!6q&?zS?BTbRLF*_%DH2|Th z`2@%uXr7oI2lFFmEm_BkrJ1QoMD2%Q!al)6aO{pXA;hlvqPI&4F%q*;gCP_P2;tY# z3hBG9#{halT1Mh3&~Ie?Qe+(P0Y1}#7qf^&i#0&7a*`8K)!QktDk@yDYIL|FHC&OH z3E~tDpxr&L0O)q+d_lJG&PSjj)7)FLBGL2)Hvb?)m{49v@}Ix zb^_`%ur0DtGY&-~q!TsB6H>%U1CmE*gR0c7tZmOW-RuP1&ZVwMq;0~0$t!a#`6JaYg7BsKFw3T78dihnITCT>(gP4&%k-9*x)HtQD7bSL;AfcGRw$vWzpv*Jdv&h~-T&5Z(I>Ga#C#gwjm6(x?7J`EJ6ons^r~uy9L_~Ta zz`Wu4i)V|c)$&*xtLeZYK6&L#qLf1i1fQY;569kmAa1Jrq>&lIQuG71X@(48Phq45DCj&uQY z0eDV0xHj{y>!m#aX31F46Ir$dKoboHRIb^WJs!$a)5R1le_~Rl5NwRQsyWSO7$zkr ztX+{ksZVc(B5D$h2NRvNG0ZPgx z&Cu&D?`)2u<_MI-d?HIF+Py+uaf!s$9dlvf%*fiPfPJ!h1!&}VhEvpNxu1N5Tq$?V zt=00ms7L-5l=T253t%`pK}Eb@o=x`CifY6SOv7pLneK*~%~4UY9yTE$eTpz~u7Fj8 z7P;*YnjKlNs*eL?f0Q4Ve=h%0eoFqW6T*Xi>>yyFGD0WEkH4 zhV|{;kD^wq99ZX2}336+9EFVasXPz}54)Q1H6i1AhU&IHGmZ9c5`MKak8 zapGs`u~iAQdd64do9kNuZDEm6j$V#y1pWa%3x7b%I^!ay36`WYVc(vWoU=A79b7TQ zlosfSGzVhGA!)@lP@EEEBf*ximDm_WITKkONB97F$b2FWkOrZ!fe^$?_#O&QK)|1@ zaA@t$g4v}=UY(h<5gr5_XQU@3ZvbwArv#jjXfHerYiAaCot*39G$4&Z%RATY-r6?R zY_r$)MD&FMxS$w{v_p0v0ydpxD}XXm;-pE3jz*e0ecKunbqeNwxI$F1g6%Ip35Ew% zM1M%P)A=sdE|g@NWR2vUWN@$1F6UhbxXpJnxXp9F=oaF>$Nd-g3DV8dU!>zbvOTW% z9_AVES?%fPHPvgc7u_ea&%8cy@>k?uKC^v{J|FqC`1I|Y+;?~1bA7}6G5y?qSNZM+ z5mBXNtWH)votzb^dRo0MO9d_&T$!>}t5XxfOdlN?6{$k&Et`DDk(oF;GID}SFk_U7 zXjPHjg3YD)e+QVA9Fa^mB0yZ?r6eYyxak8j!jyA*LaWi~@o!ar`0;6qm`4=zX3TyV z;DXI@w?+|E6^+PoXa_kzfVG3x66OoWC8lhq7bKyh|CmOJ*+B6BL*M^-&%|d3Ge0{S zFhB;7DVPJ=>^~k%Cy;_BM8IJ~v+@5C)Z(GpOD8EF&rqZ!r)AonhR&rF)Z!-|p7-Pv zk1xG_jVEV3K5qhr-%;bqpE08;k|pH8H)K3AD`v_(YD)S>#oT#M%y|4!>XFn9pzv%D zv)86YB&KAfCMJtHz&QKAfr}#_qXsS>y7nRRJ&lr*{|7F{XZ%dFeZ=(8wU~pAW)FOr z+7P%YP!pINSP)ngSQ5B3&=6Q2XbP+h+!nY!aA)A|z&(Ml1ilvdM&Mh4?*%vQa#spq$fxpY;$afGvZ-hcm4KXm_^g3 z{10Z4ZGOg0{cqD7VjfhBBW=YDby8B7j5IiBiCeBtO@niC0Z_U+ok~q#PEpZM z%i9)FyRsf1;6ce>`M#c_>eo$&lmA-Jd2ssv<@WpFu-SYSjvFKdfWQy zTs9~n{$SjCS4xvR7!HpVk7IYoy~eU^fOln(JNysz#_|I@eXei`sh?qd@M zwr?6_81(W-wV{g9xmnBZVW<o4Ik&{RI@w@o_d`xZmB} zE;H3Zj7>|fmx-c2d)|fnY@4*=v+dG~jPfnzW##XbA1Oawey+TwyrbOP7;G#sYm7;h z#;pIyTxa1m3=^->)sWQIEgIct+uy5OrCbyK9bLi}#FrRLSdF^GsBdKXdUHE}j^05J zDjyOX8EtOYUo`UO#)Q@<5u4&xH$D3#Jp?vRJIsZG~qHL)>rTiv*!}-^g`@^5) z^OQ1E`kKoJZyn6is6fafs27qptkUr>(a zb)=@Z0sohozF!U0YF=ZG=41Jgrbxbbqs+8vrOb2%YP}dHGrclWX4*DV1G!69YMV61 z52Sg##`v*xgNZs*Xl8i<(;ddGMxQE~DdAZi&)PA-XQ7wiej47u2eaO6hoQxA&T!gr z#PE)x%&^6fVUSj6*a%5koW>X-(Xio?f{^E|4D=B$*&awDBBBSrxzHa<+NOrMc$N#xcOlmmj0f;QvUEs7k6~fDP9mqYQA> zm<_y9r`2fy-wIu>PG3#UtLIlpaBtAAsC^@@PkB8{i@{y_Rv9 z-r15dd82!E8s#CoR>4Q| z*XjCs?J*jru>Ty3R#g_KikloWG&0Kdx%RwB59@aB6s@WJK)DyBa((Iu{UrTk`X&0+ z`e*fJ`fbI(0o+^k#L5Dm2A##kn%fckNd%8ufXC${eGU#j=p7dvqpM?ioExRgi_AOB z2YJ_MtS@)hUm3f*eW_B=P&0~0+3o_G_xj)nXl@Z1VjI)hPi~&Tt30xu}0^G7o zl~D8AvQ4y*y0NUpmZIV!4nBA=3^W%~<3Ye8;)o!L!!JbHB=LTu5+t!rTch>U_0m=8 z@<9p{p2b=docj2Y?pwK<-Ng2*(f}n5K*`4<7DdQRgSJp-axZk|l%3XEoOkgAiol7} z>^ZjU{M!k0FW5LhrHjP*^yXL%tAyD%(ZWOqScAvUu=e7#kyEVld!wv4JC%}NocvxN zi@aP3b5SYLK`oxQ@E)dMkPIOO(GC^cR4wLK9(k%^w)J0Vy-qiKcLdjXw*{YHb}NQ9 zSQtJ+${~NpRE`7=tJ|^dX3}M;i5|f*3G$3H-Z8iJ z4W>?d2xUvF>!(F(9>4T6KZVDZ_`jsym*2)XI}!+ZgA`hm@bqZV3k5n4BS-z2oYqUlhiTBY2O zN*(B!dfD~CUq*i8I-(sk_XQK*m#e3_MdxBp(aSI7fD|Ky)T{NEt^wc5{M4Wa3TsW> z^uR-06wI@x`>wgG+mXlRDsko(g!ou3S~qSdnY*FD*}`^H$qI zTCJVimihTu<=xt%E-I+$j)Tk;;3v|m!?vCP&45l#pe0fp#a>2Oxh3af3K(3k#5U53 zvhtmsbXPFwt#d|AT@zh*X{)l9e^Hrr={02ze^8mx_=Pfw|4|v+ctIJ*UsZ~7 zu%mn{*aIkKa>9Q`B{LO;Ie0Ee%i;33p%#u8C}bG|f5+(VAu~NVPUO5i$^*E9Gyhv{ zHIduUx<~?3!X2aOs@8V2fjU}LlP9|$YFAWEgf#_Mqq4Z2F&@@~rv++=w7|7>C##YZ-!-W1QLcasETQ zTPw`cgl8Qi@{d&itr0~zjcCM`+l>d;GJ($}G_@=^<~W_kueHf;fR$<-z$>`p^m3;> zH^}pcym+TPFUa$Pyp=7Yqy$(fUe5R7TrLgZQD&wy3z_A^pSl@eu}+1S-#T%1krUWTsK8r4=#dapi(dyt9+UiKEa|#-A6l_poGgXbIrj6VGSD zm>!wUba~cxdB$1oTz?6HAT!-N`#yRWjjb`gm`O7IZ0r)i&DE2 zq;?`mtsCcJj#j$zu}ZG-9Pcc@FWcm|vvr6PjP_xiAqFJ5#kQM3?r)-9YJGUrf7giq zJ4W=M7p(7vqh?sY_WpM;`7Hh|CXAWLxPkur9Q57{qOVTSBBwe)bBKGEp7~QuH_?Wg zRM?%Q$u;wp6j&cASF;15;=O*exi3~unXB#b|dS-2CyU8iR=l( zSwn;2y5V)rAf#*F64mIB#xP**I;*H`Ix74I)D z0sdpj_jD<{gEbaw2^Z0FOLDRooT((*>mo0sO^H)(*qihYwH3_-rI{@Z0p=(}C=dp`)-OJ&;JVht8Yy|1WwQ36N60Xr29nMUB z^K&qJPtl3ZV(B64HCnDEWtTfcgmt^Txm;Mc#o7Bbv`x4yw)L@o6zTyvDNSNd6yzK* zeI5Gm(CV9f+;XVq*Hh?8l5E5V=bv}xB;Z*RAc!@g`U1Mkb z&>kG83nkiRIxRIZL2OVxy|)tOWW1AINHqrY|5gmK9HajU6nu2d*ChQ3qzgyq)){~& z*XSr%Iq0g6ma@q^-K*O_QbfUS!@h2`b?33LYINPfBgb$qMAy6wd(GEnhX#L6c$Nk{ zLpx!sxjX06VIH(vCtWyiysD@C?~+lcOe6E+MY0=1 zeT7z|hIjOKoj#y_Nn3Qmz@ugKJI$~xyPMt5X3*E^TwR6kRoy{d=Uk1R)xV@apzoZU ztFO?%sy_(18ir+FVh%9ZY1D5o6(1n8gW%$%-?~%oox9K_u{Ft{OHyPTZMtNMP3E<= z+k4a{vw(($J?fI_xE9Jx6OtaIm(ZAcFliONiKM0_q2y$C z(l8@xCUx3_WNQ<;>G>r6T0QCvoPh*_lkD|0eus7tX1Jgu1PLjH5h4uSdxm_j z(~zX&G}3D}p~r+y^a#7qnI5Qdf6#uW&DZrOb`Rq2yeI2AerZKda|ZFwtU-^EW6p?r zj%nx_?a}+6-v1Ze#hsmKw={tjAy{84>NeZ7u>)sZn-J%j%wnK=_Q>I2X(jw+ zz4;^iLA~?~B0RO72A$#Mffcofy1!m*8vwi6 zqQa_1=MfJNC;P5Wc>}_c3hGXIGYJ*a8pO6p1!*{}?G3l1!e808s^+Q62>uCWc~h$L z1%9J)Yh!`3l;1{dKV72{q$VsCEmI*qDV12JB;P+3HNCP2&xx~T`jYkl%+DXRb=r0< zrT5V%=(F{x#ZkKu%NAA&hv!8PJ5aGlAuaSMr2SJ7@?bJ|K7qC7Et{uC^!VPXFVTWP z4G~po(?n!nsA0X`4XoUK7whBxF5A~#^fYPMevt1A`F@b^?=D(!h`DG>_jX!qda|XR zR&ncsyRe>B<#*zj=()R0C?NQYw$kV?!rOQfXrvBi!1dpL{v$52;!{}G_p1qE$bi(c#yBF=oI@jpsy7fA= zh7Q=vNA13U_w3!vNcqonpdH4bD%I`@ugrL5(JO=P{?Iv9&7nWt;7zx}kJ0bbqh7iH zmD#T>dqsepP@G+yTU=56MrhTIU8WDiKL-m{^~!|3GxjdpE8sWZ4trtM^3bPl#GBTI ze@g#@9<}%Wy|ec&Beky431!)3xn&h)s=X85n(@}6w|=IV+52bh{@8ywtz3V;b)_SZ z6KDG>{U&{>{zbj0Z!+{7iVKUk7YpT4sjG@N6_*yjSS*xBrDhawC@U=6UM7?Wr{XSJ zC<`XKO4Jrzb=5sjbl*jXwRD!+QF-_0&CXkvcaLt2Zkp~%ohola;f%sXg~N3B>Z0{? z^rH&zFPvSttneQF82vQ;lX_L*gwh$Mi%N&-@6|^a&nZqQ$}Y++swjH3=wK1I^_#6V zTidqwD)TE_UbenWvxnV-+5>08<4Zfwgn1o`d5KUP4$H}tE z9+p(D*BBqys8(0_-eh%w?{6#;`2OBXjrMW%u+HRYc=E7pLS-%br-O>G>`an+=QnKi(4{UwzR+rm7pbU zrGA}CZFOyB_PS1OyYiw1X#673c=pK_@Rf*uvd9{72X+knVwq-P@9bgumu)|tXwMsB zMU7szqBmFvfFEc#++~Ab4s+L|o?l#zY0XUuHw?}&T)2_&jdHcMd1t59!M74mHiR8E zwh-4yrdvxLv=sLRV|5{9Z{XZQ+pR^x<-6-k*R5qq)1CXu?bd>U7S4N%+l?!)TT9)g zJNK2}twmnr?fVkx_dNK5(;7t1_*i}T`yeOPo-+ijw#IRN`AEoJZqN0x`trjeC&4La z06!RVGU^?$g5sPTEBgYO56> z+MgI}=Na^UW@j7W`%FfqsW5=N6o#O*p&SeVuSsWF6TK!|>pIiR*=vFl<*da7JGq1S z`_R@wnMHpWo{55=vhovpb=(&4#cdy3&}86KnleH54~#8nt|S)B@`mgtnQ84)x|*cA zEGQ`&uNm!Ot>VyDi*AeqKg*YPUuPNkIw!y!)HE8%j7JMdW@@p+if_rPMOaMDDkoTL zII*v!OISVy%HuCO`cm7=9k?VrmCv~&E(PWtE%A++{LBlt!)3J{mb&#$qY|-sB9-7}0?mX@(W;NA!^+b@5yU zcaBN+@{MHYn8IviLMzd`5L$`5QNiXcS|*3O%P)(tCR*SWfW(R*WZd;kxJRhjD-Jy= zLfR`1eJMiPmz?W%-kpfnS;(caW|TcVw}g{rtr7Ox@4;UCCoP}fny+Ynu;$^qCxE}> zp!ClB6>(>WGVAPp?b*-JM3d8faZi3>t&LI+N)$2A1J(O!_tA%GH1td~j*EjD8N8VHcQG_CoQysE z$L-S>21EO_7I6+#|4k0%&4<_c zAvz(kF&m`q_!x}5SV7KzkB@nZPNB=d{+Tg;fT|#*;R+Ku#F887V?o;B8S2KdsG*h3 zj@cxhFN#{Dr}-hAI=)4u{TwoO4XctE*htB9x&j>sp0>~F)Ml~96wBvy zg8j+E@+h~Jrj`W9{7g@R_6c3-@uC zm)wAp?B^I(r_qV{(v_3B&*_v)BCZWrMSO<%MY-bHOm1U^c{Z{1uq|R43%S~c3bV6a z6YYfKDkmJLc|RG99}(9w8?N^Ls|?0SiN>gscmf`2^&%catn%_miM(6|Yq&t!!ch|~ z&%s%UsxCn})SRV!2F{DhE>;k2>Fg8U$l=ac>a_DkeJc7@g!sE`U4l-eXff@*B*?}K zjN-aPpIq`Qq0{Z6X@IwNBAz~1f2J|AM%+U?YbWQt&KhdoqOz9N7uSGP7mYuzl)=bX z+qLLKYYE7?%vA8~2mDw5C)!FPina`~2pV_Ie4{BfB9j+slG1;pyVR<`jrT@~jB~7m z5TC%Wv+1-oL~2FPIj&1?mk03?&0}C~xws7H4xEU!IH;>(IX44a?Y4_O>h>=CshbX> zYF^R3PpoaEw{y&!IJ2C+y>L!lBli6b>03P0%&)J6S-rV-3bAu^YVl-8%opq*lNx*J z-HofId}Sv&)Uf*i&o=?jw*b$#-5egX1Caku$bS#=-*2?%xN6r%AZ?PX_sXU+X~oT0c_gXym=?xtbep~f}0&K7~_kBMimL#;oc)}K)8MweR7b_2o2$x|qfwZk40^AUIdVpWADm!@1RCdI|@8MCuzfoasuUl{n z;vHEAtY1>5VHd(Gv`7N6mGHdt2u2FIaQAqR2`;LrX~$A+h_pgpz65G$Sp4=j%1k%c zFqgK~$V`$f4ZpH}V-1ct#2m>>FJc@o#-reA++kY7E|wVh8}wY#|03wW9{L}~+{-l4 zV#EZl8gneC;q26-0`tCD@LLS7iEfbwfxHt&OCSPrcPkT%Z3{v+UhRtGjJl`H!)xv} z_imB@7jWzuyYBH48g>J1U^jw&JS~gaKV22eM`y)m`teUzJX#TmG;eGr@i4e6+RuLl zC#4v#up6SpTf^xHpNR3QFK9QV3x_LtZ&OWsZb(H)C<76L77bVha;qvXRA*vz5U;tw zvR~`L-Gh;8v6!cYh+L_~N6SD2!$Ql?^f9^@f$~Q1N5PXSzHL7~>g=ct*gcQ0jE`F5 zy6|_DF~WmLMzjVhND^uK70G1A3gjB?p!9PRS1ag2*&CdV6tTr!P=Tr|eP zP$SqyVGMPNG!Au%HV$)%H4b-?R>T{_pvDNOF%oKwf*J~_F$8Md12saRhP*rjaELap z0IX5~t5m>hWjCyR0V_Yi${(=mZ^LQ;Knwt^0s*Umov@NZpB~U>Z>Zr3eafLvALz3$ z^x2PKsQmuG;Of-=k|yx^nlR{e!5g-LCt= zdpm=RX1!cu**-4utjr~o^>)##Amg??$h7ky2xu_~ zXfYUQ5!{Uy{}&!8fCoc>2loIELR^ZQ@T^SWK?d+36#5+s{SJeEhj;7uKjlFf^f?0h z90`4nviFHp&j21|+If&6iDrAQE6mMkwrA|c(%ZGYT`cA=PcpAv~YLM}C$;-5n4jVYpbEfBF&%XZs+s+d7sl@AqGA1k!u?$aam$CVAO00B+7!G2w2A?= z3Y5#CT)_@-iDUy@qS-*8-9Q&SJxlMiWsoMA4PNQ@jBAPO3$8)d=jk_T8F$a{F~j%L z!v@~#Io)%i=SaW%TxYp1b@jErN>|Y6dHoRH3c*O$`0`bf%X*L zIPsWOGshN>`hBLNaq?|Fo?U&rw~wscWmp5$&H`#@1GRHNlNdnuL~5@EYOe!ouLo*x z_&-xS9H<=u)K&tuRXtD}Ru%)SEKn|oas?X+)Q$pbj|OUw`H!iM?|M0?y%wmw4ye5z zsJ-D1)W%k`_+_@Uq3<)pF5P!qYGYlr^ial-r?6T@D$Gx~EBFNXWVwf1V!%YhQZH^w zq@v{scU4OQe6rj_t*}=keT4BPlgiy^Sfh>J?y*L>d%V%dJ=56N-HQw3wfB{~Pl1wt zP~r%?gm^NRZ`dkkwd_Ro*ZFs2Kp%3+hF3XnViQg49d36SJ~qYvQN z7jW$7VuN(?uOX4qIPJB>S>v!(6l^&36#;!Ip)VEm6$yPsL0_YxuQ9iwM_$uk_pJbJ z7zvs)8Z>7tXinas0LViC@;v}K#5M~3rMrO?1ds*+q`?3w7$AiLq@e(57(f~>LTdbL zNVlg)JgbvrvbnH|+eIVsC^N)3()1Q!WC8YS(4^jHoMJYe0_tP&!`Kpx9Atp1s zZV#k4Lpr%}4N2!i+M$^qN|l;@3H3C~z$v;h`H$LP zlt=hWz@L}SzX36m7#rVl;T@%wKLWiXRX*=V6-QsH)U9aK3=sB+X!k)uEfFZqjW-3mW>ie>2y0Kj&`DiSEGXxY3KhgC1j?& zh_;r_)JUrlWMo(a@~th=i5lkqH9g6QgBpU2cs2<*J&e{ZoqdW<{&mX0)e_ff! ze_-!9sV#}zl(xIW6#L|VPFzS5d0mI$bbIWP2u<4$dUmh7dl z=i~JB=DB3{pp`5ye+j(X<9ngy`eiST@ro<#d0g?lS{qMJuKDE+0;CZj+r8kO^i3;H@u&eOwV}XTY}OK@L5&GLu;?{C!1@ zFog8mY6ZI);6E$T(=G%1dCHAm?v&BfUBPaI+@~d9(b|DVPlHkBt}*s;ucAd?oWcmL zjL*W(bqnlVHBj%3;Z-+xReY$@7&k)SPs1Ma8Q4Q^A~-M|;jp8733fa~EJCc6ELFpf z?}g7oycIrwNuGsW3O+A$#qzQXxO2qkE96<&)yYz2rpq9u*s5aG3#ID91%DoZ&Kc4c}wzyn8A4c z8i+YbwjBbW!77-j*f@iQG{hZ9_dQ3U&#!wkGxbBDJydh)J0yih&OzrV6noTEoX znun!|6hrz&rH-QSCzK3M72m~}lq$Z9A;dO^_3+kR1V6!M7YlENBTYvzlbFYtB}}Mp zobD0bQ@ZHOeZ27Sd59CR0cYF_}VIh~^g3LNxaRg5M`j(cCZ6`1Vj= z5}W2zh4Gg79&V_1oc0mzQ`-3aHTl|nWB$we@8y4y|5H9+)LP_H+^<-qZDHZ|!W-Zh z$8*yKWFH{FUj8?t2n7n`n_!{mNArOkzV{xcisVnwGVsk-ROl>Gz=kSc+(vvr1 z6igJnzZg|nAK41;$%jL$6L1cKH$AAp&#GbHgMID$F2b4cK}dh#f@j65BZ4ohn0O8G zJJ=Wu^$o`N!I$$M_!&QNi8meuPYk4?y@o9TZB!~D{$N`=zkd~+Xvf1-CX__82VuYP z0o2?IHOp)@g>-KJ3aA+kC9zNKabbC`Le+56(%N+bm^-p%1t&8r z;k({^%D<`QH^_Ue_6f+F3E$Vu@A%(nIRbeRwWBdg624KFc0bj|vkT{Qcs$vD5~73e z)eVCPA&&~&ojRXwA{{VFrULX#5v)?;*mjfClgIdl=Alg!%rxBea(8nL&+)qAP4e<(1YVV1@*|?XjEy1pe0dD2+|NdUxCJT}F#$;T1Wu}jo;Hry%;Y@)< zB?ZsduzlPOtjvA8u26?sz6xdo{xwuE6+2_Z+`{`=*RgOW<_UGZ?xJq)?|+2>j!uY0 z!y7Sh zzDK@^p09kq>4ehApH4{iLJse? zAA~y2abatp%8O7#ZH#Uh0H;O=V7G_z)|SYqf^Xv~j04Mc2SoIQkDcHiWDQ&RV2mp_ z^u8!pUrX5hsA$w!qR$Ae%Tbpn^D}B=n_x!|XG*iIhy%%MiiN%UK22XZg|_9?V$Mk2 z0RMSiYQ3RBNC#n^6>&lgP!qjuDY*1JpF6O^Q`_I@o@Pnvzt(-8MZxEu-$rs#uLZ#^ z3^b?UwJNyLrVb-HrsnQ|*JNEd#C;_2+1Gl4cIQH24F9y|UNZ1#=}zPE^lT=awyh-l z*r~aHlG`>e&b?{7)kMKDyfShE_}D@hN5Z&4sv#N#+lG?1BYSL%XQj=uzZ;YrQWC-jiN& z_%;PvNM3Qy_L6U$pwIRZj$-**24^1bt}#*))1MK4%sx#Eb1g4kw+3!)GU|}`GSfCv z?p#;A#|I_pGINU&>xcdAb^4ty_n4u?;mBU6s|aTJ{004GR=a#!tTzdMx;W&~`yQcs}RINk5G zIDTh(iKPJ3M$N9cI^N~ITYGS14>B-V*GKn9enY;nehU&Hq`l&hdP3UQE)PgeVLZb8 z{z7>q+*>z5kNm}R7n$h|4a&o-%0y+T@&_f3>0k6)kUy9aZDp*3u=nPDm?ypi)vv0v z&e_yOo9xpx8Ab+Y!s(%^t#ab3ZA0G?FF5o_a^4PAS~Ss-c)yip71_l;$T zA>u&L5MNB}I)cu+k&4?!PUddE4qO~*JHI*EIWj}AbG*yZ(zzcj*ncW#z#JZ5>B{=Dw4aabL$E7-Tj0LP zo!ugK#T~+DvEPu}(}Z1SIQx4LKs-QC1vt zfOQVsc-R^)HSG@CA0$xxD%|i;6d7PCg{TKTXQi9^+#G~CK9DoQD%6Rp{QY!Gn1^)< zcZEjkt`B{@rB*qH4^c*ihsz!q{19k}e}ZrKMS2wMC}pNOlLq+bw4z5U0;q_6UG0Id zLo5Pc=MZ}qWXmc@hu~Wsu~`PE8`Z!;@!6fj(Kd%Rn!LG)mKtTY=_2qQxnA4ns-5c| zmP4nJ+L2XJ7JR?r6zy8+QPC8>HR9{I-(nm$@u6+onQx;1!!iFc@?+8I=1}wL^EJ2p zC;qVL`Fu;BXkbb`dFJeb1w!^4a#8cKMc#v?H zi-+Z-)7Yk-L9DUOH3o4#NB1FjHu_YJR4E6Zfc!AsG06XpM2ipb4{-Cc238||5ErBm z%GYst9-uuAPfvy}GEEEpiadV~&yF$eQ+^cs+KqQj=fP+2w(Q8@H@rRyisU({IZ+p; z*`)Oy4xvmZl$jQ?2HtqVF=1RMj!E#4&zXe0XW+(| z#y)=zx?r#x%)YI?s)_zOXxCpTXZJ8;m?043Fvo)Nn(%(0{Q)G zsQL+=1`%dBR*ZSZvC2$u!QGi7u9med@K;-_K|9J!uWtU`eq-h`<&pt@-0$=^v-_n*O{?)9+DK0Qq_g^d3LGX9Ssp33uR8@W%Wa1?zNW#Jlw zvd|H7!s6$9*g7;~%<$niBP(!5OlNRL(@3SQGm-(S^wCYlPSD*d!(dR<)O82Z+S{lOJW1iVv%sY}y3jAYD-! zYrU@qKWvz1Kqw0clqW+r!n)!tF%PSpWkz3pg~*IZiH0s~p0mtsw8_jukUG|jGYJC=Nz?)1gbkUE3>#ksT7SpxL!{q%{v?hf2Fh*?d z0Lc+FR%7hV7~EAJC+GM}H{%*c$I7Geb~LoNf6e{$KEXrutvd<}h~z`>`mF9!tevXS9qLZ^j38H&5o_=?O5cxSm{ zB!KB`mU~C84*7>1^PZzJxqDG{qDJAzaR3yC|@CPS4K{_diPK_+!5-V zaA)Y#)t}Vt1DYmQUxoOVWLqB#EX_b)56gn;3lOc6V#^Kt8$I85$DSuw->P{B;D=S` z@INUh0Jk)Aiz4OLFqJn4oSUB?Ilwxi@gz zMH2sh#cLH+a1yf;-t^e&x6^f(`+nf$AKc4)vsXtD73>4=ZhX^TBWoMnE7&jiI*hY} z6M1xz8wRI2Cp~??ytB!%;>8=Z1rPi$+dWW247QIgiUfDspK@~yI0)f6YluF@te%YOwq)Kh8Nu%8s2tqFn{8nVm(Zs z>@?n$x@UAHx)*f)xbdI`IyZ_d2r0C-v^3k=`BK5ACqeI9}VTbNW5t>laF>E!Jf66>qRW2mN%Uszr<~gU@FeiCB_$EMAcSvw-LdcwRVh)xq{)( z4c{8D)8j0SXP!D3>vf?G;7+Jek5qWZ-bQV~7QfQiW|!xFPq^Vx$Le0v9nftBzjE2Y z6$JA{Qa=PRUu4I8HeimUdeT0sS@tr#!%Xm}`e@M`hIbj3FfTATV)VWVp0EwXr;VOD z^h*yT@YY-vXcW~d)bRnY#@~l!Snx*PM^2v3t#GS-@j#j@Yl-;Vo_Ob0bS~_nUE#*>Kj{YV)8K>sR8)j1OXfKAr43XUooU z4u8)6t1J#&cYxn@CjBSuRACop&Eg_pjA%<9vU{Q$G=FH&9}r;8qyHT`m1_s=DWH~j zTszHo;?^1EEwiP+#v?7?cHY}@tg=+u)&HD1j%XdVf^j(@h`keSK=MP z*t>`2)6*evJ7%F(0d_a~VFYMVR=laM!+g$vH{8;{)m-YQv7py+p`}lRod1lRDZm-? zW@(=Qz(KApjp#{c&XHdvL0=}uSID*a4e@s!W`=H=uuP+sr(;2k1OPPI*}9AA5J)>?-!Y!q&T- z&uGB=UD12BpzeZl4&1Vjy?SxCXft4AeyStpCpv zIp{AXc);MSbL{{X{{1D#Cxo%IlliE&TshrHpPq9rCZ`$w9JR8mgLi>nCmnvpR>2?* z@NAm(c-{Yy+t_xv+caWBL^PZ^9nC&P#{*}!Fqg3VoKQGVZOEw;u(sEbN1!}B^$2wG6pTBcQ@nv zr?}tg%Qb^;@rGZ9_<{Ic+u<}*YnL8ue?6FIzK60f;6<#z6y6hTrCa87#QdRa(49u= ztaIRS*>Iy94yZE_2L*3!&VXB%(PGUDe=c+>eM0ju&0r4K5xT0t|2elPYa~Bn&g7VT z`ivPgfZ#>Pig>|!sS{p;9^x7NZK|;TmMiG@IzfwCzvs86;%sMW-f(J-3bxaWN<0_l<=WA$RjFx}^_L#Je@Ph|kme=Sx6K9(TI^-Z2|RT$><@ zRJ-s+@HrlP>=QZ>e6F|-faF6IhCcffS`GflQ-Fh*bBbo3+8Ki@^F+(9^fkK7{`)&W z+x_6X2JZKSIXu53W|t0TIlZl}LCNyh?{aoCe1Y!7!rx-bS_*VsUuG_8j;rA=w^!hjm-!b^|u>q>BlRc~te5$|M_tSct zkGKx5DwJBreA>9UBL>f3Tn$#1kMJ-ZByTG;dEE?}59fM#?o-9BgA{r$U*qu|288tDQtVKK@8PV>=eL<(y^wjQ0+Si7`o}-So9@ZU4 z#nwJ;l}7ms-1dW487vKDT7(& zVVMU{2{u@dK|0=+7APx>z7T9I9KDFSx?Kw6PB#nV7HT@i8+NpF?Ky4102gQIr#Tqo ziP2XdJ{%1D2LGcrzFK^a;&?qQeU5HzksTFa`BowSdL8wxoW%m2NN*_*i*mG{$NR%k zjy&NeK7ltqajvlayiFPc3A|sc0|~qY8@v^K9s0?gWL1ENltXx3c~aQ^QMDjZ@6++~ zw}9q|1}$(Y2PvP%^34|hl&vY7# za`*$WBJlLhqoU1({13%74Auh8Sk~3cvbh>pfouNKwhbVwD{Zj8pj+yN9`U5-lSfxq z53`-5<6Al7)(Mmp)*-+Arn*)ecK8bywGFojez^OdTRj+d#CWa>Zv((DWMDkuQXX&R zPxIOwG^_4qupa(N=Hgbm56mV3+fU(CHs<2v&?Ej9g9+@90sdZ=h-&c{+P2`Fm$pjz zz}DWdE57s07~i4=t)wf&dP2*7Z#!rJ8w7Dw|A0}A zh=>G>t{|lKckHSC;iJ$K+(-53?_dLe0dLI-yc}K^#UF)J#UFt;4K{j~O%AYN-LywX z+Dp-D9oFz*3)YRSnrK13q_OF7U(kKvRGQ&U+(kQY9FWA>lJ+P_99Y;W9RvJl5$L6l z9)>6@c(dUVwB5z>G&TcZ;xFuNg5OZ^uqMOFCH!htoJJ>yQ@@6Vx4`G^4?J4Psbk}H z&NXA^C8Jzi^ZIu>?_&Z0Y^M=r#_R|R|vEHVR#cB>T46S7X@Cm|^TUU6Ppjom!?}WE z-E#KQixD)P<1WxnxIq_OWa0NQoA5U?s~XztfDTiPbBs|sg>G!s8eP1uetv7TTzi^W zwGCih9l?e?DRWWh1i~;bw zegRc%@P z*$(^XPO!fM*k1$elK}ga6YQ@5_SXRWB)~p(2b}+9hx@NiaK8h%-vitq0Pc@YaK8h% z-vitq0Pc@cZkxOu=^a3R&(k2;0`DcDt@*)rZkx+_Ie(non=0J4Fm6`oC%i{SUVcm} zFFy{S6Y%*GKBwVx20q`w2We7s-a>AomY078IbXx)Bz#W6=R5d(51$|4^P{v3-bTRK zuNySNEjxl%9DW++wv*E_jJXs`oy($ib08)^TUQ2{3vDy^tSFmb_HdaaC#Nj2?1{1| zPC4s}lZ#`^9xcW?JG(_UEunY9FGAj+r|6&1C+c(bCt!6MdVW&NGgU;G2X1+xkO!@RjeyE74Q5+^mU2yy%Tn;Ew=ly&=2W|sNTR}1Rh3t+wvgtx^uLu zz5f3oDqHmz?|zGRx^oQi5{R;WhMu5T=>l~gy02hQ^BP?VGk%}u2YO*8YRyGeLzvOb zL(DuF*9K;lZj-K5_aabkMPX*CAWuG8jEfw_+|SHrmO-s?Iy}etfqs(wmgf8_u@;UU z{kKBgDMqB{SV^3*c8wNqaNWy%$V8MsP(HhS3H)Z1ABSb4uNzkbM-Lkqtm}w5{reHe z&ium&OK0ATQO6?R1s^BwaByw7^OJJ~<2zlVntP&u2|Zi4OqZeCp#K$OtY#aQ8de)N z8GNh!DofcH*gl5C?5>DH-FDqR-C^A|nkg+Wm6>{xJ92*p{-DjZ2yT=2A~)!w4DX@K zfqOz+F8&sPbF41doWimIk~$6JBOEiZ`s)2RLqt$X(1q=64~F?eY%XnxnY zGx9uLsR>5)in=bay|>ge0DiGocvAvmj7s~KUZaOH&a19OdbmPYjYqpadc z&|@H+3$J1HaOXEd5xxxzLV{yTIt_#NIO@9+QP@pX_sM0~W9Vlp}O@p>{r zOmXQB828)V#@%%^1tFo<{(s6V`d^nvv$E9wt3e&a6MDqv2NU1GcIZ!muBD|f0wu!8 z{kB3nDz(nP-A(Eq;2vPX-h8>T(rvrNIzl=sRl$q3Jm5VOh7TLARXOYt^n!}>9Q*kd zP6{q4mlCbEtm-K99mGnn?~ri9`%WeFJ;VFoRQ$i$G90g^`&0@fN9m8#QP>sYIH%A@Tpw%-)c?w?Zw zxL{p)%2TvO_@DD$UJ?*HcE0)k-*hm^dv`te-gECc_nhCcjFM(^DU4R!srXU5utJ(R zG4V9`tvr}o7}*OedahYm_3mhVZ@}zdp?h8TtS$rky*;CiAj=J@M{c_O%%+PxK@XzzVJ$&s=_`g7_*VHa|FQ>HfxGrq=*izjT%`VL$&Bx&J?e1a4^Q!-R3(kPlpoA9q!_Db+WrtF%iqK)r30EgHOuC*8ryca| zS_oPf=d-I0e#ksY|6jG*y`{g!zi$!c+7Gm!X}{L;{t4KZ8SEKiUhxamLTbMCQ_#TY z+Q)G`zk503h-ri`jsIGIF`BJ+jpke2+H*gZ;4Ub5z3?^gx5!8>JkQ{%4v~r7YsBd8 z+cR)R^QWZMs_iPhYPZV8m-&3i&2JI>9`{}j?!oW%svbZOtRMJp#U~ZD72og2GI~=3 zU?&%5toxofMuFDXiPTdR_B9lC`!R~dYfoOAVMmWW{>15C$G$Fg9H=;1@nyxiiiv=T zAo#E@;CM?~^P+~qO;0uWUFZW+HTa7gCK#SKEHu1o*kLdjN-BRMG9l9YHn&Lc=)pRv zix+KIuT|6!9nZK0x`XJ7ffbU9i2acj{VSp>22|ke)WVJl_V~#%7NgA101JQ^RtXxr zcOb?L-1xH=S_8kzAr4Jm@^mII*`Lj)^~p6i^WkYRt#)fa&l{&VE@~KY{mBNui@&e^ zJ@ZCvp&>ipltZ5(1pe-1|28wNE)TQB`5+A4-NM#9WrL?NQ~0GRzJ*6j*LCa=zxG2ePHj& zyC74VEJhI6nU0>rF@HArl!=c`GjLPf$K3p*HLSEJmVX%(yHlrg8r<- zo91oOB=l`gB0(AUdpL^-JUN2n$?lTL7zMuz;lAtI1-@~cS^IJ@KFG}8+0jcR8zi@h zsfxLZmle4hRY5VF2q&+mJEZid&YDALmo$#Hus!WDx1D>8d&%FRRB2jdde!uXDb$+H zjlL1uJ9+iP&VRK|>kQ$vp32!nclAqZ;n2Ys!pTqImU5X%Cd|sEvQ09S#c)^71HADY zaV_?*lE7m=izR~dLh}0L_04mq#(Oy|A)tSpB{GVmi}1ezMYF9!ulD7fCFVF+#XKv@ z<}F}1Q0*pJ;4LfPuC8@=0ty_~E#z@&U4=QyYeBkehj7P7+nW8vQMi4qw143&g_mmh z+IyC9XAs-rh8OHwIEg(pTadlo^$h$;dNhv`pVsvE`H3PveN7?H4FwB!vCrRC;kyni9MgPc+PrS{-gY=yj}jPe23g1FHxW1 zP}I>L3F(Blj;1YwWFWLKG}~q2c}OPg1;+@)L&v+ErY*M#L3ZRlvJc7NiDIJyBXF!B zUne!BQ8m1LLizLM3(HYw1iD|v>k5s+s8NFL8MZ7m6aUrD+xt6qKb|)}iq@~pZ9I4l z7;2JS;~|{8AzC`eGXT56u-%|H>^5{ClV>h0ZWz|`RDLwb>}R+<`C%bKr{Q0qd$%s zj=&w~2qYFvEJ!by&(Rs4b{~a;2?fs=EQEdTknB#EQNXr@X5zovd1b$26dsQE(DT}U z5j@Ddo<1D2{pZCEgD*~O@VxS$h>C!B{l`JKQ!f8N{+ax1`3m{#a*f=m&V%`e??N;2 zU+uhy?ZmG827{50QPXt~KJVNEeqS%AwJdH(_<3T3Z}b0VFE#(iy-d594Khz^L%`+# z&0Zq!`1i*0QFzAME<7WFcmlK-3%YCD99%+Jxu9)x@PTi;YTNYN0XuGY7v#7}+==^{ zpR*f=T}*B0)%2*8;!ezN(hf?8T~FIj%V*1%%U_eLL%*ab@PEE z@ESVgJHwq^f=43ZbuoZGqMM+5Ubhg?>JG{rdNk03APx+Fiu!Aplh0+xDQfNS;wF!} z$F6c4pM}H(KuuT$=rUckp~7egJ6SJo=f(59aC-xq(1(r<;1V~Eyqek&)cBtpo2Bwi za;4njjF$LtNeFM0_}!Er7nFS5$I7)QcO4hhNB!q?oGz^05<&yAM3cGAah6;4J4Th? zsoM#R_#i! zphAY^1?Ta%-R|de;r4oyekpe=&6mTc#doPt|1+w2D)eiJ1iuKQJ%Em}%XE|uv7ub| z7^%>vN1caVW$QUfQ^@AzYAy%xEFx8!fhRXj>rciBcBq@6lIHU6L?3mmpn$I-^5Co4to*9z(SmNZwS#WdmBgiE067v9IF2j;$Nwy5Kq^75*XQzKjpnn&7 z;y>s25_sdUq0wXZ8;}8Ub!1u*MN7oY0PUX|;umQkevycs3P`$3?UOHMHV(0;GzMIJ z)N?p;t%<+MwS<*1SN^O#Sv^Sosrsz?g8DXaFEya^X&~AWR|YrhcwGUu6m8`*tpm9h zORRk4KlEP7Z;&A`wSP84_aSCF!WV-1$`5rE}&$yA% znADuy=zZz`M-K)3!3TSo1y(}?i{@A#^_=2DIqANp-T>suSK!R6)>(CB1xE@nI^&wa zKLp|q`oqFDLWsEDUFLmobuI+y8c}%N@m^i>I(Vxrey|M0@oXmw8-%rw*qycqY)10I z9%2pbvS>MSf^d&b=-O>~{mzu8*_;OcE+N>89Pg4Io@+L06dJSU!*0(7tCKr<7xU^0 z|CaYbVR>QCb|&q_)XKS)FIVPP34CR9<+4h&b`gVh56@dBDP}2_DK;y76k&>ZMY6_a zfuqtD>oA{2XDq-^lq~RbkZP1_vTBYBF|_da5Y<@Kv#Jc8@OzMMly0(aj^lTAk^2o` zK2got)F{f$@I`OfHp-33QiW7ivI!L5oW8Qx+b*VTLk{!C^gVIxbtulO%C2+#}+(RFtuQA0Y*Ws z=zlce^MG#x;tP_?Cza1C=X`DZZwgQbSOSDLHx;slK87%^mO`>P(CMggE#3-d0)BBBjvqDVm-V!VqZD7$)&s7N@8pIbR`PG zBT=x?;@UO|b_d#qS#bY($!c zI=Fsc&Sw&HVJieIKVBy_M#0yxl@ugxEVFCt-R+wA+cLf*-`6+vT|f9W#vX z^3;itxw(hba7*nt!y&aT#jhIQG!__l8%sE9yWdHo1^NrL(9Z5}(r95Mus7UYpA+GG zD!{4bqI#OxDcN>{EZJ{+qfkkH@F8li2UM#c!C_nX;^Wd3F-> z=Newt<0!dD;Kch>O#dR6et}g|z2jmpUJSmaEw{#6@Q3YA&qmImxR1peztquU68yJq)X1sekXoSxGwLIBXT4QHW1|fq0ae>9O!Gp;^dMg+${-?yug?E z8Q53O5<~Wl-S_Oi46x+)NAHY>st3^ou1_ zs$x}@>V2JnhcDO+57CX~tZWb&c5mRZK$WIgQ>A%dPpXPkm8w6f(Bm~g5+(UL;#dD( zqG-|ApgX%>bib&#X@F^fb$vBT3s_&*8>g>9dg~xk+%#;q>PI;p@w;|UBbLvhjJFdi zvY4zQ-&diH7mpbe%wWlk^qo~Y+W?Tqy-H)>y7r>kC6y`p+U zC5(CqgJb$os1~c%tG4R8rt7IM|GMoYSwvQneuEL30PpMzvfDCWGMp@v9g%%1J1awOCPac8?6fl6qGX8} zMT+8S#cYKzdvHc~O(Db+=zba%?n7KUytPoXU3OFEN%kcX?`=PMoW$Kwh=2!Jw2s;e zd)uPRBu6Alg+;Mnal8xqt-vqP17S9_4I}#^A9O##IZlmWNxjUl*?{Mrctdh!O~sjt zrV6&At3)f5x6)ykjq71Waxi%GdD1T{WOkIC+hOHnY~6NF8pKa*KYr%8@wF9h94)7O zV>zA}F^!r_sp04)ix{`8DxAVYs)8N2)6Mr5#g=q2T=;f!3yM0AFlIBnkG}TRI^Frt?8#3su|}d z`NiqR=$_Hd)5!{`f{KEp1%if!x2E;C@EWHI-BF#xuE}XX`|rtSFX$KR*Xy@}gtp6o zC%Xvez8_^B?zo>4q@)3$1rco9`Pv7u2jfn~tpFT&{u8l~zXq0@5HF^cSLS~vkA`Uyry6|vK~c&XT? zM`^c^-<=b5Z@XZPE=V!I-4C>Iu#@}4j{E-I_Xpol7G*-g`fFI&c;DxIU-I3}3iS%@ zUT{+$X%g)f9TN$2H}rNSE##B;Gx_ZGIp*^@D~wSoBcxaNv--T_bHZm1i(`P;15={2 zqJ`O45qi}7bMJ4wcRO>E;7#}!qI>Rqow@p#E|l9wcJf>oS>T%{UH3o)C7 z=(1~B!IwkWBeo~Lm6)ISYGSZ6ZTHf)fi<#TwN{1eC|BQYf7Xt`agv>x0Dhx*r$Swa}xh%hnmW|(m5lOYLypOz}JX9Vo50i(e zgVlZ2eboKbq3UpT7^hF~3${T{o>&o5(WjzsMQBC8im-}sIGG50xp2C1J$X&B95nUM zI8G^e0@$EgqM5CE8hD~Jmwt(Uw*F~1t^8}6<(he#XSg)TRr%e2hJKNLoqnetaf1`c zCrJK0k!&5rouSdXYKCEvVVz;80WrvuRH@EW_Hb6POL3Qy@`<-$kGiLNPW6&A<#Yzu z`#Vng3)A=;A`JN%@9ydAZ5$$#UImI@IX8a`u~!*og=oYjUefB13A`HtJ0*A< zLs1Y1uLxoznY=P6vzMWo*HrTFO5xouc=r~(+wFYUlaz(0SF|5&xYBCZIuSH%)x5=o zyF4eXK7qq^mx%aw1s72eov;Z0Oz>xhKMVX7!=DxYN}w&^fr9NfLHkY6eiO9cWQGWV z7Kjj74AJPU5T&ogs|C|J?75H{c4H|-;oSvMc;E60hCWHFQC>&%ykR#+JG`92l{gbI zwTvnS+U)|`y#=(x)B^4r-YD(&JMOm}BHQp*WU#yUkVBSXeC}|1b-K(1C7GZk6O?4y z4N(IrD7gqqHbKc|FCm=-ZY#Qu7r{lImqEXb(62J+*Za_~2lCG;nsPYp{WKZS!wB>! z1A4p<^yr?BbfjB;M+%e?oBX8)X!XCQw#>fFzWowfzQr_wH5J%Wp|(b#EUxYd=~b}I zJ3!zrQ*-mejk_Ga-~=ho4D{Wn*!x_h8I&*t@6?O4+R{losj*eBkseOH$I5A2bx84( z#Rrp*_=L3a^qeg+QgcLQRF>#{YOYA220S&vQxiNj%@s+j*S9RE`SkN3{d`Ej0Mftc zOb<^@@YDoPO)o-vg_QS-4vF2swsY-5^d6yI4bZMkMVg{F-Sa!-w?YXIrZ`XRn{_-r zym+B$rYX!CQQXTEVf}~coT&*)y(x!~H$agGb0IC{KnrQLx|^pJrsrGRb1qV)06YjK z3P)<_Va`Z+iijf$43pt>!PUe{WxQ zX&bk`yuh2R8Bjd#nsw0bAJ{_DM#_^ZpJ5DPwJ8b}Tg{8Pkwh#vimV`yl8!NV;2cV; zx0+voeugkx&C_Yj&F2F@dU%pX8=)^+RY`aW(<#5f(fYSa8b%||A=WF>Y&GM}@gVj+ zB1+v~9Ysf|ej#Ll1SVq;8TdV-4;=y0aD+7?JQ(J(FMUdLP;*xEsm7Q7g+M=sVC%NF z+qEN$5$FA${j)LW;eIv07FHLu=w|}sZY6dGseCCd{ja2TQ{sVcr z9Osgdo^KPM>s--SSc|hg`0kdxL5?FLq{sH*e}T|CKG%~)Ns!Cge!zAZSsaiI`0vby zTKAFaZj0u+<8@zyJUwL>i@J=KjOxlAv1MqLxli#S*K0#l_w-%4WB2rhjos6Cc=mIFtS!>mEdMR50$pTP5Xt1*8{ai8M$ zq8mWVvx-l_V~dws7f}U-paDc058`Yv-EqyoNUrxe(FHfq-=e*I_4-}F7U3;afY-rJ zwrf5ASp}b~r1<0?>x6H8;aRg(o}9rNfCuxskSket((#mxCy?&%F3xkklW{isfW`%7 zxG+eH?K+#C2#Nd4+;7w0BK-jqv=lIH?{Yg3qMQjkkUhKI5swWJVctvzk6<1{OTbyf zbbvQ5J|a0NKE?g*&HgyOw-LSC+l#P`eB>JB6o4*b`Ry8HOJ#1zWDs4bNL(cTP+SxU ze<5*FQ>rQ5lxUHEXu&|GTgk-hAcUc^xOBJ2+d=j?~o3a7Cy z^`h(%A@v2^n!f8X?nKRP@rQoxNi;?W+D!}=kCvc(c#oq$($t;qn$G^T>rJEEh^RX6 z*J5nL;B{(y1cV<5iBrujPDgZ{#hd(~Z2ZR8ce)j^bzIsdVNx;7C!_t3#1wIs zWEb&Z8)&kg)PlEt+(TOs>-Oe?vu(kOUkLZn7qL%6dqkF0T5aEItAm#AoBLrp>T~;A z0Sza75G#hW)6SMrbYIxp`U2`!%I$~sIkLwZmvpN!gi%9%T2i)DtMI1(;Xq~+SLaA` z?O!@|gnR@mq+!+i-4JzAn)uqxOxzv zm3z_RYAI-lPP>)g)wTj<7M_P9+QcyFA2?eJw52-0B9@sQ(2PN>eM6_v0-nFjPTfGc z03%4htN28*1F^9H5ARdJ^0~v!6^ytftW}&D9%KlKh)=4{_S(Tv-bb|lN~!&o8+hiC zUPk0WoXfBLjK6u5CPaez4s!72$vQ)5_cC4cP}?9=0M_6AAUgOVDT3uYX&EV7B~xH| za){T7Es-xl6xDpHU}wJRJCMwFgEn6$Rb?oXOKu~wv5!@eYs@W?mWpMhekaxe0h!@H zMK0h9vc_e;c*bIlOFaQ~yB@G^WuzS5lHA*pCHQn3`yefFCi0O@j9hE_IJK4ita6Ah zy2*}fZi*yJf@tx6^m6BK#HwFn-`uEdF#=!7QVz;UPE$w?VEc}AcBc*2QKmYg8fo%W zwyMC76vm67)eY634E@c!Zk%(t{;m-bWFJeIrEjhfsXi@P)-c~sW?uz=Tm9k~&@^(~ zTT2>p<(+fsNy)~R1%9@c)$q5?ZzKy5Dq3*Wc+y)sN6=TvcF^aO5Q{yAM13^S24ZYZ zus{@Ki#%4Q04v09&xehOQDE5^&pkVLNp5a#0-bAFV#%;fvSg>PPv4n7*jZ{|kGcCi z@RYC17h2>^3m!^ZQh7g+z5>x^l=jywFI(opy+9t&ISmlT@M%PLKD3tv>{V~t_5Ms7 z%0B!V1-+TN+nNdT|2blm^u@$KB)+GtR^HXB?e~cgPM@DotxBHG8g}HH?t*9L08@}D z#2PFY(Xt%(Cn!Tr2TYHA>N!Az_ol&1c$;h~Alhw{EeCAlZTTjX)Rvo1$#&{S{4lrp1tQ=Lq}^aj22f7o}~|!4yUn`2r#-;n?Ik-i+$( zP2=rTCIj}Clo-lJevD-+N5~YR#UB^Fhj=+~{;2IC@|Xd9y)@106X75ETf*QEcJm$O_S$UnMdY-vX=MEy2T+4$I79kq|`iaRpqh>*_= ztye+oQ|?b(kFj%u%*!|KCWN~(UdP18^A0lmZ<5;;)qOtUqisLp*@IdGR;#ao)#`A@ z4&NF0{-{)X)EYKI4VxS_Y=jy%J#q~RmnHzlF8XLrgIO13w!Jy{vhXCWfqx>*msbL0 zrZrGYB%?V_0$<{WT?BlbZ&N$rN;kcSbXcaG^yVV5Vb5f6 z$K9Pv@tj^ye4aD~?84U37m_;Tv}7~HK+}^pxcg5L;eBo|HV5KU{xc3S%Msltk9>FJ zskjs?Mv&uEHHNkNn%M*rb*BG>sFTRy5fu^daPi!RzO6F=zdn&p0688xFQ!6n^EAY~ z#8h$OF_Pw}n<7kk7}ksO&0&!8XA)4~iowq^sx|QE$X`V_dUrddGNW4quDOS3_yZ^?uN39&Pb3cg*lYtm^W*wRpBYlIkb)8 zZ}$z^@M-Gfxa-G}=mRrI`2G845ZPmyh?@GSrQ+DGj6DG}MIBy-|3=|_F~W=i1U{HO z1Wn?sfeEr2PLE?DT!U-Ycvwrhc}BX;vjq$Zn3|Y=2}d#a^sZSp>48-4QX67Q>&a=L z_hFL=`d3*ejBuKL6kQ7AID=e7>cXcna6<&=_!Nk8dV)i*j)Av2av0p|L_&_O){95u&@(V}G^hEcOMcR-(%=_~We>MJ; zizi(4ALluO`}oz#JnTcjW*&|suny3JeI!*)tP*dC{*qV@=W|5CM;jPKlufpyrTuB5 zP;8S3@6e_T&zN|2kSWA~gU)jfr~}Z<`&nmD{^7D)STE+l*cmsYYXKh-%jNT)E5h1g zmxgtZW3tpShH~zSixZ$O1@~NDx6a2k;MaK1edT9z_rAtMU*+UbkP!I(_5n2E6OR5C zM%e-}{;#XYrwJvjYr^V!^RN257W$ie3+o14_F|rXn5T`XlOuJJLmRT;)5(uVQhbkj z9{pSEuV)JIT&)yjL$#$OJcbG2Gy@Y~)4}?vZ}a7T{mJnQPjI}|#Wlj2gB3e+B*4tn z!W<+xzlsBGfsVYxyFKSiL7B|=N`JewCAr}QahDLRYkPy_p&Ap(MIZKs$RiM0Bz!i+ znmPbBe3!!8{FJRziy{4TMae2r9-Rkn@1&V2b6i7b1QO(xQ_?ZbPJiN3G-F zzpMjO^jt^xeQ_;^pRj<#HgJ!^j+mW7KhP@Sj$04#T~DP~14e{kSEJ)fbkSuDpehRG z@iX9IKDIcg+li}iO4xQ2CGwn`f_s8Az^Gya^h9o12cS#VG8NXfMe70C%U9t6k%{b`5%S z{vfc^cYpU=?Qq?u39H222T~8CCfMsXZTUNV=dswDwg^`3!o4bbZ?1M&%nLU9f(-?} z6Rg$|bqF!IOj{o2>-V2)k;`-8!4|pvFCL{ul@0IPuU@@-xtF;}`6RJTze@iics-%b z18gRMpNLYq^0V#sXOjRQ2)NM``UHI|5liBoee8V|728Qt_w>zr^c8sZGu5a}w)-2^ zsNw8U!U%iepdF31CDMOFl*H1w&W8K_>X^4it!8$Q8paw&EnrNeC^m?Gd(?EsIC?dk zVjoUFZT~u9j{Td2VYJr%eZmmh*ziNbVEViE5wuQ<(fo4RZ##Tw^KRqFkhqede)OE( z8Srg>kiGGzgdlnk)cb9F>}Sk{^9g?R?y-?>pV6d*=ySS}3UfSoPCjd&-ftK@)y9nj1b9z+c%nA$FNAmT z+nRh0a0-aV$>9wrzDFP$6GY#6V=W;fV_>gM)g);=I(_Jgs?pGf=tdEJv%`~)YV@IR zwR_URpsNXPEL7Q4`JjK&ji|X90%vapJXP9N{erMi@A9UG+=PPmc|TcFdtEjvV{u1p#&ZZR3YeD z{SSj|@)ttK(QQ2A--SY1F*V(2R0&?CT$(0yyv)5qT6pfN^K zl4QfYfO8AZf?Z~+Z7)z&bqTd@stZQ@QQ&BUc6`_FL!VPVX6P6JLvheW3} zf1QA_HTjj#?}14>?Z5cP5jZT#8z!cGRbYkRA+t?h&I4*`Q0oom<#k6tU zc&22Whwtpe=UsnJFMo<8QPH;O&<%9^Kxc zV*>^LBecM9`TL~$!QTR3D{T3G@V(H-*{^|mXJAIyZu~IVla_$I9GUoaM{kJAg|UTj z%|^0UZu~UZpO$2w0S@g2k<8RvMT{i7cjDRhP`a)|O!ouMWE&Ys+eTV?QT_WR`nrvV zB)eZfy44%gu$lb{IxEf_B4WWTb*wwj&Ve)mdz1DuryIVAvlMh`QuS7fAwVCehj}S$ z{I0#gIXAh|9l3Xz?}gH>znFzJfNgfIfX)~SPW|qj64$u;zgmfvSHD&jKXWAr!s znTN4s#g_-(9XMT*8NE`xMWU6IN_@@bZ9YbPcR2{(%$3wcZxQ30m2U6u2H~5(N$R6f zTf0)S#rZCVoi4^V-%Cox_*@Hb4zwvLz;Mo;#C&V{8>i!LGha6Mx74&zQ_mARj> zA2o(rL0Kpt1Si47Ht>)J5-Lb4Q1;d6rAH&9His z8GmsA>k;o6?$5?ZV=bQ2?W2b=+nU}qB_O^5ji0(ZzGf?E*2h`GE#YSOl1rFa%MNOj zW&3D)=n-?JEyRk?Jz{XY@M!_`OBP84|Ik_*h#CSPE)~XHMA@BBgQX-h ze9B1HBj!Y#HVRW?etb8L;mbVTM$pk*opvZ4%gV{fO(TN8hr0~-^1SH+ihx=Nv6%bs zZl!tBgj=gug>QrCv{=gp@omXo@iB=f?P&~h_No_XL(UNd{i+Ge!8$Q5mf-52&~~h^ z=eG0dySJTdxv^{8Q3}O#eLg2|q_8M&zG9FTpg0kj4OMHb5E!q$v!L zQAT)1Ir35P%oyN)Tj|~|J=`Tl(Q>$(K$}$50pb~8Ig6G;gz285S>a2g)hk9@(Tzyq zEU1WLv|;k3#VwK+S9~;o3*KTjUF?AKa)q*k7*m`>ZJ-VVHYDG|AoH=UF~yDcUgl4f zn6mj&*OXTh6xt4nJiew6Po7x~9pqyQlif?UwgF?B6wPb(ylaZXoBIYXzPP+S+Eq zx(6aEVDtsGJzwz^@q97(eHUym;8C@XZN-=h^X>=GQu1*XYAt%Py9iHlUkTpKCKX=* zs(h^E%5V8sog5RFz}>Ts*yY8LIhlC6=9Ng)S|1Ea|TnjLdM2k{t<+~;u?t@8{+tcL#2ML8zmwf!FP^}(9%6Qhb} zfF}r+ff0r7NpegZOrBPZ=$uC12MweN>idBRwwB+KQANN9Ouz>wu`+5H+|msPdtDQ> zxF{fvN&q|S5nkA)Ni5>r%BZ1`BHjzpJYpbcGUWV%K))L7xr{kP%mmpBTbaRNMBfZ3 zDGf@xM{q5m(dMw0TP?gE5Ti_G(AS8;)+l*@xj@xmtLP@u^pD|x_Vd4aN^wvDab2kt zsu3avV;t~pEKd!TNRmKC8Lt?o5UBAM=vuJ78`wIaI}LP?p@nzv!U%^zOvSUczYu7< zC6xJ9_<0yL96zDNUkHrugyVu^$ZH%k=AVi`7#~!Z{3%nP%-pVzVNzIIY6v@!rE7v% zyXEq)AJ$xvPCg@RJsG~#kN4n-U^cLOZol*?z7I?~DJ@G31Z}*L=mq{3WnlZ#8NVQ^ zlv$9P!DdWcb+dmMlU5;(l|Ck&5Ed&eonZaYM*{ByQyyLqWBlipQqNOcnPp5pBb;3OGsxZg&_|Lqc=AN|p%Bgsh>3}fN01247oq3fDzgv$V^Rz8QC$bo zY~Du=fphm%DvNrrwS^e|QwMmIzC-+I-qv;=G~li?ufN43F2Ia!i|`fs^1rZsp}s!w z>vZjcy5`nqb4$yq_*DC}YuT4yd5T}{{x>`0ZSy{Oam@=m({wYKv{CFQSu38-O;=Cu zO!qh6kWg@fKjn}TIweh?2a_9=$@q4r~^@O=GJeBHI$s z4GEM7J*deJdIaaXxcb>YFP;}0z05?rasI^{jNPYZ-hJbIv>jQ%WT$Rmo&^qI{k2q* z!`M={F~|e>{$xpR0hhUX-k;`sTPnF(tG1xDjh{FeAH37ogF=lxkj3%|#r zC4sEj)-?I6z`A#8SDhcmym~%@v49rvvD)#qA#qK$)#uAvEw=hri#WrcC0S}?h>f;W z@l5X{(aUTHxzB@=<+hda@Aoc`UMZ2=26NwlgW2%OQi;M=);lM9uw=7sbUc2Gl_+gX zdJl@0!z^1P4T-bO%Sg*(8W?Tta}3&J37inu?twO3kGaf^uWYpsuE)z3TQ;cNM!2s@ zo2ehr&#(PP_N>|vHi-T1s~6~%=j(uT{LuPUt8HFZpr&(9-CQ_xyl;In!YD1t*bS!WhyF|P}B8xsI35t>g1_We^{OBZcDtP}1DTR{u z1FybpxqHVqJlxOU_a(nU(J9fBqEQArK`5ove17%tw11}7mO35y|1ehIF}7^kO<$%&h z@JGO#>o21IJJ1y~Qe;lhj@M4pW-Hez-&E!+-%=h_o>2Z>d0uuyc3;+uj3N(`C&;hF z_0g_8*P)Hr$IJ%sdWJsk4?pe~%;eXMHr!`Fxt;^VtHQ&zJb9m-+l<{mPXd>(_a#^r`S#>oeB-HJ>lM z-|$)NBlo%G<9L;`?&Y;BJeIG9GS@9z;jwPT*3B`w>sPPa+}k6~(*yn&Hf)mP^J&ND zWXC7Ib^jmX;nDT~1ox8&@bL0;ymCC@lkv~p-ge90{U;0AXnSX89h8ROL!ol~>~+_} z1D}1Qbv>V*HF@&X*|R;S&YY5+HS_tI>GNiKZ1wQi=8?lanlo$CbKIAwXQWSle)`O3 zxgRs9PMXDiNq>6woJo^sOyzz|oi!_+e>{EW9RACk=`T#>zs;IDd+MyYQ>S>Od(QV< z;knN951uN|LeCPKIE*_;g{wqkR3?)pv8Tz#**5yOW3nmqT(0nd)kl)rdu z>8ooRb0@w2=Jvcdm+tJ`Ib@T^-%^pL*3>^neh_&gl8*dK24kw>SDnDW=j%chL< k7~zrNVeuI5Vf5JT@s`IfkGDNaJxV+}J7YYG13WzbAK{1Mf&c&j literal 0 HcmV?d00001 diff --git a/projects/microb2010/tests/test_board2008/main.c b/projects/microb2010/tests/test_board2008/main.c new file mode 100755 index 0000000..732bd73 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/main.c @@ -0,0 +1,193 @@ +/* + * 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 + * (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: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "cmdline.h" +#include "sensor.h" +#include "actuator.h" +#include "cs.h" + +#if __AVR_LIBC_VERSION__ == 10602UL +//#error "won't work with this version" +#endif + +/* 0 means "programmed" + * ---- with 16 Mhz quartz + * CKSEL 3-0 : 0111 + * SUT 1-0 : 10 + * CKDIV8 : 1 + * ---- bootloader + * BOOTZ 1-0 : 01 (4K bootloader) + * BOOTRST : 0 (reset on bootloader) + * ---- jtag + * jtagen : 0 + */ + +struct genboard gen; +struct mainboard mainboard; + +void do_led_blink(void *dummy) +{ +#if 1 /* simple blink */ + LED1_TOGGLE(); +#endif +} + +static void main_timer_interrupt(void) +{ + static uint8_t cpt = 0; + static uint8_t encoder_running = 0; + + cpt++; + + /* BAAAAd */ + if (encoder_running) + return; + + encoder_running = 1; + sei(); + + encoders_microb_manage(NULL); + encoder_running = 0; + + if ((cpt & 0x3) == 0) + scheduler_interrupt(); +} + +int main(void) +{ + /* LEDs */ + DDRG = 0x18; + DDRB = 0x10; + + /* brake */ + BRAKE_DDR(); + BRAKE_OFF(); + + LED1_ON(); + LED2_ON(); + LED3_ON(); + + memset(&gen, 0, sizeof(gen)); + memset(&mainboard, 0, sizeof(mainboard)); + mainboard.flags = DO_ENCODERS | DO_RS | + DO_POS | DO_POWER | DO_BD; + + /* UART */ + uart_init(); +#if CMDLINE_UART == 0 + fdevopen(uart0_dev_send, uart0_dev_recv); + uart_register_rx_event(0, emergency); +#elif CMDLINE_UART == 1 + fdevopen(uart1_dev_send, uart1_dev_recv); + uart_register_rx_event(1, emergency); +#else +# error not supported +#endif + + /* LOGS */ + error_register_emerg(mylog); + error_register_error(mylog); + error_register_warning(mylog); + error_register_notice(mylog); + error_register_debug(mylog); + + encoders_microb_init(); + + /* TIMER */ + timer_init(); + timer0_register_OV_intr(main_timer_interrupt); + + /* PWM */ + PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, + TIMER1_PRESCALER_DIV_1); + PWM_NG_INIT16(&gen.pwm1_1A, 1, A, 10, PWM_NG_MODE_SIGNED, + &PORTG, 0); + PWM_NG_INIT16(&gen.pwm2_1B, 1, B, 10, PWM_NG_MODE_SIGNED, + &PORTG, 1); + + + /* servos */ + PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10, + TIMER1_PRESCALER_DIV_1); + PWM_NG_INIT16(&gen.pwm3_3A, 3, A, 10, PWM_NG_MODE_SIGNED, + &PORTG, 2); + PWM_NG_INIT16(&gen.pwm4_3B, 3, B, 10, PWM_NG_MODE_SIGNED | PWM_NG_MODE_SIGN_INVERTED, + &PORTD, 7); + + /* SCHEDULER */ + scheduler_init(); + + scheduler_add_periodical_event_priority(do_led_blink, NULL, + 100000L / SCHEDULER_UNIT, + LED_PRIO); + /* all cs management */ + microb_cs_init(); + + /* sensors, will also init hardware adc */ + sensor_init(); + fessor_init(); + elevator_init(); + + /* TIME */ + time_init(TIME_PRIO); + + /* strat */ + gen.logs[0] = E_USER_STRAT; + gen.log_level = 5; + + sei(); + + printf_P(PSTR("\r\n")); + printf_P(PSTR("Respekt Robustheit!\r\n")); + cmdline_interact(); + + return 0; +} diff --git a/projects/microb2010/tests/test_board2008/main.h b/projects/microb2010/tests/test_board2008/main.h new file mode 100755 index 0000000..157c79a --- /dev/null +++ b/projects/microb2010/tests/test_board2008/main.h @@ -0,0 +1,225 @@ +/* + * 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 + * (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: main.h,v 1.8 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#define LED_TOGGLE(port, bit) do { \ + if (port & _BV(bit)) \ + port &= ~_BV(bit); \ + else \ + port |= _BV(bit); \ + } while(0) + +#define LED1_ON() sbi(PORTB, 4) +#define LED1_OFF() cbi(PORTB, 4) +#define LED1_TOGGLE() LED_TOGGLE(PORTB, 4) + +#define LED2_ON() sbi(PORTG, 3) +#define LED2_OFF() cbi(PORTG, 3) +#define LED2_TOGGLE() LED_TOGGLE(PORTG, 3) + +#define LED3_ON() sbi(PORTG, 4) +#define LED3_OFF() cbi(PORTG, 4) +#define LED3_TOGGLE() LED_TOGGLE(PORTG, 4) + +#define BRAKE_DDR() do { DDRF |= 0x01; } while(0) +#define BRAKE_ON() do { PORTF |= 0x01; } while(0) +#define BRAKE_OFF() do { PORTF &= 0xFE; } while(0) + +/* only 90 seconds, don't forget it :) */ +#define MATCH_TIME 89 + +/* decrease track to decrease angle */ +#define EXT_TRACK_MM 302.0188 +#define VIRTUAL_TRACK_MM EXT_TRACK_MM + +#define ROBOT_LENGTH 320 +#define ROBOT_WIDTH 320 + +/* it is a 2048 imps -> 8192 because we see 1/4 period + * and diameter: 55mm -> perimeter 173mm + * 8192/173 -> 473 */ +/* increase it to go further */ +#define IMP_ENCODERS 2048 +#define WHEEL_DIAMETER_MM 55.0 +#define WHEEL_PERIM_MM (WHEEL_DIAMETER_MM * M_PI) +#define IMP_COEF 10. +#define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF) + +#define LEFT_ENCODER ((void *)1) +#define RIGHT_ENCODER ((void *)0) + +#define LEFT_PWM ((void *)&gen.pwm3_3A) +#define RIGHT_PWM ((void *)&gen.pwm4_3B) + +#define FESSOR_PWM ((void *)&gen.pwm2_1B) +#define FESSOR_ENC ((void *)2) + +#define WHEEL_PWM ((void *)&gen.pwm1_1A) +#define WHEEL_ENC ((void *)3) + +#define ELEVATOR_PWM ((void *)&gen.pwm3_3A) +#define ELEVATOR_ENC ((void *)0) + +/** ERROR NUMS */ +#define E_USER_STRAT 194 +#define E_USER_I2C_PROTO 195 +#define E_USER_SENSOR 196 +#define E_USER_CS 197 + +#define LED_PRIO 170 +#define TIME_PRIO 160 +#define ADC_PRIO 120 +#define FESSOR_PRIO 130 +#define ELEVATOR_PRIO 130 +#define CS_PRIO 100 +#define STRAT_PRIO 30 +#define I2C_POLL_PRIO 20 +#define EEPROM_TIME_PRIO 10 + +#define CS_PERIOD 5000L + +#define NB_LOGS 4 + +/* generic to all boards */ +struct genboard { + /* command line interface */ + struct rdline rdl; + char prompt[RDLINE_PROMPT_SIZE]; + + /* motors */ + struct pwm_ng pwm1_1A; + struct pwm_ng pwm2_1B; + struct pwm_ng pwm3_3A; + struct pwm_ng pwm4_3B; + + /* log */ + uint8_t logs[NB_LOGS+1]; + uint8_t log_level; + uint8_t debug; +}; + +struct cs_block { + uint8_t on; + struct cs cs; + struct pid_filter pid; + struct quadramp_filter qr; + struct blocking_detection bd; +}; + +/* mainboard specific */ +struct mainboard { +#define DO_ENCODERS 1 +#define DO_CS 2 +#define DO_RS 4 +#define DO_POS 8 +#define DO_BD 16 +#define DO_TIMER 32 +#define DO_POWER 64 + uint8_t flags; /* misc flags */ + + /* control systems */ + struct cs_block angle; + struct cs_block distance; + struct cs_block fessor; + struct cs_block wheel; + struct cs_block elevator; + + /* x,y positionning */ + struct robot_system rs; + struct robot_position pos; + struct trajectory traj; + + /* robot status */ + uint8_t our_color; + volatile int16_t speed_a; /* current angle speed */ + volatile int16_t speed_d; /* current dist speed */ + int32_t pwm_l; /* current left pwm */ + int32_t pwm_r; /* current right pwm */ + uint8_t enable_pickup_wheels; /* these PWM are on sensorboard */ + +}; + +/* state of mechboard, synchronized through i2c */ +struct mechboard { + uint8_t mode; + uint8_t status; + int8_t lintel_count; + uint8_t column_flags; + + /* pwm */ + int16_t pump_left1; + int16_t pump_right1; + int16_t pump_left2; + int16_t pump_right2; + + /* currents (for left arm, we can just read it on adc) */ + int16_t pump_right1_current; + int16_t pump_right2_current; + + /* pwm for lintel servos */ + uint16_t servo_lintel_left; + uint16_t servo_lintel_right; +}; + +/* state of sensorboard, synchronized through i2c */ +struct sensorboard { + uint8_t status; + /* opponent pos */ + int16_t opponent_x; + int16_t opponent_y; + int16_t opponent_a; + int16_t opponent_d; + + /* scanner */ +#define I2C_SCAN_DONE 1 + uint8_t scan_status; +#define I2C_COLUMN_NO_DROPZONE -1 + int8_t dropzone_h; + int16_t dropzone_x; + int16_t dropzone_y; +}; + +extern struct genboard gen; +extern struct mainboard mainboard; +extern struct mechboard mechboard; +extern struct sensorboard sensorboard; + +/* start the bootloader */ +void bootloader(void); + +#define WAIT_COND_OR_TIMEOUT(cond, timeout) \ +({ \ + microseconds __us = time_get_us2(); \ + uint8_t __ret = 1; \ + while(! (cond)) { \ + if (time_get_us2() - __us > (timeout)*1000L) {\ + __ret = 0; \ + break; \ + } \ + } \ + if (__ret) \ + DEBUG(E_USER_STRAT, "cond is true at line %d",\ + __LINE__); \ + else \ + DEBUG(E_USER_STRAT, "timeout at line %d", \ + __LINE__); \ + \ + __ret; \ +}) diff --git a/projects/microb2010/tests/test_board2008/pid_config.h b/projects/microb2010/tests/test_board2008/pid_config.h new file mode 100755 index 0000000..fa95f08 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/pid_config.h @@ -0,0 +1,30 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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 + * + * + * + */ + +#ifndef PID_CONFIG_H +#define PID_CONFIG_H + +/** the derivate term can be filtered to remove the noise. This value + * is the maxium sample count to keep in memory to do this + * filtering. For an instance of pid, this count is defined o*/ +#define PID_DERIVATE_FILTER_MAX_SIZE 4 + +#endif diff --git a/projects/microb2010/tests/test_board2008/rdline_config.h b/projects/microb2010/tests/test_board2008/rdline_config.h new file mode 100644 index 0000000..e69de29 diff --git a/projects/microb2010/tests/test_board2008/scheduler_config.h b/projects/microb2010/tests/test_board2008/scheduler_config.h new file mode 100755 index 0000000..daf5853 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/scheduler_config.h @@ -0,0 +1,47 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: scheduler_config.h,v 1.2 2009-03-15 20:18:33 zer0 Exp $ + * + */ + +#ifndef _SCHEDULER_CONFIG_H_ +#define _SCHEDULER_CONFIG_H_ + +#define _SCHEDULER_CONFIG_VERSION_ 4 + +/** maximum number of allocated events */ +#define SCHEDULER_NB_MAX_EVENT 10 + + +#define SCHEDULER_UNIT_FLOAT 512.0 +#define SCHEDULER_UNIT 512L + +/** number of allowed imbricated scheduler interrupts. The maximum + * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate + * more than once per event. If it is less, it can avoid to browse the + * event table, events are delayed (we loose precision) but it takes + * less CPU */ +#define SCHEDULER_NB_STACKING_MAX SCHEDULER_NB_MAX_EVENT + +/** define it for debug infos (not recommended, because very slow on + * an AVR, it uses printf in an interrupt). It can be useful if + * prescaler is very high, making the timer interrupt period very + * long in comparison to printf() */ +/* #define SCHEDULER_DEBUG */ + +#endif // _SCHEDULER_CONFIG_H_ diff --git a/projects/microb2010/tests/test_board2008/sensor.c b/projects/microb2010/tests/test_board2008/sensor.c new file mode 100644 index 0000000..0cb65ba --- /dev/null +++ b/projects/microb2010/tests/test_board2008/sensor.c @@ -0,0 +1,290 @@ +/* + * Copyright Droids Corporation (2009) + * 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 + * (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: sensor.c,v 1.6 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "sensor.h" + +/************ ADC */ + +struct adc_infos { + uint16_t config; + int16_t value; + int16_t prev_val; + int16_t (*filter)(struct adc_infos *, int16_t); +}; + +/* reach 90% of the value in 4 samples */ +int16_t rii_light(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + (int32_t)adc->prev_val / 2; + return adc->prev_val / 2; +} + +/* reach 90% of the value in 8 samples */ +int16_t rii_medium(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + ((int32_t)adc->prev_val * 3) / 4; + return adc->prev_val / 4; +} + +/* reach 90% of the value in 16 samples */ +int16_t rii_strong(struct adc_infos *adc, int16_t val) +{ + adc->prev_val = val + ((int32_t)adc->prev_val * 7) / 8; + return adc->prev_val / 8; +} + + +#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x ) + +/* 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, see init below */ +static void do_adc(void *dummy) +{ + /* launch first conversion */ + adc_launch(adc_infos[0].config); +} + +static void adc_event(int16_t result) +{ + static uint8_t i = 0; + + /* filter value if needed */ + if (adc_infos[i].filter) + adc_infos[i].value = adc_infos[i].filter(&adc_infos[i], + result); + else + adc_infos[i].value = result; + + i ++; + if (i >= ADC_MAX) + i = 0; + else + adc_launch(adc_infos[i].config); +} + +int16_t sensor_get_adc(uint8_t i) +{ + int16_t tmp; + uint8_t flags; + + IRQ_LOCK(flags); + tmp = adc_infos[i].value; + 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; + uint8_t invert; +}; + +/* pullup mapping: + * CAP 1,5,6,7,8 + */ +static struct sensor_filter sensor_filter[SENSOR_MAX] = { + [S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ + [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */ + [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */ + [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */ + [S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 0 */ + [S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ + [S_DISP_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 6 */ + [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ + [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ + [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ + [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */ + [S_RESERVED4] = { 1, 0, 0, 1, 0, 0 }, /* 11 */ + [S_RESERVED5] = { 1, 0, 0, 1, 0, 0 }, /* 12 */ + [S_RESERVED6] = { 1, 0, 0, 1, 0, 0 }, /* 13 */ + [S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */ + [S_RESERVED8] = { 1, 0, 0, 1, 0, 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 |= ((PINE & _BV(5)) >> 5); + +/* 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_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); +} + +/* virtual obstacle */ + +#define DISABLE_CPT_MAX 200 +static uint8_t disable = 0; /* used to disable obstacle detection + * during some time */ + +/* called every 10 ms */ +void +sensor_obstacle_update(void) +{ + if (disable > 0) { + disable --; + if (disable == 0) + DEBUG(E_USER_STRAT, "re-enable sensor"); + } +} + +void sensor_obstacle_disable(void) +{ + DEBUG(E_USER_STRAT, "disable sensor"); + disable = DISABLE_CPT_MAX; +} + +void sensor_obstacle_enable(void) +{ + disable = 0; +} + +uint8_t sensor_obstacle_is_disabled(void) +{ + return disable; +} + + +/************ global sensor init */ + +/* called every 10 ms, see init below */ +static void do_sensors(void *dummy) +{ + do_adc(NULL); + do_boolean_sensors(NULL); + sensor_obstacle_update(); +} + +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); +} + diff --git a/projects/microb2010/tests/test_board2008/sensor.h b/projects/microb2010/tests/test_board2008/sensor.h new file mode 100644 index 0000000..87c8bf5 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/sensor.h @@ -0,0 +1,62 @@ +/* + * Copyright Droids Corporation (2009) + * 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 + * (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: sensor.h,v 1.4 2009-04-24 19:30:41 zer0 Exp $ + * + */ + +/* 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_CAP1 0 +#define S_CAP2 1 +#define S_COLUMN_RIGHT 2 +#define S_COLUMN_LEFT 3 +#define S_START_SWITCH 4 +#define S_DISP_LEFT 5 +#define S_DISP_RIGHT 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); + +/* 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); + +void sensor_obstacle_disable(void); +void sensor_obstacle_enable(void); +uint8_t sensor_obstacle_is_disabled(void); diff --git a/projects/microb2010/tests/test_board2008/spi_config.h b/projects/microb2010/tests/test_board2008/spi_config.h new file mode 100644 index 0000000..76697c3 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/spi_config.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids Corporation (2008) + * + * 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 + * + */ + +/* + * Author : Julien LE GUEN - jlg@jleguen.info + */ + + +/* + * Configure HERE your SPI module + */ + + + +/* Number of slave devices in your system + * Each slave have a dedicated SS line that you have to register + * before using the SPI module + */ +#define SPI_MAX_SLAVES 1 + diff --git a/projects/microb2010/tests/test_board2008/strat_base.c b/projects/microb2010/tests/test_board2008/strat_base.c new file mode 100644 index 0000000..9945e2c --- /dev/null +++ b/projects/microb2010/tests/test_board2008/strat_base.c @@ -0,0 +1,271 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_base.c,v 1.6 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "cmdline.h" +#include "strat_base.h" +#include "sensor.h" + +/* true if we want to interrupt a trajectory */ +static uint8_t traj_intr=0; + +/* filled when a END_OBSTACLE is returned */ +struct opponent_obstacle opponent_obstacle; + +/* asked speed */ +static volatile int16_t strat_speed_a = SPEED_DIST_FAST; +static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST; +static volatile uint16_t strat_limit_speed_a = 0; /* no limit */ +static volatile uint16_t strat_limit_speed_d = 0; + +static volatile uint8_t strat_limit_speed_enabled = 1; + + +/* Strings that match the end traj cause */ +/* /!\ keep it sync with stat_base.h */ +const char *err_tab []= { + "END_TRAJ", + "END_BLOCKING", + "END_NEAR", + "END_OBSTACLE", + "END_ERROR", + "END_INTR", + "END_TIMER", + "END_RESERVED", +}; + +/* return string from end traj type num */ +const char *get_err(uint8_t err) +{ + uint8_t i; + if (err == 0) + return "SUCCESS"; + for (i=0 ; i<8; i++) { + if (err & (1 < 200) || + (ABS(mainboard.speed_a) > 200)) + + trajectory_hardstop(&mainboard.traj); + pid_reset(&mainboard.angle.pid); + pid_reset(&mainboard.distance.pid); + bd_reset(&mainboard.angle.bd); + bd_reset(&mainboard.distance.bd); +} + +/* reset position */ +void strat_reset_pos(int16_t x, int16_t y, int16_t a) +{ + int16_t posx = position_get_x_s16(&mainboard.pos); + int16_t posy = position_get_y_s16(&mainboard.pos); + int16_t posa = position_get_a_deg_s16(&mainboard.pos); + + if (x == DO_NOT_SET_POS) + x = posx; + if (y == DO_NOT_SET_POS) + y = posy; + if (a == DO_NOT_SET_POS) + a = posa; + + DEBUG(E_USER_STRAT, "reset pos (%s%s%s)", + x == DO_NOT_SET_POS ? "" : "x", + y == DO_NOT_SET_POS ? "" : "y", + a == DO_NOT_SET_POS ? "" : "a"); + position_set(&mainboard.pos, x, y, a); + DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__); +} + +/* + * decrease gain on angle PID, and go forward until we reach the + * border. + */ +uint8_t strat_calib(int16_t dist, uint8_t flags) +{ + int32_t p = pid_get_gain_P(&mainboard.angle.pid); + int32_t i = pid_get_gain_I(&mainboard.angle.pid); + int32_t d = pid_get_gain_D(&mainboard.angle.pid); + uint8_t err; + + pid_set_gains(&mainboard.angle.pid, 150, 0, 2000); + trajectory_d_rel(&mainboard.traj, dist); + err = wait_traj_end(flags); + pid_set_gains(&mainboard.angle.pid, p, i, d); + return err; +} + +static void strat_update_traj_speed(void) +{ + uint16_t d, a; + + d = strat_speed_d; + if (strat_limit_speed_d && d > strat_limit_speed_d) + d = strat_limit_speed_d; + a = strat_speed_a; + if (strat_limit_speed_a && a > strat_limit_speed_a) + a = strat_limit_speed_a; + + trajectory_set_speed(&mainboard.traj, d, a); +} + +void strat_set_speed(uint16_t d, uint16_t a) +{ + uint8_t flags; + IRQ_LOCK(flags); + strat_speed_d = d; + strat_speed_a = a; + strat_update_traj_speed(); + IRQ_UNLOCK(flags); +} + +void strat_get_speed(uint16_t *d, uint16_t *a) +{ + uint8_t flags; + IRQ_LOCK(flags); + *d = strat_speed_d; + *a = strat_speed_a; + IRQ_UNLOCK(flags); +} + + +void interrupt_traj(void) +{ + traj_intr = 1; +} + +void interrupt_traj_reset(void) +{ + traj_intr = 0; +} + +uint8_t test_traj_end(uint8_t why) +{ + uint16_t cur_timer; +/* point_t robot_pt; */ + +/* robot_pt.x = position_get_x_s16(&mainboard.pos); */ +/* robot_pt.y = position_get_y_s16(&mainboard.pos); */ + + /* trigger an event at 3 sec before the end of the match if we + * have balls in the barrel */ + cur_timer = time_get_s(); + + if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) { + /* end of match */ + if (cur_timer >= MATCH_TIME) + return END_TIMER; + } + + if ((why & END_INTR) && traj_intr) { + interrupt_traj_reset(); + return END_INTR; + } + + if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj)) + return END_TRAJ; + + /* we are near the destination point (depends on current + * speed) AND the robot is in the area bounding box. */ + if (why & END_NEAR) { + int16_t d_near = 100; + + if (mainboard.speed_d > 2000) + d_near = 150; + +/* if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && */ +/* is_in_boundingbox(&robot_pt)) */ +/* return END_NEAR; */ + } + + if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) { + strat_hardstop(); + return END_BLOCKING; + } + + if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) { + strat_hardstop(); + return END_BLOCKING; + } + +/* if ((why & END_OBSTACLE) && strat_obstacle()) { */ +/* strat_hardstop(); */ +/* return END_OBSTACLE; */ +/* } */ + + return 0; +} + +uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line) +{ + uint8_t ret = 0; +/* int16_t opp_x, opp_y, opp_d, opp_a; */ + + while (ret == 0) + ret = test_traj_end(why); + +/* if (ret == END_OBSTACLE) { */ +/* if (get_opponent_xyda(&opp_x, &opp_y, */ +/* &opp_d, &opp_a) == 0) */ +/* DEBUG(E_USER_STRAT, "Got %s at line %d" */ +/* " xy=(%d,%d) da=(%d,%d)", get_err(ret), */ +/* line, opp_x, opp_y, opp_d, opp_a); */ +/* } */ +/* else { */ +/* DEBUG(E_USER_STRAT, "Got %s at line %d", */ +/* get_err(ret), line); */ +/* } */ + return ret; +} diff --git a/projects/microb2010/tests/test_board2008/strat_base.h b/projects/microb2010/tests/test_board2008/strat_base.h new file mode 100644 index 0000000..8c15fa0 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/strat_base.h @@ -0,0 +1,120 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_base.h,v 1.3 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +/* return values for strats and sub trajs */ +#define END_TRAJ 1 /* traj successful */ +#define END_BLOCKING 2 /* blocking during traj */ +#define END_NEAR 4 /* we are near destination */ +#define END_OBSTACLE 8 /* There is an obstacle in front of us */ +#define END_ERROR 16 /* Cannot do the command */ +#define END_INTR 32 /* interrupted by user */ +#define END_TIMER 64 /* we don't a lot of time */ +#define END_RESERVED 128 /* reserved */ + +/* default speeds */ +#define SPEED_DIST_FAST 2500 +#define SPEED_ANGLE_FAST 2000 +#define SPEED_DIST_SLOW 1000 +#define SPEED_ANGLE_SLOW 1000 +#define SPEED_DIST_VERY_SLOW 400 +#define SPEED_ANGLE_VERY_SLOW 400 + +/* useful traj flags */ +#define TRAJ_SUCCESS(f) (f & (END_TRAJ|END_NEAR)) +#define TRAJ_FLAGS_STD (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR|END_TIMER) +#define TRAJ_FLAGS_NO_TIMER (END_TRAJ|END_BLOCKING|END_NEAR|END_OBSTACLE|END_INTR) +#define TRAJ_FLAGS_NO_NEAR (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR|END_TIMER) +#define TRAJ_FLAGS_NO_NEAR_NO_TIMER (END_TRAJ|END_BLOCKING|END_OBSTACLE|END_INTR) +#define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR) + +/* area */ +#define AREA_X 3000 +#define AREA_Y 2100 + +#define START_X 200 +#define START_Y 200 +#define START_A 45 + +#define CENTER_X 1500 +#define CENTER_Y 1050 + +#define CORNER_X 3000 +#define CORNER_Y 2100 + +/* only valid after a END_OBSTACLE */ +struct opponent_obstacle { + int16_t x; + int16_t y; + int16_t a; + int16_t d; +}; +extern struct opponent_obstacle opponent_obstacle; + +/* stop as fast as possible, without ramp */ +void strat_hardstop(void); + +#define DO_NOT_SET_POS -1000 +/* Reset position. If arg == DO_NOT_SET_POS, don't update value for + * it. */ +void strat_reset_pos(int16_t x, int16_t y, int16_t a); + +/* decrease gain on angle PID, and go forward until we reach the + * border. */ +uint8_t strat_calib(int16_t d, uint8_t flags); + +/* launch start procedure */ +void strat_start(void); + +/* go to an x,y point without checking for obstacle or blocking. It + * should be used for very small dist only. Return END_TRAJ if we + * reach destination, or END_BLOCKING if the robot blocked more than 3 + * times. */ +uint8_t strat_goto_xy_force(int16_t x, int16_t y); + +/* escape from disc polygon or another zone */ +struct build_zone; +uint8_t strat_escape(struct build_zone *zone, uint8_t flags); + +/* return true if we have to brake due to an obstacle */ +uint8_t strat_obstacle(void); + +/* set/get user strat speed */ +void strat_set_speed(uint16_t d, uint16_t a); +void strat_get_speed(uint16_t *d, uint16_t *a); + +/* when user type ctrl-c we can interrupt traj */ +void interrupt_traj(void); +void interrupt_traj_reset(void); + +/* limit speed when we are close of opponent */ +void strat_limit_speed_enable(void); +void strat_limit_speed_disable(void); +void strat_limit_speed(void); + +/* get name of traj error with its number */ +const char *get_err(uint8_t err); + +/* test trajectory end condition */ +uint8_t test_traj_end(uint8_t why); + +/* loop until test_traj_end() is true */ +#define wait_traj_end(why) __wait_traj_end_debug(why, __LINE__) +uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line); diff --git a/projects/microb2010/tests/test_board2008/strat_utils.c b/projects/microb2010/tests/test_board2008/strat_utils.c new file mode 100644 index 0000000..edd795a --- /dev/null +++ b/projects/microb2010/tests/test_board2008/strat_utils.c @@ -0,0 +1,404 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_utils.c,v 1.5 2009-05-02 10:08:09 zer0 Exp $ + * + */ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "main.h" +#include "strat_base.h" +#include "strat_utils.h" +#include "sensor.h" + +/* return the distance between two points */ +int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2) +{ + int32_t x,y; + x = (x2-x1); + x = x*x; + y = (y2-y1); + y = y*y; + return sqrt(x+y); +} + +/* return the distance to a point in the area */ +int16_t distance_from_robot(int16_t x, int16_t y) +{ + return distance_between(position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), x, y); +} + +/** do a modulo 360 -> [-180,+180], knowing that 'a' is in [-3*180,+3*180] */ +int16_t simple_modulo_360(int16_t a) +{ + if (a < -180) { + a += 360; + } + else if (a > 180) { + a -= 360; + } + return a; +} + +/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ +double simple_modulo_2pi(double a) +{ + if (a < -M_PI) { + a += M_2PI; + } + else if (a > M_PI) { + a -= M_2PI; + } + return a; +} + +/* return the distance to a point in the area */ +int16_t angle_abs_to_rel(int16_t a_abs) +{ + return simple_modulo_360(a_abs - position_get_a_deg_s16(&mainboard.pos)); +} + +void rel_da_to_abs_xy(double d_rel, double a_rel_rad, + double *x_abs, double *y_abs) +{ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + *x_abs = x + d_rel*cos(a+a_rel_rad); + *y_abs = y + d_rel*sin(a+a_rel_rad); +} + +double norm(double x, double y) +{ + return sqrt(x*x + y*y); +} + +void rel_xy_to_abs_xy(double x_rel, double y_rel, + double *x_abs, double *y_abs) +{ + double d_rel, a_rel; + d_rel = norm(x_rel, y_rel); + a_rel = atan2(y_rel, x_rel); + rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs); +} + +/* return an angle between -pi and pi */ +void abs_xy_to_rel_da(double x_abs, double y_abs, + double *d_rel, double *a_rel_rad) +{ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + *a_rel_rad = atan2(y_abs - y, x_abs - x) - a; + if (*a_rel_rad < -M_PI) { + *a_rel_rad += M_2PI; + } + else if (*a_rel_rad > M_PI) { + *a_rel_rad -= M_2PI; + } + *d_rel = norm(x_abs-x, y_abs-y); +} + +void rotate(double *x, double *y, double rot) +{ + double l, a; + + l = norm(*x, *y); + a = atan2(*y, *x); + + a += rot; + *x = l * cos(a); + *y = l * sin(a); +} + +/* return true if the point is in area */ +uint8_t is_in_area(int16_t x, int16_t y, int16_t margin) +{ + if (x < margin) + return 0; + if (x > (AREA_X - margin)) + return 0; + if (y < margin) + return 0; + if (y > (AREA_Y - margin)) + return 0; + return 1; +} + + +/* return true if the point is in area */ +uint8_t robot_is_in_area(int16_t margin) +{ + return is_in_area(position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + margin); +} + +/* /\* return true if we are near the disc *\/ */ +/* uint8_t robot_is_near_disc(void) */ +/* { */ +/* if (distance_from_robot(CENTER_X, CENTER_Y) < DISC_PENTA_DIAG) */ +/* return 1; */ +/* return 0; */ +/* } */ + +/* /\* return 1 or 0 depending on which side of a line (y=cste) is the */ +/* * robot. works in red or green color. *\/ */ +/* uint8_t y_is_more_than(int16_t y) */ +/* { */ +/* int16_t posy; */ + +/* posy = position_get_y_s16(&mainboard.pos); */ +/* if (mainboard.our_color == I2C_COLOR_RED) { */ +/* if (posy > y) */ +/* return 1; */ +/* else */ +/* return 0; */ +/* } */ +/* else { */ +/* if (posy < (AREA_Y-y)) */ +/* return 1; */ +/* else */ +/* return 0; */ +/* } */ +/* } */ + +/* return 1 or 0 depending on which side of a line (x=cste) is the + * robot. works in red or green color. */ +uint8_t x_is_more_than(int16_t x) +{ + int16_t posx; + + posx = position_get_x_s16(&mainboard.pos); + if (posx > x) + return 1; + else + return 0; +} + +int16_t sin_table[] = { + 0, + 3211, + 6392, + 9512, + 12539, + 15446, + 18204, + 20787, + 23170, + 25330, + 27245, + 28898, + 30273, + 31357, + 32138, + 32610, + 32767, +}; + +int16_t fast_sin(int16_t deg) +{ + deg %= 360; + + if (deg < 0) + deg += 360; + + if (deg < 90) + return sin_table[(deg*16)/90]; + else if (deg < 180) + return sin_table[((180-deg)*16)/90]; + else if (deg < 270) + return -sin_table[((deg-180)*16)/90]; + else + return -sin_table[((360-deg)*16)/90]; +} + +int16_t fast_cos(int16_t deg) +{ + return fast_sin(90+deg); +} + + +/* get the color of our robot */ +uint8_t get_color(void) +{ + return mainboard.our_color; +} + +/* /\* get the color of the opponent robot *\/ */ +/* uint8_t get_opponent_color(void) */ +/* { */ +/* if (mainboard.our_color == I2C_COLOR_RED) */ +/* return I2C_COLOR_GREEN; */ +/* else */ +/* return I2C_COLOR_RED; */ +/* } */ + +/* /\* get the xy pos of the opponent robot *\/ */ +/* int8_t get_opponent_xy(int16_t *x, int16_t *y) */ +/* { */ +/* uint8_t flags; */ +/* IRQ_LOCK(flags); */ +/* *x = sensorboard.opponent_x; */ +/* *y = sensorboard.opponent_y; */ +/* IRQ_UNLOCK(flags); */ +/* if (*x == I2C_OPPONENT_NOT_THERE) */ +/* return -1; */ +/* return 0; */ +/* } */ + +/* /\* get the da pos of the opponent robot *\/ */ +/* int8_t get_opponent_da(int16_t *d, int16_t *a) */ +/* { */ +/* uint8_t flags; */ +/* int16_t x_tmp; */ +/* IRQ_LOCK(flags); */ +/* x_tmp = sensorboard.opponent_x; */ +/* *d = sensorboard.opponent_d; */ +/* *a = sensorboard.opponent_a; */ +/* IRQ_UNLOCK(flags); */ +/* if (x_tmp == I2C_OPPONENT_NOT_THERE) */ +/* return -1; */ +/* return 0; */ +/* } */ + +/* /\* get the da pos of the opponent robot *\/ */ +/* int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a) */ +/* { */ +/* uint8_t flags; */ +/* IRQ_LOCK(flags); */ +/* *x = sensorboard.opponent_x; */ +/* *y = sensorboard.opponent_y; */ +/* *d = sensorboard.opponent_d; */ +/* *a = sensorboard.opponent_a; */ +/* IRQ_UNLOCK(flags); */ +/* if (*x == I2C_OPPONENT_NOT_THERE) */ +/* return -1; */ +/* return 0; */ +/* } */ + +/* uint8_t pump_left1_is_full(void) */ +/* { */ +/* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L1) && */ +/* (sensor_get_adc(ADC_CSENSE3) > I2C_MECHBOARD_CURRENT_COLUMN)); */ +/* } */ + +/* uint8_t pump_left2_is_full(void) */ +/* { */ +/* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_L2) && */ +/* (sensor_get_adc(ADC_CSENSE4) > I2C_MECHBOARD_CURRENT_COLUMN)); */ +/* } */ + +/* uint8_t pump_right1_is_full(void) */ +/* { */ +/* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R1) && */ +/* (mechboard.pump_right1_current > I2C_MECHBOARD_CURRENT_COLUMN)); */ +/* } */ + +/* uint8_t pump_right2_is_full(void) */ +/* { */ +/* return !!( (mechboard.column_flags & I2C_MECHBOARD_COLUMN_R2) && */ +/* (mechboard.pump_right2_current > I2C_MECHBOARD_CURRENT_COLUMN)); */ +/* } */ + +/* /\* number of column owned by the robot *\/ */ +/* uint8_t get_column_count_left(void) */ +/* { */ +/* uint8_t ret = 0; */ +/* ret += pump_left1_is_full(); */ +/* ret += pump_left2_is_full(); */ +/* return ret; */ +/* } */ + +/* /\* number of column owned by the robot *\/ */ +/* uint8_t get_column_count_right(void) */ +/* { */ +/* uint8_t ret = 0; */ +/* ret += pump_right1_is_full(); */ +/* ret += pump_right2_is_full(); */ +/* return ret; */ +/* } */ + +/* /\* number of column owned by the robot *\/ */ +/* uint8_t get_column_count(void) */ +/* { */ +/* uint8_t ret = 0; */ +/* ret += pump_left1_is_full(); */ +/* ret += pump_left2_is_full(); */ +/* ret += pump_right1_is_full(); */ +/* ret += pump_right2_is_full(); */ +/* return ret; */ +/* } */ + +/* uint8_t get_lintel_count(void) */ +/* { */ +/* return mechboard.lintel_count; */ +/* } */ + +/* uint8_t get_mechboard_mode(void) */ +/* { */ +/* return mechboard.mode; */ +/* } */ + +/* uint8_t get_scanner_status(void) */ +/* { */ +/* return sensorboard.scan_status; */ +/* } */ + +/* /\* return 0 if timeout, or 1 if cond is true *\/ */ +/* uint8_t wait_scan_done(uint16_t timeout) */ +/* { */ +/* uint8_t err; */ +/* err = WAIT_COND_OR_TIMEOUT(get_scanner_status() & I2C_SCAN_DONE, timeout); */ +/* return err; */ +/* } */ + +/* uint8_t opponent_is_behind(void) */ +/* { */ +/* int8_t opp_there; */ +/* int16_t opp_d, opp_a; */ + +/* opp_there = get_opponent_da(&opp_d, &opp_a); */ +/* if (opp_there && (opp_a < 215 && opp_a > 145) && opp_d < 600) */ +/* return 1; */ +/* return 0; */ +/* } */ diff --git a/projects/microb2010/tests/test_board2008/strat_utils.h b/projects/microb2010/tests/test_board2008/strat_utils.h new file mode 100644 index 0000000..2bfa35a --- /dev/null +++ b/projects/microb2010/tests/test_board2008/strat_utils.h @@ -0,0 +1,76 @@ +/* + * Copyright Droids Corporation, Microb Technology (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 + * (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: strat_utils.h,v 1.3 2009-04-24 19:30:41 zer0 Exp $ + * + */ + + +#define DEG(x) (((double)(x)) * (180.0 / M_PI)) +#define RAD(x) (((double)(x)) * (M_PI / 180.0)) +#define M_2PI (2*M_PI) + +struct xy_point { + int16_t x; + int16_t y; +}; + +/* wait traj end flag or cond. return 0 if cond become true, else + * return the traj flag */ +#define WAIT_COND_OR_TRAJ_END(cond, mask) \ + ({ \ + uint8_t __err = 0; \ + while ( (! (cond)) && (__err == 0)) { \ + __err = test_traj_end(TRAJ_FLAGS_NO_NEAR); \ + } \ + __err; \ + }) \ + +int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2); +int16_t distance_from_robot(int16_t x, int16_t y); +int16_t simple_modulo_360(int16_t a); +double simple_modulo_2pi(double a); +int16_t angle_abs_to_rel(int16_t a_abs); +void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs); +double norm(double x, double y); +void rel_xy_to_abs_xy(double x_rel, double y_rel, double *x_abs, double *y_abs); +void abs_xy_to_rel_da(double x_abs, double y_abs, double *d_rel, double *a_rel_rad); +void rotate(double *x, double *y, double rot); +uint8_t is_in_area(int16_t x, int16_t y, int16_t margin); +uint8_t robot_is_in_area(int16_t margin); +uint8_t robot_is_near_disc(void); +uint8_t y_is_more_than(int16_t y); +uint8_t x_is_more_than(int16_t x); +int16_t fast_sin(int16_t deg); +int16_t fast_cos(int16_t deg); +uint8_t get_color(void); +uint8_t get_opponent_color(void); +int8_t get_opponent_xy(int16_t *x, int16_t *y); +int8_t get_opponent_da(int16_t *d, int16_t *a); +int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a); +uint8_t get_column_count(void); +uint8_t get_column_count_right(void); +uint8_t get_column_count_left(void); +uint8_t get_lintel_count(void); +uint8_t get_mechboard_mode(void); +uint8_t pump_left1_is_full(void); +uint8_t pump_left2_is_full(void); +uint8_t pump_right1_is_full(void); +uint8_t pump_right2_is_full(void); +uint8_t get_scanner_status(void); +uint8_t wait_scan_done(uint16_t timeout); +uint8_t opponent_is_behind(void); diff --git a/projects/microb2010/tests/test_board2008/time_config.h b/projects/microb2010/tests/test_board2008/time_config.h new file mode 100755 index 0000000..14db608 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/time_config.h @@ -0,0 +1,23 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: time_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $ + * + */ + +/** precision of the time processor, in us */ +#define TIME_PRECISION 25000l diff --git a/projects/microb2010/tests/test_board2008/timer_config.h b/projects/microb2010/tests/test_board2008/timer_config.h new file mode 100755 index 0000000..47d9f18 --- /dev/null +++ b/projects/microb2010/tests/test_board2008/timer_config.h @@ -0,0 +1,36 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2006) + * + * 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: timer_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $ + * + */ + +#define TIMER0_ENABLED + +/* #define TIMER1_ENABLED */ +/* #define TIMER1A_ENABLED */ +/* #define TIMER1B_ENABLED */ +/* #define TIMER1C_ENABLED */ + +/* #define TIMER2_ENABLED */ + +/* #define TIMER3_ENABLED */ +/* #define TIMER3A_ENABLED */ +/* #define TIMER3B_ENABLED */ +/* #define TIMER3C_ENABLED */ + +#define TIMER0_PRESCALER_DIV 8 diff --git a/projects/microb2010/tests/test_board2008/uart_config.h b/projects/microb2010/tests/test_board2008/uart_config.h new file mode 100644 index 0000000..d230caf --- /dev/null +++ b/projects/microb2010/tests/test_board2008/uart_config.h @@ -0,0 +1,65 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * 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: uart_config.h,v 1.3 2009-05-02 10:08:09 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* + * UART0 definitions + */ + +/* compile uart0 fonctions, undefine it to pass compilation */ +#define UART0_COMPILE + +/* enable uart0 if == 1, disable if == 0 */ +#define UART0_ENABLED 1 + +/* enable uart0 interrupts if == 1, disable if == 0 */ +#define UART0_INTERRUPT_ENABLED 1 + +#define UART0_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART0_USE_DOUBLE_SPEED 1 + +#define UART0_RX_FIFO_SIZE 64 +#define UART0_TX_FIFO_SIZE 127 +#define UART0_NBITS 8 + +#define UART0_PARITY UART_PARTITY_NONE + +#define UART0_STOP_BIT UART_STOP_BITS_1 + + + + + +/* .... same for uart 1, 2, 3 ... */ + +#endif + -- 2.20.1