From cb5f1745c8dd1475898c7d2f85d67813b1d3c457 Mon Sep 17 00:00:00 2001 From: Nick Van Doorn Date: Tue, 5 Mar 2019 14:34:08 -0800 Subject: Initial commit --- Makefile | 18 ++++ main.c | 295 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ watcher.sh | 9 ++ 3 files changed, 322 insertions(+) create mode 100644 Makefile create mode 100644 main.c create mode 100755 watcher.sh diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..95c5774 --- /dev/null +++ b/Makefile @@ -0,0 +1,18 @@ +# lift life +# http://www.cs.colby.edu/maxwell/courses/tutorials/maketutor/makefile.4 +CC=gcc +CFLAGS=-I. -Wall +DEPS= +OBJ= +MAIN_OBJ = main.o + +%.o: %.c $(DEPS) + $(CC) -c -o $@ $< $(CFLAGS) + +main: $(OBJ) $(MAIN_OBJ) + $(CC) -o $@ $^ $(CFLAGS) + +.PHONY: clean +clean: + rm -rf *.o + diff --git a/main.c b/main.c new file mode 100644 index 0000000..359c096 --- /dev/null +++ b/main.c @@ -0,0 +1,295 @@ +#include +#include +#include +#include + +#define FAKE_SENSOR_VAL 50 +#define MOTOR_OFFSET_RIGHT 0 +#define MOTOR_OFFSET_LEFT 0 + +#define POWER_LEVEL_LOW 30 +#define POWER_LEVEL_MED 50 +#define POWER_LEVEL_HIGH 70 +#define SEND_IT 100 + +#define IR_THRESHOLD_STRAIGHT 50 +#define IR_THRESHOLD_HAS_SIGNAL 200 +#define MIN_FREE_FORWARD_SPACE 50 // cm + +#define MOVE_TIME_SEARCH 1000 + +#define POWER_LEVEL_APPROACH 20 +#define MOVE_TIME_APPROACH 500 + +enum RobotState_t { + // right at the start before + // we do anything + IDLE = 0, + // This is the state we'll likely spend + // the most time in (looking for the beacon) + SEARCHING = 1, + // When we're near the beacon + // but not quite aligned for a drop yet + ALIGNMENT = 2, + // In position to drop the object + DROP_OBJECT = 3, + // signal that we're done somehow + // and stop moving + COMPLETE = 4, + // special state that we use to recover + // from collisions (can be thought of + // like an interrupt state) + COLLISION = 5 +}; + +enum RobotCorrection_t { MOVE_STRAIGHT = 0, MOVE_RIGHT = 1, MOVE_LEFT = 2 }; + +// use a global +struct RobotContext_t { + enum RobotState_t state; + enum RobotState_t prevState; +}; + +void waitMs(int waitTimeMs) { usleep(waitTimeMs * 1000); } + +char *stateToString(enum RobotState_t state) { + switch (state) { + case IDLE: + return "IDLE"; + case SEARCHING: + return "SEARCHING"; + case ALIGNMENT: + return "ALIGNMENT"; + case DROP_OBJECT: + return "DROP_OBJECT"; + case COMPLETE: + return "COMPLETE"; + case COLLISION: + return "COLLISION"; + default: + return "STATE OUT OF BOUNDS"; + } +} + +void setMotor(int motorNumber, int powerLevel) { + printf("Setting motor #%d to %d power level", motorNumber, powerLevel); +} + +int readSensor(char *sensorName) { + printf("Reading from sensor %s\n", sensorName); + return FAKE_SENSOR_VAL; +} + +int readIrRight() { return readSensor("Photo Transistor Right"); } + +int readIrLeft() { return readSensor("Photo Transistor Left"); } + +enum RobotCorrection_t readIrDirection() { + int irRight, irLeft, irDiff; + bool diffInRange, hasRightSignal, hasLeftSignal; + irRight = readIrRight(); + irLeft = readIrLeft(); + irDiff = irLeft - irRight; + diffInRange = irDiff < IR_THRESHOLD_STRAIGHT; + hasRightSignal = irRight > IR_THRESHOLD_HAS_SIGNAL; + hasLeftSignal = irLeft > IR_THRESHOLD_HAS_SIGNAL; + if (diffInRange && hasRightSignal && hasLeftSignal) + return MOVE_STRAIGHT; + else if (irDiff < 0) { + return MOVE_RIGHT; + } else { + return MOVE_LEFT; + } +} + +int readForwardDistance() { return readSensor("Sonic Distance Sensor"); } + +// TODO +// normal comment +void sonicSensorCorrection() {} + +void dropObject() { printf("Dropping object...\n"); } + +void controlDriveMotors(int powerLevelLeft, int powerLevelRight) { + // motor "1" is the left wheel + setMotor(1, powerLevelLeft + MOTOR_OFFSET_LEFT); + // motor "2" is the right wheel; + setMotor(2, powerLevelRight + MOTOR_OFFSET_RIGHT); +} + +void driveStraight(int powerLevel, int time) { + controlDriveMotors(powerLevel, powerLevel); + waitMs(time); + controlDriveMotors(0, 0); +} + +void turnRight(int powerLevel, int time) { + controlDriveMotors(powerLevel, -powerLevel); + waitMs(time); + controlDriveMotors(0, 0); +} + +void turnLeft(int powerLevel, int time) { + controlDriveMotors(-powerLevel, powerLevel); + waitMs(time); + controlDriveMotors(0, 0); +} + +// TODO +bool checkForCollisions() { return false; } + +void waitForStartButton() { + char c; + while (true) { + c = getchar(); + if (c == 'n') { + return; + } + } +} + +void handleIdle(struct RobotContext_t *ctx) { + // wait for start button + waitForStartButton(); + // and then go into our "main" + // searching state + ctx->state = SEARCHING; +} + +bool shouldLeaveSearching() { return true; } + +/** + * How do we want our search to work? + * + * Each time we run this, + * we'll follow roughly the steps below + * + * 1. Check how much space is directly ahead of us. + * If there is only a small amount (a few cm or less), + * we should turn until we have more "search space". + * This makes sure we're never pointed at a wall + * + * 2. Look at the difference between the readings + * on the two phototransistors. + * + * If the diff ~ 0 and signal levels meet a + * certain minimum threshold, then we should proceed + * straight for a short distance + * + * If the diff > 0, we should turn until + * this difference gets closer. If it strats increasing, + * back track and turn the other way. + */ +void handleSearching(struct RobotContext_t *ctx) { + enum RobotCorrection_t move; + if (readForwardDistance() < MIN_FREE_FORWARD_SPACE) { + sonicSensorCorrection(); + } + move = readIrDirection(); + switch (move) { + case MOVE_STRAIGHT: + driveStraight(POWER_LEVEL_MED, MOVE_TIME_SEARCH); + break; + case MOVE_RIGHT: + turnRight(POWER_LEVEL_LOW, MOVE_TIME_SEARCH); + break; + case MOVE_LEFT: + turnLeft(POWER_LEVEL_LOW, MOVE_TIME_SEARCH); + break; + } + if (shouldLeaveSearching()) { + ctx->state = ALIGNMENT; + } +}; + +/** + * This state is reponsible for the approach, + * so if things get too inaccurate, you kick back + * to the SEARCHING state + * + * TODO + */ +void handleAlignment(struct RobotContext_t *ctx) { + enum RobotCorrection_t move = readIrDirection(); + // if we aren't still aligned + // to move straight, kick back + // to the searching state, + // and bail hard on this + if (move != MOVE_STRAIGHT) { + ctx->state = SEARCHING; + return; + } +} + +void handleDrop(struct RobotContext_t *ctx) { + dropObject(); + waitMs(1000); + driveStraight(-POWER_LEVEL_MED, 2000); +} + +void handleComplete(struct RobotContext_t *ctx) { printf("Done!\n"); } + +void handleCollision(struct RobotContext_t *ctx) { + // we must search again before returning + // to the alignment state after recovering from + // a collision + if (ctx->prevState == ALIGNMENT) { + ctx->state = SEARCHING; + } else { + ctx->state = ctx->prevState; + } +} + +void startStateMachine(struct RobotContext_t *ctx) { + bool inCollisionState; + while (true) { + printf("Current state %s\n", stateToString(ctx->state)); + // we assume one of routines called in the switch + // statement changes ctx->state, so we make a backup + // of it before any of those routines run + ctx->prevState = ctx->state; + inCollisionState = ctx->state == COLLISION; + if (checkForCollisions() && !inCollisionState) { + ctx->state = COLLISION; + } + switch (ctx->state) { + case IDLE: + handleIdle(ctx); + break; + case SEARCHING: + handleSearching(ctx); + break; + case ALIGNMENT: + handleAlignment(ctx); + break; + case DROP_OBJECT: + handleDrop(ctx); + break; + case COMPLETE: + handleComplete(ctx); + break; + case COLLISION: + handleCollision(ctx); + break; + } + } +} +/** + * Problem description: + * + * 1. Robot must locate infrared beacon + * 2. Get within a certain distance to the infrared beacon + * 3. Drop object on beacon + * 4. Signal completion somehow + * + * * must avoid walls as best possible + */ +int main() { + // RobotContext_t refers to the "state of the world" + // and "context of our robot" in our current enviornment + + // initial state is idle + struct RobotContext_t ctx = {.state = IDLE}; + struct RobotContext_t *ctxPtr = &ctx; + startStateMachine(ctxPtr); +} diff --git a/watcher.sh b/watcher.sh new file mode 100755 index 0000000..00c7b82 --- /dev/null +++ b/watcher.sh @@ -0,0 +1,9 @@ +#!/usr/bin/env bash + +buildRun() { + make && ./main +} + + +test +while sleep 5; do buildRun; done -- cgit v1.2.3