I’ll be talking about a project I worked on earlier this year. We had the task of building a robot that carried out certain tasks as dictated by the problem statement. I was on the software team, and we also had a mechanical team and an electrical team. The main tasks of the software team were to write the motion and sensor codes. It was a great experience as it was my first real C++ experience and really allowed me to become familiar with the intricacies of the language.

One of the first few things we did was to work out a motion function. The terrain was fitted with white lines, and we were able to use these for moving the robot around. Here is the actual line-following algorithm used in the end:

// Line follow, taking junctions into account
void line_follower() {
	    while (true) {
		
            int val = rlink.request (READ_PORT_5);
            int line_sensors = val bitand 15; //extract 4 most LSB values
            
            // Extract individual bits
            int line_sensors_A = line_sensors bitand 1;
            // Shifts to get singular booleans
            int line_sensors_B = (line_sensors bitand 2) >> 1;
            int line_sensors_C = (line_sensors bitand 4) >> 2;
            int line_sensors_D = (line_sensors bitand 8) >> 3;
            
            // Either line-following sensor AND either junction sensor
            if ((line_sensors_A bitor line_sensors_D) bitand (line_sensors_B bitor line_sensors_C)){
                cout<<"BREAKING WITH:"<<convertDecimalToBinary(line_sensors)<<endl;
                break;
            }
            
            switch (line_sensors) {
                case 6 : //0110
                    //cout<<"0110 Stay straight"<<endl;
                    rlink.command (MOTOR_1_GO, 128+motor_1_initial+speed_factor);
                    rlink.command (MOTOR_2_GO, motor_2_initial+speed_factor);
                    i=1;
                    break;
                case 2 : //0010
                    //cout<<"0010 Slight right"<<endl;
                    rlink.command (MOTOR_1_GO, 128+motor_1_initial+5+speed_factor);
                    rlink.command (MOTOR_2_GO, motor_2_initial-5+speed_factor);
                    break;
                case 4 : //0100
                    //cout<<"0100 Slight left"<<endl;
                    rlink.command (MOTOR_1_GO, 128+motor_1_initial-5+speed_factor);
                    rlink.command (MOTOR_2_GO, motor_2_initial+5+speed_factor);
                    break;
                case 0 : //0000
                    //cout<<"0000 DANGER: Off path"<<endl;
                    recovery();
                    break;

            }
            
    }
}

One of the cooler functions that I got to write was the recovery function. This was to be activated if the robot ever deviated too far away from the line:

// Recovery motion
// AGV swivels in increasing arcs from left to right
void recovery() {
    // To account for loss of signals on hill
    delay(150);
	int val = rlink.request (READ_PORT_5);
	int line_sensors = val & 15; //extract 4 most LSB values 

    // If signal re-found (usually on hill)
    if (line_sensors == 6 || line_sensors == 2 || line_sensors == 4){}
    // Actual recovery mode
	else {
        // Use increments of i by one and modulo 2 to go from L to R to L to R...
        rlink.command (BOTH_MOTORS_GO_SAME, ((i+1)%2)*(motor_1_initial+speed_factor)+(i%2)*(128+motor_1_initial+speed_factor));
        
        // Use increments of i by one to linearly increase time taken for each L/R turn
        stopwatch watch;
        watch.start();

        while (watch.read()<100*i) {
            int val = rlink.request (READ_PORT_5);
            int line_sensors = val & 15; //extract 4 most LSB values
           
            // If line re-found
            if (line_sensors == 2 || line_sensors == 4 || line_sensors == 6) break;
        }
        
        // Just to ensure stopwatch stops
        int total_time = watch.stop();
        // cout<<"Failure number: "<<i<<" Time taken: "<<total_time<<endl;

        i++;
	}
}

I enjoyed the use of modulo 2 to alternate between left and right swivels (essentially the robot would swivel around whenever it lost the line until the sensors picked up the line again). It may seem quite simplistic, but I was really happy when I managed to get it to work. It was one of those ‘original ideas’ that I don’t really get that often.