1.cpp

#include "mbed.h"
#include "physcom.h"

#define SPEED 0.1
#define THRESHHOLD 400
#define DEBUG

using namespace physcom;

#ifdef DEBUG
Serial pc(USBTX, USBRX);
#endif

M3pi robot;

int main() {

    robot.sensor_auto_calibrate();

    wait(2);

    while (1) {
        int sensors[5];
        robot.calibrated_sensors(sensors);

        #ifdef DEBUG
        pc.printf("sensor values: %d; %d; %d; %d; %d;\r\n", sensors[0], sensors[1],sensors[2], sensors[3], sensors[4]);
        #endif

        if (sensors[0] >= THRESHHOLD && sensors[4] >= THRESHHOLD) break; //We arrived at a T-junction.

        //We are going off the line. Adjust accordingly.

        if (sensors[3] >= THRESHHOLD || sensors[4] >= THRESHHOLD) {
            //The line is to the right. Drive to the right by speeding up the left motor.

            if (sensors[4] >= THRESHHOLD) {
                //More radical turn since were probably on a corner.

                robot.activate_motor(0, SPEED);
                robot.activate_motor(1, 0);
            } else {
                robot.activate_motor(0, SPEED*1.5);
                robot.activate_motor(1, SPEED);
            }
        } else if (sensors[1] >= THRESHHOLD || sensors[0] >= THRESHHOLD) {
            //The line is to the left. Drive to the left by speding up the right motor.

            if (sensors[0] >= THRESHHOLD) {
                //More radical turn since were probably on a corner.

                robot.activate_motor(0, 0);
                robot.activate_motor(1, SPEED);
            } else {
                robot.activate_motor(0, SPEED);
                robot.activate_motor(1, SPEED*1.5);
            }
        } else {
            //We are on a straight line. Drive forward.

            robot.activate_motor(0, SPEED);
            robot.activate_motor(1, SPEED);
        }
    }

    //Stop driving.

    robot.activate_motor(0, 0);
    robot.activate_motor(1, 0);
}