robotathon.ino

#include <Servo.h>
#include <stdio.h>

#define DEBUG true

int PIN_LEFT_MOTOR = PB_6;
int PIN_RIGHT_MOTOR = PB_7;
int PIN_LINE_SENSOR[8] = { PD_0, PD_1, PD_2, PD_3, PE_1, PE_2, PE_3, PB_4 };
// int PIN_TRIG_LEFT = PC_6;
// int PIN_ECHO_LEFT = PC_7;
// int PIN_TRIG_RIGHT = PD_6;
// int PIN_ECHO_RIGHT = PD_7;
int PIN_DIST_LEFT = PB_0;
int PIN_DIST_RIGHT = PB_1;

Servo left_motor;
Servo right_motor;

int line[8];
float dist_left;
float dist_right;

void setup() {
    if (DEBUG) {
        Serial.begin(115200);
        Serial.println("IT'S ALIVE!!!");
    }
    left_motor.attach(PIN_LEFT_MOTOR);
    right_motor.attach(PIN_RIGHT_MOTOR);
    left_motor.write(90);
    right_motor.write(90);
    pinMode(PIN_LEFT_MOTOR, OUTPUT);
    pinMode(PIN_RIGHT_MOTOR, OUTPUT);
    // pinMode(PIN_TRIG_LEFT, OUTPUT);
    // pinMode(PIN_TRIG_RIGHT, OUTPUT);
    // pinMode(PIN_ECHO_LEFT, INPUT);
    // pinMode(PIN_ECHO_RIGHT, INPUT);
    pinMode(PIN_DIST_LEFT, INPUT);
    pinMode(PIN_DIST_RIGHT, INPUT);
    for (int i = 0; i < 8; ++i) {
        pinMode(PIN_LINE_SENSOR[i], INPUT);
    }
}

void set_motors(float left_speed, float right_speed){
    int scaled_left = map(left_speed * 100, -100, 100, 0, 180);
    int scaled_right = map(right_speed * 100, -100, 100, 0, 180);
    left_motor.write(scaled_left);
    right_motor.write(scaled_right);
    if (DEBUG) {
        Serial.print("Set left motor to ");
        Serial.print(left_speed, 3);
        Serial.print(" (");
        Serial.print(scaled_left);
        Serial.print(") and right motor to ");
        Serial.print(right_speed, 3);
        Serial.print(" (");
        Serial.print(scaled_right);
        Serial.println(")");
    }
}

void sense_line(){
    if (DEBUG) {
        Serial.print("Line sensor data: ");
    }
    for (int i = 0; i < 8; ++i) {
        int raw = analogRead(PIN_LINE_SENSOR[i]);
        line[i] = raw;
        if (DEBUG) {
            char temp[6];
            sprintf(temp, "%4d ", raw);
            Serial.print(temp);
        }
    }
    if (DEBUG) {
        Serial.println("");
    }
}

float read_ultrasonic(int trig, int echo){
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);
    return pulseIn(echo, HIGH) * .017;
}

void sense_dist(){
    // dist_left = read_ultrasonic(PIN_TRIG_LEFT, PIN_ECHO_LEFT);
    // dist_right = read_ultrasonic(PIN_TRIG_RIGHT, PIN_ECHO_RIGHT);
    dist_left = analogRead(PIN_DIST_LEFT);
    dist_right = analogRead(PIN_DIST_RIGHT);
    if (DEBUG) {
        Serial.print("Distance sensor data: left ");
        Serial.print(dist_left);
        Serial.print("        right ");
        Serial.println(dist_right);
    }
}

void loop() {
    sense_line();
    sense_dist();
    set_motors(0.0, 0.0);
    delay(500);
}