| # Copyright (c) 2013 The Chromium OS Authors. All rights reserved. |
| # Use of this source code is governed by a BSD-style license that can be |
| # found in the LICENSE file. |
| |
| """ Go and poke at the board to a constant pressure in an even grid |
| covering the surface. It picks a point and descends until the limit switch |
| is triggered, resulting in consistent pressure across the samples. |
| |
| To setup, make sure the limit switch is connected to the brown/white wire |
| pair, as that is the switch that is polled. |
| """ |
| |
| import sys |
| |
| import roibot |
| import run_program |
| |
| # Book keeping for the robots internal registers |
| GPIO_COUNTER = 1 |
| DESCEND_LOOP_START = 1 |
| DESCEND_LOOP_END = 2 |
| |
| ROW_LOOP_COUNTER = 2 |
| ROW_LOOP_START = 3 |
| ROW_LOOP_END = 4 |
| |
| COL_LOOP_COUNTER = 3 |
| COL_LOOP_START = 5 |
| COL_LOOP_END = 6 |
| |
| RETURN_LOOP_COUNTER = 4 |
| RETURN_LOOP_START = 7 |
| RETURN_LOOP_END = 8 |
| |
| # Configuration parameters for the grid |
| DESCEND_STEP_SIZE = 0.1 |
| SPEED = 60 |
| STEP_SIZE = 4 |
| BACKOFF_AMOUNT = -4 |
| |
| |
| def descend(robot): |
| """ Inch down tiny amounts until the limit switch is triggered """ |
| robot.addCmd(roibot.program.setTag(DESCEND_LOOP_START)) |
| |
| # Move down a tiny bit |
| mov_cmd = roibot.program.moveCoordinates( |
| False, 0, 0, DESCEND_STEP_SIZE, 0, True, 0, False) |
| robot.addCmd(mov_cmd) |
| |
| # Copy the value of the GPIOs to the GPIO counter |
| robot.addCmd(roibot.program.copyInputToCounter(0, 1, GPIO_COUNTER)) |
| |
| # If the button was pressed, jump out of the loop |
| jmp_cmd = roibot.program.jumpCounterConditional( |
| DESCEND_LOOP_END, GPIO_COUNTER, '=', 0b00001000) |
| robot.addCmd(jmp_cmd) |
| |
| # Otherwise jump back to the start |
| robot.addCmd(roibot.program.jump(DESCEND_LOOP_START)) |
| |
| robot.addCmd(roibot.program.setTag(DESCEND_LOOP_END)) |
| |
| |
| def probe_row(robot, bounds): |
| """ probe a lot of points along a y-row of the surface """ |
| num_steps = int((bounds.maxY() - bounds.minY()) / STEP_SIZE) |
| |
| # Initialize loop and counter |
| robot.addCmd(roibot.program.setCounter(ROW_LOOP_COUNTER, 0)) |
| robot.addCmd(roibot.program.setTag(ROW_LOOP_START)) |
| |
| # Drop down and take a reading |
| descend(robot) |
| |
| # Lift back up |
| raise_cmd = roibot.program.moveCoordinates( |
| False, 0, 0, BACKOFF_AMOUNT, 0, True, 0, True) |
| robot.addCmd(raise_cmd) |
| |
| # Move one step along |
| mov_cmd = roibot.program.moveCoordinates( |
| False, 0, STEP_SIZE, 0, 0, True, 0, True) |
| robot.addCmd(mov_cmd) |
| |
| # Decrement counter, if it's num_steps then you jump out of the loop |
| robot.addCmd(roibot.program.incrementCounter(ROW_LOOP_COUNTER, 1)) |
| |
| # Otherwise jump back to the start |
| jmp_cmd = roibot.program.jumpCounterConditional( |
| ROW_LOOP_END, ROW_LOOP_COUNTER, '=', num_steps) |
| robot.addCmd(jmp_cmd) |
| robot.addCmd(roibot.program.jump(ROW_LOOP_START)) |
| |
| robot.addCmd(roibot.program.setTag(ROW_LOOP_END)) |
| |
| # Go back to the start before returning, by moving the same number of |
| # steps backwards |
| robot.addCmd(roibot.program.setTag(RETURN_LOOP_START)) |
| |
| # Take one step backwards |
| mov_cmd = roibot.program.moveCoordinates( |
| False, 0, -STEP_SIZE, 0, 0, True, 0, True) |
| robot.addCmd(mov_cmd) |
| |
| # Count how many steps you have left to take. If it's done, leave the loop |
| robot.addCmd(roibot.program.decrementCounter(ROW_LOOP_COUNTER, 1)) |
| jmp_cmd = roibot.program.jumpCounterConditional( |
| RETURN_LOOP_END, ROW_LOOP_COUNTER, '=', 0) |
| robot.addCmd(jmp_cmd) |
| |
| # Otherwise, take another step |
| robot.addCmd(roibot.program.jump(RETURN_LOOP_START)) |
| |
| robot.addCmd(roibot.program.setTag(RETURN_LOOP_END)) |
| |
| |
| def write_program(robot, bounds, *args, **kwargs): |
| robot.addMoveCommand(bounds.minX(), bounds.minY(), bounds.upZ(), SPEED) |
| |
| num_steps = int((bounds.maxX() - bounds.minX()) / STEP_SIZE) |
| |
| # Initialize loop and counter |
| robot.addCmd(roibot.program.setCounter(COL_LOOP_COUNTER, num_steps)) |
| robot.addCmd(roibot.program.setTag(COL_LOOP_START)) |
| |
| probe_row(robot, bounds) |
| |
| # Move one step along to the next row |
| mov_cmd = roibot.program.moveCoordinates( |
| False, STEP_SIZE, 0, 0, 0, True, 0, True) |
| robot.addCmd(mov_cmd) |
| |
| # Decrement counter, if it's 0 then you jump out of the loop |
| robot.addCmd(roibot.program.decrementCounter(COL_LOOP_COUNTER, 1)) |
| jmp_cmd = roibot.program.jumpCounterConditional( |
| COL_LOOP_END, COL_LOOP_COUNTER, '=', 0) |
| robot.addCmd(jmp_cmd) |
| |
| # Otherwise, go back to the beginning |
| robot.addCmd(roibot.program.jump(COL_LOOP_START)) |
| |
| robot.addCmd(roibot.program.setTag(COL_LOOP_END)) |
| |
| |
| if __name__ == '__main__': |
| if len(sys.argv) != 2: |
| print 'Usage: ./pressure_map.py device_name' |
| else: |
| device = sys.argv[1] |
| run_program.run_program(write_program, device) |