Robot for Robocup Junior (RCJ) Rescue Soccer, Arduino program.

RCJ Soccer ML-R robot 3, Program

Download and install Arduino IDE.
Download and install Teensyduino.
Download all the ML-R libraries (or at least Barriers, Commands, Displays, IMUBoschBNO055Analog, IRFinders, Motors, PID, ReflectanceSensors, Switches, and VL53L0XsAnalog) using https://www.github.com/PribaNosati/MRMS. Download zip files, start Arduino IDE and add libraries one by one, selecting Sketch - Include Library - Add .ZIP Library.

C++

You will have to learn it, at least basics. Learning is beyond scope of these lessons. The part of C++ You will need here is C. C is a subset of C++. If You learn basics of C, that will be OK for the beginning: variables, loops (for, while), branching (if), functions. Try http://www.learn-c.org/ or some other web. Lessons for C++ tend to be too complicated for a beginner and Arduino programming doesn't demand too many advanced topics to get the job done.

First program

Enter this code in the main window of Arduino IDE:

void setup(){
}

void loop(){
}

Here we have 2 functions: setup and loop. When Arduino microcontroller is powered up, setup is run once. After that loop is is executed an endless number of times.

Power on Teensy board by switching on regulator board using the switch. Connect Teensy to Your PC using a USB cable.

Go to Tools - Board and choose Teensy 3.2.

Go to Tools - Port and anc choose COMXX where XX matches the port number where Teensy is connected.

Find Nike button (first on the left, "Verify") and click it. The sketch (program You made) will be compiled. Message "Done compiling" reports success. If You get an error message, You will have to correct the program.

Simple RCJ Soccer program

Previous program doesn't do anything so we will extend it. Watch for // parts of the code in the next example. Arduino ignores everything You type after //, in that line. In this way we add comments.

/**
Purpose: a sample code for RCJ Soccer, advanced robot.
@author ML-R team
@version 0.0 2019-01-28
Licence: You can use this code any way you like.
*/

// External libraries
#include <Barriers.h>
#include <Commands.h>
#include <Displays.h>
#include <IMUBoschBNO055Analog.h>
#include <IRFinders.h>
#include <Motors.h>
#include <PID.h>
#include <ReflectanceSensors.h>
#include <Switches.h>
#include <VL53L0XsAnalog.h>

//Konfiguracija
#define BLUETOOTH_BPS 9600 // Bluetooth module specific. Usually 9600 or 115200.
#define SPEED_LIMIT 100 // 0 - 100. For example if you test powerful motors. 0 turns the motors off - useful for testing.

//Objects definitions
Barriers barriers(&Serial1); // Infrared barriew, checks ball possession
Commands commands(&Serial1); // Commands for the robot.
Displays displays;
IMUBoschBNO055Analog imu; // Compass. Reads north direction in an absolute system (o the playground). The other one is robot's (relative).
IntervalTimer timer; // Timer interrupts the main program and updates line and ball data.
IRFinders irFinders(&Serial1); // Ball sensors.
Motors motors(true, &Serial1); // Motors
PID pid(1, 2000, 0); // PID controller, regulates motors' speeds
ReflectanceSensors reflectanceSensors(0, 0.25, &Serial1); // Line sensors
Switches switches(&Serial1); // Switches, for example for starting of the robot.
VL53L0XsAnalog lidars(&Serial1);


// Robot's finite states
enum State { APPROACHING_BALL, AVOIDING_LINE, IDLE, PAUSED, POSSESSES_BALL };
State state; // Current state

// Functions' declarations
void setState(::State);
void print(String message, bool eol = false);

void setup()
{
	Serial.begin(9600); // Communication with a computer
	Serial1.begin(BLUETOOTH_BPS); // Bluetooth link with a mobile.
	Wire.begin(); // Start I2C, needed for IMU (and ultrasonice sensors, if I2C used).

	//Add IR barriers
	barriers.add(A3);

	//Add commands
	commands.add("sta", 0, "Start", 13); // Start the robot. Switch (pin 13) does the same.
	commands.add("cal", reflectanceSensorsCalibrate, "Line calibration"); // Put the robot above a white line and enter this command.
	commands.add("lix", lineTestX, "Line X"); // Displays a line as Xs
	commands.add("lib", lineTestNumeric, "Line numeric"); // Displays a line as numerical values
	commands.add("mot", motorsTest, "Motors"); // Turns on and off all the motors
	commands.add("imu", imuTest, "IMU"); // Compass test
	commands.add("dis", reflectanceSensorsDisplayCalibration, "Calibration info"); // Displays minimums and maximums for reflectance sensors
	commands.add("lid", lidarsTest, "Lidars"); // Tests all the LIDARS
	commands.add("bal", irFindersTest, "Ball"); // Finds ball's position
	commands.add("gos", goSomewhere, "Go"); // Go
	commands.add("swi", switchTest, "Switch"); // Tests switches
	commands.add("ban", barrierTestNumeric, "Barrier numeric"); // Displays barriers's readings as numerical values
	commands.add("bax", barrierTestX, "Barrier X"); // Displays barriers's readings as Xs
	commands.add("bac", barrierCalibrate, "Barrier calibration");
	commands.add("tes", test, "Test"); // A custom test

	//Add 8x8 display
	displays.add(0x70);

	//Add IMU
	imu.add(A8);

	//Add IR ball sensors - unchecked
	irFinders.add(A18, A1); // Angle and distance - check function declaration

	//LIDARs
	lidars.add(A6); // 0, 0°
	lidars.add(A7);  // 1, 90°
	lidars.add(A15); // 2, 180°
	lidars.add(A16); // 3, -90°

	//Add motors
	motors.add(25, 5, true, true); //45° (relative, 0 degrees is the robot's front, angle increases counterclockwise) 
	motors.add(4, 6, true, true); //135°
	motors.add(9, 10, false, false); //-135°
	motors.add(3, 23, false, false); //-45°

	//Line sensors
	reflectanceSensors.add(A17); // 0, 90°
	reflectanceSensors.add(A12); // 1, 180°
	reflectanceSensors.add(A10); // 2, -90°
	reflectanceSensors.eepromRead();//Reading EEPROM calibration data into RAM

	//Switches
	switches.add(13);

	//Red, wait 0.6 sec., green - signal that robot is working
	displays.fillScreen(0, LED_RED);
	displays.writeDisplay(0);
	delay(600);
	displays.fillScreen(0, LED_GREEN);
	displays.writeDisplay(0);

	print("Start", true);
	setState(PAUSED);

	//Timer interrupt - periodically invokes update().
	timer.begin(update, 3000); //Every 3 msec
}

void loop()
{
	/*Direction in the absolute system. The robot should constantly maintain relative 0 (robot's front) aligned to this value. So, if headingToMaintain
	is 30°, IMU must always show around 30°. If it shows 40°, that means the robot's position is too much anticlockwise, by 10 degrees.*/
	static float headingToMaintain = 0; // A static variable instead of a global one.

	/* In every finite state we have to check what conditions are met and change the state, if the conditions are right. This loops runs at a high
	frequency, except for a state PAUSED, where the program waits the the state change.*/
	switch (state) {
	case APPROACHING_BALL: {
		// No ball in possession, but it is detected.
		float direction = irFinders.irSource().angle;
		if (abs(direction) < 10)
			direction *= 2;
		else if (abs(direction) < 45)
			direction += copysign(70, direction);
		else
			direction = normalized(direction + copysign(40, direction)); // The target position is always a little behind the ball so that we approach it from behind. So we increase the angle for 30 degrees.
		motors.goOmni(50, direction, rotationToMaintainHeading(headingToMaintain), SPEED_LIMIT);//Speed will be constant.

		if (millis() - reflectanceSensors.lastTimeAnyBright() < 100)
			setState(AVOIDING_LINE);
		else if (!irFinders.anyIRSource()) //No ball detected?
			setState(IDLE);
		else if (millis() - barriers.lastTimeInterrupt() < 100)
			setState(POSSESSES_BALL);
	}
	break;

	case AVOIDING_LINE: {//Check all the lidars and finds a free escape route.
		const int cmToDeclareFree = 30;
		float f = lidars.distance(0) / 10.0; // Divide by 10 to get cm
		float r = lidars.distance(1) / 10.0;
		float b = lidars.distance(2) / 10.0;
		float l = lidars.distance(3) / 10.0;
		uint8_t cnt = 0;
		if (f > cmToDeclareFree) cnt++;
		if (r > cmToDeclareFree) cnt++;
		if (l > cmToDeclareFree) cnt++;
		if (b > cmToDeclareFree) cnt++;
		if (cnt == 4)
			;
		else {
			stop();
			while (abs(normalized(imu.heading() - headingToMaintain)) > 15)
				motors.goOmni(0, 0, rotationToMaintainHeading(headingToMaintain), SPEED_LIMIT);
			float direction = 0;
			bool found = true;
			if (cnt == 1)
				break;
			else if (cnt == 2) {
				if (f > cmToDeclareFree && r > cmToDeclareFree)
					direction = -45;
				else if (r > cmToDeclareFree && b > cmToDeclareFree)
					direction = -135;
				else if (b > cmToDeclareFree && l > cmToDeclareFree)
					direction = 135;
				else if (l > cmToDeclareFree && f > cmToDeclareFree)
					direction = 45;
				else
					found = false;
			}
			else { //cnt == 3
				if (f <= cmToDeclareFree)
					direction = 180;
				else if (r <= cmToDeclareFree)
					direction = 90;
				else if (b <= cmToDeclareFree)
					direction = 0;
				else //l
					direction = -90;
			}
			if (found) {
				uint32_t startMs = millis();
				while (millis() - startMs < 400)
					motors.goOmni(70, direction, rotationToMaintainHeading(headingToMaintain), SPEED_LIMIT);
			}
		}
		setState(IDLE);
	}
	break;

	case IDLE: {
		// Return to front of our goal
		float errorY = 20 - lidars.distance(2) / 10.0; // Y axis error. The target position is 20 cm in front of our goal.
		bool useLeftWall = lidars.distance(1) < lidars.distance(3);
		float errorX = useLeftWall ? lidars.distance(3) / 10 - 60 : 60 - lidars.distance(1) / 10; // X axis error. Target distance to a more distant wall is 60 cm.

		goToEliminateErrors(errorX, errorY, headingToMaintain); // Change the robot's positions in order to eliminate position errors.

		if (millis() - reflectanceSensors.lastTimeAnyBright() < 100) //Line?
			setState(AVOIDING_LINE);
		else if (irFinders.anyIRSource()) // Ball?
			setState(APPROACHING_BALL);
	}
	break;

	case PAUSED:
		// Interrupt while playing. While not a "sta" command entered, execute all the other commands but does not break the pause. 
		// "sta" continues the program.
		stop();
		do {
			commands.list();
		} while (commands.prompt());
		headingToMaintain = imu.heading(); // Store heading to maintain.
		setState(IDLE);
		break;

	case POSSESSES_BALL:
		// Go towards the opponent's goal.
		float errorY = 10; // Y error. This number is constant because we want to robot to constantly go ahead.
		bool useLeftWall = lidars.distance(1) < lidars.distance(3);
		float errorX = useLeftWall ? lidars.distance(3) / 10.0 - 60 : 60 - lidars.distance(1) / 10.0; // X axis error. Target distance to a more distant wall is 60 cm.
		goToEliminateErrors(errorX, errorY, headingToMaintain); // Change the robot's positions in order to eliminate position errors.

		if (millis() - reflectanceSensors.lastTimeAnyBright() < 100) // A line?
			setState(AVOIDING_LINE);
		else if (millis() - barriers.lastTimeInterrupt() > 100)
			setState(APPROACHING_BALL);
		break;
	}

	//If not timer interrupt used, we can update() the cumulatives here. 
	//irFinders.update();

	// If a key is pressed, using a computer or a mobile, stops the motors and waits for the complete command.
	if (commandAvailable())
		setState(PAUSED);

	//Print Frames Per Second
	static uint32_t fpsCount = 0;
	static uint32_t lastFPSMs = 0;
	fpsCount++;
	if (millis() - lastFPSMs > 1000) {
		print(" FPS: " + (String)fpsCount, true);
		lastFPSMs = millis();
		fpsCount = 0;
	}
}

/*********************** Functions for commands *****************/

bool barrierCalibrate() { barriers.calibrate(); return true; }
bool barrierTestNumeric() { barriers.test(true, commandAvailable); return true; }
bool barrierTestX() { barriers.test(false, commandAvailable); return true; }
bool goSomewhere() { while (!commandAvailable())motors.goOmni(100, 0, 0, SPEED_LIMIT); stop(); return true; }
bool imuTest() { imu.test(commandAvailable); return true; }
bool irFindersTest() { irFinders.test(commandAvailable, true); return true; }
bool lineTestNumeric() { reflectanceSensors.test(true, commandAvailable); return true; }
bool lineTestX() { reflectanceSensors.test(false, commandAvailable); return true; }
bool motorsTest() { motors.test(2, commandAvailable); return true; }
bool reflectanceSensorsCalibrate() { motors.goOmni(0, 0, 30, SPEED_LIMIT); reflectanceSensors.calibrate(); stop(); return true; }
bool reflectanceSensorsDisplayCalibration() { reflectanceSensors.eepromRead(); return true; }
bool switchTest() { switches.test(commandAvailable); return true; }
bool test() { motors.setSpeed(0, 15); return true; }
bool lidarsTest() { lidars.test(commandAvailable); return true; }

/** A function that we use as pointer parameter for tests, so that the robot can stop, when a key is pressed.
@return - If true, a key was pressed
*/
bool commandAvailable() {
	return commands.available();
}

/** If something goes wrong, it will be the best to stop the motors and the program.
*/
void error(String message) {
	stop();
	print(message);
	while (true)
		;
}

/** Moves the robot in order to elinimate errors (for x and y directions).
@param errorX - X axis error.
@param errorY - Y axis error.
@param headingToMaintain - Heading to maintain.
*/
void goToEliminateErrors(float errorX, float errorY, float headingToMaintain) {
	static uint32_t lastMs = 0;

	float heading;
	if (fabsf(errorX) > 0.001) // To avoid overflow.
		heading = toDeg(atan2f(-errorX, errorY));
	else
		heading = errorY > 0 ? 180 : -180;

	float speed;
	if (abs(normalized(imu.heading() - headingToMaintain)) < 20) // If a rotational error is quite big, correct without moving to x or y directions.
		speed = pid.calculate(abs(errorX) + abs(errorY)); // PID sets the speed.
	else
		speed = 0;

	if (false && millis() - lastMs > 500) {
		lastMs = millis();
		print(" Errors: (" + (String)errorX + ", " + (String)errorY + "), " + (String)heading + " deg. Sp: " + (String)speed + ", rot: " +
			(String)rotationToMaintainHeading(headingToMaintain) + ".", true);
	}
	motors.goOmni(speed, heading, rotationToMaintainHeading(headingToMaintain), SPEED_LIMIT);
}

/** Angle difference
@param angle1 - In degrees
@param angle2 - In degrees
@return - Difference, between -180 and 180 degrees
*/
float normalized(float angle) {
	if (angle < -180)
		angle += 360;
	else if (angle > 180)
		angle -= 360;
	return angle;
}

/** Print to 2 serial ports: Arduino monitor and Your mobile (Bluetooth).
@param message
@param eol - end of line
*/
void print(String message, bool eol) {
	if (eol) {
		Serial.println(message);
		Serial2.println(message);
	}
	else {
		Serial.print(message);
		Serial2.print(message);
	}
}

/** A rotation needed to maintain the requested heading
@return - Rotation
*/
float rotationToMaintainHeading(float headingToMaintain) {
	float rotation = map(normalized(headingToMaintain - imu.heading()), -180, 180, -200, 200);
	return rotation;
}

/** State change
@param newState - Robot's new state.
*/
void setState(::State newState) {
	state = newState;
	switch (state) {
	case APPROACHING_BALL:

		break;
	case AVOIDING_LINE:

		break;
	case IDLE:

		break;
	case PAUSED:

		break;
	case POSSESSES_BALL:

		break;
	default:
		break;
	}
}

/** Stop all the motors
*/
bool stop() {
	motors.go(0, 0);
	return true;
}

/** Periodic functions, invoked by timer interrupt or simply in the program's main loop.
*/
void update() {
	reflectanceSensors.anyBright();
}