Implementation

In this page, you can find all the technical solutions implemented in our robot. From the simplest move to the most elaborated function.


int distance_to_angle(int distance){
    return ((distance*360)/(PI*DIAMETRE));
}
			

void goStraight(int speed, int distance){
	initPosition();

    int angle = distance_to_angle(distance);
    synchronisedGoStraight(sn_wheels, speed, angle); // the wheels are synchronized : they turn at the same speed at the same time
	Sleep(50);
	while(robot_is_moving()){ // waiting until the speed of the two motors has reached 0.
		Sleep(10);
		refreshGlobalPosition();
	}
	refreshPosition();
}

Moving forward

One of the most basic functions is to move forward. In order to do so, both wheels turn at the same speed in the same direction.
A conversion must be done from the distance to travel to the rotation of the wheels.

Slowing down

A safety improvements consists in slowing down when the robot detects an obstacle.
It prevents Robbie from bumping into obstacles.
The decrease of speed during the slow down is a linear function of the distance to the obstacle.



/*
@desc :
	* the function is called while the robot is already moving
	* adapt the speed to the distance the robot still has to cover or to the obstacles the robot is going to meet.
@param :
	* int max_speed : max_speed of the motor during the movement
	* int maxDistance : the distance the robot has to do should not be more than maxDistance (example : an obstacle has to be discovered at maxDistance in front of the robot)
	* int securityDistance : the robot is supposed to stop closed to securityDistance of an obstacle (here only used to ajust the speed)
	* int brakingDistance : the robot will begin to brake at brakingDistance
	* int speedDivider : the max_speed will be divided by this factor along the braking period.
@return : the return value will be 1 if the robot had to shorten his previous path or 0 else.
*/
char manage_speed(int max_speed, int maxDistance,int securityDistance,int brakingDistance, int speedDivider){
	int distance;
	int angle;
	int deltaAngle = fabs(leftFinalPosition-leftStartPosition)-fabs(get_left_motor_position()-leftStartPosition);
	char distanceMaxDone = 1;
	if(deltaAngle<0){
		distance=0;
		deltaAngle=0;
	}
	else{
		distance = angle_to_distance(deltaAngle);
	}
	//printf("Distance remaining : %d \n",distance);
	//printf("Maximum distance that the robot should do : %d \n",maxDistance);

	if(distance > maxDistance && brakingDistance > maxDistance){ // testing if the robot has detected stg forcing the robot to stop earlier than in the forecast.
		distance = maxDistance;
		deltaAngle = distance_to_angle(distance);
		leftFinalPosition = get_left_motor_position() + deltaAngle;
		rightFinalPosition = get_right_motor_position() + deltaAngle;
		distanceMaxDone = 0;
	}

	// distance used in processing of the new speed.
	if(distance > maxDistance){
		distance = maxDistance;
	}

	// Calculate the new speed and send the new order to the motor : regular braking
	int newSpeed = max_speed - (((speedDivider-1)*max_speed/speedDivider)*(brakingDistance-distance))/(brakingDistance-securityDistance);
	if(newSpeed > max_speed){
		newSpeed=max_speed;
	}
	multi_set_tacho_speed_sp(sn_wheels, newSpeed);
	multi_set_tacho_position_sp(sn_wheels, deltaAngle);
    multi_set_tacho_command_inx(sn_wheels, TACHO_HOLD);
	return distanceMaxDone;
}

Sweeping

In order to notice every obstacle, Robbie moves its head and hence sweeps the space thanks to an actuator.
The sweep is managed by a specific thread, which modifies the state of the actuator : running, sleeping or finished.


void continuous_sweep(){
	int amplitudeAngle = 60;
	while(1){
		//sweep_state=1 when running state
		if(sweep_state==1){
			//Sweep
			absolute_servo_sonar(-amplitudeAngle);
			absolute_servo_sonar(amplitudeAngle);
		}
		else {
			Sleep(100);
			//sweep_state=2 when finished
			if(sweep_state == 2){
				return;
			}
		}
	}
}

Rotations

During a rotation, both wheels turn at the same time but in opposite directions. The center of Robbie doesn't move during a rotation and the robot stays in the same circle. Thus, it is impossible to bump into an obstacle during a rotation.
For example, when the left wheel turns forward and the right wheel turns backward, Robbie turns to the right.

The gyroscope is used to correct the rotation of the robot. After any rotation, Robbie turns a bit according to the difference between the ordered angle and the angle measured by the gyroscope.




void rotation(int speed, int angle){

	if(angle < 0){
		rotationPolarity = -1;
	}
	else{
		rotationPolarity = 1;
	}

    int distance_roue = (angle * PI * ECART_ROUES) / (360);
    int angle_roue = distance_to_angle(distance_roue);
    goStraightForAngle(sn_wheels[0], speed, -angle_roue);
    goStraightForAngle(sn_wheels[1], speed, angle_roue);
	Sleep(50);
	while(robot_is_moving()){ // waiting until the speed of the two motors has reached 0.
		Sleep(50);
	}

}

void preciseRotation(int speed, int angle){// using the gyroscope to adjust the angle.

	int angle_gyro_start;
	int angle_gyro_end;
	int difference;
	
	initPosition();
	
	angle_gyro_start = (getGyroAngle()+getGyroAngle()+getGyroAngle())/3;
	rotation(speed, angle);
	angle_gyro_end = (getGyroAngle()+getGyroAngle()+getGyroAngle())/3;

  	difference = angle - (angle_gyro_end - angle_gyro_start);
	
	if(difference!=0 && fabs(difference) < 90){ // we put 90 as a boundary if the gyroscope is detecting something not normal
		rotation(speed/2, difference); // we divide the speed by 2 to make sure that the robot will do a precise rotation
	}

	TETA1 = (TETA1 + angle)%360;
	TETA = TETA1;
	initPosition();

}
			  

void getColor(){
    if ( ev3_search_sensor( LEGO_EV3_COLOR, &sn_color, 0 )) {
        set_sensor_mode_inx(sn_color,LEGO_EV3_COLOR_RGB_RAW);
        get_sensor_value( 0, sn_color, &value_r );
		get_sensor_value( 1, sn_color, &value_g);
		get_sensor_value( 2, sn_color, &value_b);
        fflush( stdout );
    }
}


int is_red_obstacle(){
    getColor();
    if (value_r > value_g + value_b){
		printf("obstacle red found\n");
		return 1;
    } else {
		printf("not red\n");
		return 0;
}

Color sensor

The components red, green and blue given by the sensor are used to define if an obstacle is red.
The condition is that the red value is greater than the sum of the green and blue values.

Check the case forward for obstacles

When an obstacle is detected in front of the robot, the algorithm calls this function. It makes the robot look in where the obstacle is in the 5 squares forward.
The head moves so that the sonar gets a distance for each of the different squares. If there is a distance that is inferior to 10cm, the color sensor determines if the object is red or not. If it is, it's a movable object, else it's a non-movable object. The map is then updated accordingly.

More details about our project

Visit our Github account

Github