robot tour code 24-25

main.ino

#include <math.h>

/**
 * TODO:
    * Implement mechanism for strafing.
    * Find the fastest way to rotate (i.e. 270 -> 0 moves 270deg left instead of 90deg right).
*/ 

/** A vector class which represents an (x, y) Cartesian vector. */
class Vector {
public:
    double x, y;

    Vector() : x(0), y(0) {}
    Vector(double x_val, double y_val) : x(x_val), y(y_val) {}

    void set(double x_val, double y_val) {
        x = x_val;
        y = y_val;
    }

    double distance_to(const Vector& other) const {
        return sqrt(pow(other.x - x, 2) + pow(other.y - y, 2));
    }
};

/** Pin constants. */
#define ENCODER_LEFT    2
#define ENCODER_RIGHT   3
#define MOTOR1_DIR      4
#define MOTOR1_PWM      5
#define MOTOR2_DIR      6
#define MOTOR2_PWM      7
#define MOTOR3_DIR      8
#define MOTOR3_PWM      9
#define MOTOR4_DIR      10
#define MOTOR4_PWM      11

/**
  * These constants are defined based on the robot's geometry. Measure
  * and calibrate them before the competition.

  * WHEEL_RADIUS_CM: The radius of the wheel, in cm.
  * TICKS_PER_REV: The number of encoder ticks that occur during one wheel revolution.
  * WHEEL_BASE_CM: The distance between the two wheels, in cm.
  * THRESHOLD_CM: The threshold for equality between current and target positions.
  * THRESHOLD_ANGLE: The threshold for equality between current and target angles.
  * ROTATION_SPEED: How fast the robot should be moving while rotating.
  * FORWARD_SPEED: How fast the robot should be moving while moving forward.
*/

#define WHEEL_RADIUS_CM    3.0
#define TICKS_PER_REV      400
#define WHEEL_BASE_CM      15.0
#define THRESHOLD_CM       1.0
#define THRESHOLD_ANGLE    5.0
#define ROTATION_SPEED     100
#define FORWARD_SPEED      150

/**
  * These are global mutable variables which direct how the robot should work.
  
  * The strategy is that there is a current and target position/angle which are
  * initialized at <0, 0> and 0^{\circ}. Based on the number of left and right encoder
  * ticks, you can compute a linear and angular velocity to be added to the current
  * position and angle. When parsing an instruction, the robot can be directed to rotate.
  * to a specific angle (which is set as the "target" angle). As it rotates, the current angle changes.
  * Once it is within the target angle (specified by THRESHOLD_ANGLE), it will stop rotating and
  * then handle the position part, which will be done by moving forward until current_position is
  * in range of the target position. Then, the next instruction will be handled until completion. 
*/

Vector current_position;
Vector target_position;
double current_angle = 0;
double target_angle = 0;
int left_encoder_ticks = 0;
int right_encoder_ticks = 0;

/** An array of instructions for the robot to use. */
const char *instructions[] = {"up 50", "right 30", "down 50", "left 30"};
const int num_instructions = sizeof(instructions) / sizeof(instructions[0]);
int current_instruction_index = 0;

/** Updates the position and angle based on encoder ticks. */
void update_physics();
/** Parses the current instruction. */
void parse_instruction(const char *instruction);
/** Rotates to an angle. */
void rotate_to_angle(double target_angle);
/** Moves forwards. */
void move_forward(double distance_cm);
/** Moves onto the next instruction. */
void execute_next_instruction();
/** Drives the motor to rotate/move forward. */
void drive_motors(double left_speed, double right_speed);
/** Updates the left encoder ticks. */
void left_encoder_isr();
/** Updates the right encoder ticks. */
void right_encoder_isr();

// Arduino setup
void setup() {
    Serial.begin(9600);

    pinMode(MOTOR1_DIR, OUTPUT);
    pinMode(MOTOR1_PWM, OUTPUT);
    pinMode(MOTOR2_DIR, OUTPUT);
    pinMode(MOTOR2_PWM, OUTPUT);
    pinMode(MOTOR3_DIR, OUTPUT);
    pinMode(MOTOR3_PWM, OUTPUT);
    pinMode(MOTOR4_DIR, OUTPUT);
    pinMode(MOTOR4_PWM, OUTPUT);

    pinMode(ENCODER_LEFT, INPUT_PULLUP); // Left encoder
    pinMode(ENCODER_RIGHT, INPUT_PULLUP); // Right encoder
    attachInterrupt(digitalPinToInterrupt(ENCODER_LEFT), left_encoder_isr, CHANGE);
    attachInterrupt(digitalPinToInterrupt(ENCODER_RIGHT), right_encoder_isr, CHANGE);

    current_position.set(0, 0);
}

// Arduino loop
void loop() {
    if (current_instruction_index < num_instructions) execute_next_instruction();
    else {
        Serial.println("All instructions completed!");
        while (1); // NO-OP
    }
}

void update_position() {
    /** Computes the distance travelled. */
    double left_distance = (left_encoder_ticks / (double)TICKS_PER_REV) * 2 * M_PI * WHEEL_RADIUS_CM;
    double right_distance = (right_encoder_ticks / (double)TICKS_PER_REV) * 2 * M_PI * WHEEL_RADIUS_CM;
    double average_distance = (left_distance + right_distance) / 2;

    // Differential drive: physics_new = physics_old + ∫ physics'(t) dt.

    /** Computes the delta angle and updates. */
    double delta_angle = (right_distance - left_distance) / WHEEL_BASE_CM;
    current_angle += delta_angle * (180.0 / M_PI);

    current_angle = fmod(current_angle + 360.0, 360.0);

    /** Updates the position. */
    current_position.x += average_distance * cos(current_angle * M_PI / 180.0);
    current_position.y += average_distance * sin(current_angle * M_PI / 180.0);

    /** Resets ticks. */
    noInterrupts();
    left_encoder_ticks = 0;
    right_encoder_ticks = 0;
    interrupts();
}

void parse_instruction(const char *instruction) {
    char direction[10];
    double distance_cm;

    /** Is this voodoo or divine intellect? */
    sscanf(instruction, "%s %lf", direction, &distance_cm);
    if (strcmp(direction, "up") == 0) {
        target_angle = 0;
        target_position.set(current_position.x, current_position.y + distance_cm);
    } else if (strcmp(direction, "down") == 0) {
        target_angle = 180;
        target_position.set(current_position.x, current_position.y - distance_cm);
    } else if (strcmp(direction, "left") == 0) {
        target_angle = 270;
        target_position.set(current_position.x - distance_cm, current_position.y);
    } else if (strcmp(direction, "right") == 0) {
        target_angle = 90;
        target_position.set(current_position.x + distance_cm, current_position.y);
    }
}

void rotate_to_angle(double target_angle) {
    while (abs(current_angle - target_angle) > THRESHOLD_ANGLE) {
        if (current_angle < target_angle) drive_motors(ROTATION_SPEED, -ROTATION_SPEED); // CLOCKWISE
        else drive_motors(-ROTATION_SPEED, ROTATION_SPEED); // COUNTERCLOCKWISE

        update_physics();
    }

    drive_motors(0, 0);
}

// Function to move the robot forward
void move_forward(double distance_cm) {
    while (current_position.distance_to(target_position) > THRESHOLD_CM) {
        drive_motors(150, 150);
        update_physics();
    }
    drive_motors(0, 0);
}

void drive_motors(double left_speed, double right_speed) {
    digitalWrite(MOTOR1_DIR, left_speed > 0 ? HIGH : LOW);
    analogWrite(MOTOR1_PWM, abs(left_speed));
    digitalWrite(MOTOR2_DIR, left_speed > 0 ? HIGH : LOW);
    analogWrite(MOTOR2_PWM, abs(left_speed));

    digitalWrite(MOTOR3_DIR, right_speed > 0 ? HIGH : LOW);
    analogWrite(MOTOR3_PWM, abs(right_speed));
    digitalWrite(MOTOR4_DIR, right_speed > 0 ? HIGH : LOW);
    analogWrite(MOTOR4_PWM, abs(right_speed));
}

void left_encoder_isr() {
    left_encoder_ticks++;
}

void right_encoder_isr() {
    right_encoder_ticks++;
}

void execute_next_instruction() {
    const char *instruction = instructions[current_instruction_index];
    Serial.print("Executing: ");
    Serial.println(instruction);

    parse_instruction(instruction);
    rotate_to_angle(target_angle);
    move_forward(target_position.distance_to(current_position));

    current_instruction_index++;
}