Conversion Functions

This section includes helper functions that are used throughout the robot code to convert between distance (in centimeters) and motor rotation (in degrees), as well as to check the current battery level of the EV3 brick.

check_battery()

This function displays the current battery level on the LCD screen of the EV3 brick.
The value returned by EV3.BatteryLevel is the battery voltage in millivolts (e.g., 7900 = 7.9V).
It's useful for debugging or preventing the robot from running with low power. 

Sub check_battery
  LCD.Clear()                         ' Clears any previous text from the screen
  LCD.Write(0, 70, EV3.BatteryLevel)  ' Writes the battery level at X=0, Y=70
  LCD.Update()                        ' Refreshes the LCD screen to show the value
EndSub

cm2degree(cm, degrees)

This function converts a physical distance in centimeters into the number of degrees the motor must rotate to cover that distance.
It's required because motors only work with rotation values, not linear distances.

The formula is:
degrees = (cm × 360) / (π × wheel_diameter)
Here, @wheel_diameter is a global variable set to match your robot's wheels. 

Function cm2degree (in number cm, out number degrees)
  degrees = cm * 360 / ( Math.Pi * @wheel_diameter )
EndFunction

degree2cm(degrees, cm)

This function does the reverse of cm2degree. It converts motor rotation in degrees back into a linear distance in centimeters.

The formula is:
cm = degrees / (360 / (π × wheel_diameter)) 

Function degree2cm (in number degrees, out number cm)
  cm =  degrees / ( 360 / ( Math.Pi * @wheel_diameter ) )
EndFunction

Acceleration & Deceleration

When a robot starts or stops movement, it can jerk or skid if motors instantly jump to a high speed or stop abruptly. To make movement smoother and prevent mechanical stress, we use acceleration and deceleration curves that gradually increase or decrease speed.

How Acceleration Works

The idea is to slowly raise motor speed from a minimum value (e.g., @start_speed) to the desired maximum speed (max_speed) over a certain distance defined by @AccelerateEncoder. This creates a ramp-up effect.

Formula:
V = (CurrentEncoder / @AccelerateEncoder) × (max_speed - @start_speed) + @start_speed
This makes the speed increase proportionally as the robot travels further. 

' Acceleration Curve
If acceleration = "on" Then
  V = ( CurrentEncoder / @AccelerateEncoder ) * ( max_speed - @start_speed * sens ) + @start_speed * sens
  V = Math.Abs(V) * sens
  If Math.Abs(V) > Math.Abs(max_speed) Then
    V = max_speed
  EndIf
ElseIf acceleration = "off" Then
  V = max_speed
EndIf

How Deceleration Works

Deceleration is the opposite: gradually reducing speed as the robot gets closer to the end of its movement. It improves stopping accuracy and prevents the robot from overshooting.

It works based on the distance remaining to the goal, using @DecelerateEncoder as the range.

Formula:
V = ((encoder - CurrentEncoder) / @DecelerateEncoder) × (max_speed - @start_speed) + @start_speed 

' Deceleration Curve
If deceleration = "on" Then
  If encoder - CurrentEncoder < @DecelerateEncoder Then
    V = ((encoder - CurrentEncoder) / @DecelerateEncoder) * (max_speed - @start_speed * sens) + @start_speed * sens
    V = Math.Abs(V) * sens
    If Math.Abs(V) < Math.Abs(@start_speed * sens) Then
      V = @start_speed * sens
    EndIf
  EndIf
EndIf

When to Use

  • Use acceleration = "on" when you want smoother starts, especially in longer movement sequences.
  • Use deceleration = "on" when the robot needs to stop precisely (e.g., before turning or grabbing an object).
  • You can combine both acceleration and deceleration for full curve control.

Line Follower - Dual Sensor

This section explains how to follow a black line using two sensors and a PD controller.
It includes acceleration, deceleration, and end brake logic to ensure smooth, accurate, and stable movement.

Why use Acceleration & Deceleration?

Motors are not perfect — if you instantly set a high speed, the robot may jerk, spin, or lose the line. Acceleration gradually increases speed at the beginning, making the robot stable and reducing slippage.

Deceleration gradually reduces the speed near the end of movement. Without it, your robot may overshoot the stop point or shake during a turn transition. It helps with precision.

Why use End Brake?

The end brake logic stops the motors firmly once the target distance is reached. It is especially useful when your robot has to stop before an action — like turning, reading a color, or picking up an object. If end_brake = "on", it uses Motor.Stop() at the end of the movement.

What is PD Control?

The robot reads the difference between the two sensors to calculate an error (how far from center it is). Then it applies a Proportional and Derivative correction:
P = kp × error controls how hard the robot should react.
D = kd × (error - previous_error) slows down rapid changes. 

Function LineFollower_2S_Encoder (in number max_speed, in number encoder_cm, in string acceleration, in string deceleration, in string end_brake)

  Sensor.SetMode(@left_sensor,0)
  Sensor.SetMode(@right_sensor,0)
  Motor.ResetCount("BC")
  
  error_old = 0
  exit_condition = 1
  cm2degree(encoder_cm, encoder)
  
  While exit_condition = 1
    s1 = Sensor.ReadPercent(@left_sensor)
    s2 = Sensor.ReadPercent(@right_sensor)
    CurrentEncoder = (Math.Abs(Motor.GetCount(@left_motor)) + Math.Abs(Motor.GetCount(@right_motor))) / 2
  
    If max_speed < 0 Then
      sens = -1
    Else
      sens = 1
    EndIf
  
    If acceleration = "on" Then
      V = (CurrentEncoder / @AccelerateEncoder) * (max_speed - @start_speed * sens) + @start_speed * sens
      V = Math.Abs(V) * sens
      If Math.Abs(V) > Math.Abs(max_speed) Then
        V = max_speed
      EndIf
    ElseIf acceleration = "off" Then
      V = max_speed
    EndIf
  
    If deceleration = "on" Then
      If encoder - CurrentEncoder < @DecelerateEncoder Then
        V = ((encoder - CurrentEncoder) / @DecelerateEncoder) * (max_speed - @start_speed * sens) + @start_speed * sens
        V = Math.Abs(V) * sens
        If Math.Abs(V) < Math.Abs(@start_speed * sens) Then
          V = @start_speed * sens
        EndIf
      EndIf
    EndIf
  
    If @debug = "on" Then
      LCD.Write(10,30, @start_speed*sens)
      LCD.Write(50,30, V)
      LCD.Update()
    EndIf
  
    error = s1 - s2
    P = @kp_lf * error
    D = @kd_lf * (error - error_old)
  
    If @debug = "on" Then
      LCD.Write(10,50, V)
      LCD.Write(50,50, P + D)
      LCD.Write(100,50, V + P + D)
      LCD.Update()
    EndIf
  
    If @motor_type = "medium" Then
      Motor_Left = -1 * (V + (P + D))
    Else
      Motor_Left = V + (P + D)
    EndIf
  
    Motor_Right = V - (P + D)
    error_old = error
  
    Motor.StartPower(@left_motor, Motor_Left)
    Motor.StartPower(@right_motor, Motor_Right)
  
    If CurrentEncoder > encoder Then
      exit_condition = 0
    EndIf
  EndWhile
  
  If end_brake = "on" Then
    Motor.Stop("BC", "True")
  EndIf
  
EndFunction

Line Following with a Single Sensor

When only one sensor is available, we can still follow a line using a PD controller and a reference value for grey. The difference from a dual-sensor setup is that we compare the current sensor reading to a fixed grey value instead of another sensor. This is especially useful when you want a smaller or simpler robot that uses fewer ports.

LineFollower_1S_Encoder()

This function allows the robot to follow a line using only one sensor until it reaches a specific encoder value. The robot calculates the error between the current sensor reading and the reference grey value. Based on the selected side ("left" or "right"), the controller applies power corrections using PD logic. 

Function LineFollower_1S_Encoder (in number port, in string side, in number kp, in number kd, in number max_speed, in number encoder_cm, in string acceleration, in string deceleration, in string end_brake)
  Sensor.SetMode(port, 0) ' Mode 0, light reflection
  Motor.ResetCount("BC")
  
  error_old = 0
  exit_condition = 1
  
  cm2degree(encoder_cm, encoder)
  
  While exit_condition = 1
    s1 = Sensor.ReadPercent(port)
    s2 = @grey
    CurrentEncoder = ( Math.Abs(Motor.GetCount(@left_motor)) + Math.Abs(Motor.GetCount(@right_motor)) ) / 2
  
    If acceleration = "on" Then
      V = ( CurrentEncoder / @AccelerateEncoder ) * ( max_speed - @start_speed ) + @start_speed
      If V > max_speed Then V = max_speed
    Else
      V = max_speed
    EndIf
  
    If deceleration = "on" Then
      If encoder - CurrentEncoder < @DecelerateEncoder Then
        V = ( (encoder - CurrentEncoder) / @DecelerateEncoder ) * ( max_speed - @start_speed ) + @start_speed
        If V < @start_speed Then V = @start_speed
      EndIf
    EndIf
  
    If side = "left" Then
      error = s1 - s2
    Else
      error = s2 - s1
    EndIf
  
    P = kp * error
    D = kd * (error - error_old)
    error_old = error
  
    Motor_Left = V + (P + D)
    Motor_Right = V - (P + D)
    Motor.StartPower(@left_motor, Motor_Left)
    Motor.StartPower(@right_motor, Motor_Right)
  
    If CurrentEncoder > encoder Then exit_condition = 0
  EndWhile
  
  If end_brake = "on" Then
    Motor.Stop("BC", "True")
  EndIf
EndFunction

Detecting Intersections

To follow a line until an intersection (instead of an encoder value), we use a second color sensor or the one we are doing the line follower on. This sensor is continuously monitored, and when its value drops below a black threshold (usually around 10–20), the robot recognizes it has reached an intersection.

The logic is similar to LineFollower_1S_Encoder, but the exit condition changes: 

If stop_port_reading < @black_value Then
  exit_condition = 0
EndIf

This check allows the robot to stop exactly when a dark line or intersection is detected by the stop sensor, improving timing and precision.

Line Following with Two Sensors - Intersection

This page focuses on performing a two-sensor line following routine until an intersection is reached. This is essential for navigating robotics courses with branching paths, stops, or checkpoints.

How It Works

Using a standard PD controller, the robot constantly corrects its course based on two sensors placed on either side of the line. When one of the sensors detects a black value (an intersection), the robot knows it's time to stop or align for rotation.

Intersection Exit Condition

The condition If s1 < @black_value + 2 Or s2 < @black_value + 2 Then tells the robot to exit the loop as soon as either sensor sees black, meaning the robot has reached a junction or crossroad.

Why Alignment and End Brake?

    Alignment: After reaching an intersection, aligning the robot with the center of rotation ensures more accurate turns. End Brake: If enabled, it stops the motors with a brake for better positioning and consistency. 
Function LF_2S_Intersection (in number max_speed, in string acceleration, in string move_until_center_of_rotation, in string end_brake)

  Sensor.SetMode(@left_sensor, 0) ' Sensor Mode 0, for reflected light
  Sensor.SetMode(@right_sensor, 0) ' Sensor Mode 0, for reflected light
  Motor.ResetCount("BC")
  
  error_old = 0
  exit_condition = 1
  
  While exit_condition = 1
    s1 = Sensor.ReadPercent(@left_sensor)
    s2 = Sensor.ReadPercent(@right_sensor)
    CurrentEncoder = ( Math.Abs(Motor.GetCount(@left_motor)) + Math.Abs(Motor.GetCount(@right_motor)) ) / 2
  
    If acceleration = "on" Then
      V = ( CurrentEncoder / @AccelerateEncoder ) * ( max_speed - @start_speed ) + @start_speed
      V = Math.Abs( V )
      If V > max_speed Then V = max_speed
    ElseIf acceleration = "off" Then
      V = max_speed
    EndIf
  
    error = s1 - s2
    P = @kp_lf * error
    D = @kd_lf * (error - error_old)
  
    If @motor_type = "medium" Then
      Motor_Left = -1 * ( V + (P + D)  ' for medium motors one motor will always be negative to move forward
    Else
      Motor_Left =  V + (P + D)
    EndIf
  
    Motor_Right = V - (P + D)
    error_old = error
  
    Motor.StartPower(@left_motor, Motor_Left)
    Motor.StartPower(@right_motor, Motor_Right)
  
    If s1 < @black_value + 2 Or s2 < @black_value + 2 Then
      exit_condition = 0
    EndIf
  
  EndWhile
  
  If move_until_center_of_rotation = "on" Then
    MoveSync_Encoder(@wheel_to_sensor_distance, max_speed, "off", "off", "off")
  EndIf
  
  If end_brake = "on" Then
    Motor.Stop("BC", "True")
  EndIf

EndFunction

Move with Encoder

This function is used when you want to move the robot a specific distance. Because EV3 motors measure rotation in degrees, we convert centimeters to degrees using the cm2degree() helper function.

Move_Sync_Encoder()

Synchronizes both motors to move in a straight line using encoder feedback. Includes optional acceleration and deceleration profiles. It also uses a PD (Proportional-Derivative) controller to keep both motors aligned. 

Function Move_Sync_Encoder (in number encoder_cm, in number max_speed, in string acceleration, in string deceleration, in string end_brake)
  Motor.ResetCount(@left_motor)
  Motor.ResetCount(@right_motor)
  cm2degree(encoder_cm, encoder)
  error_old = 0
  exit_condition = 1

  While exit_condition = 1
    left_encoder = Motor.GetCount(@left_motor)
    right_encoder = Motor.GetCount(@right_motor)
    CurrentEncoder = (Math.Abs(left_encoder) + Math.Abs(right_encoder)) / 2

    If acceleration = "on" Then
      V = (CurrentEncoder / @AccelerateEncoder) * (max_speed - @start_speed) + @start_speed
      If V > max_speed Then 
        V = max_speed 
      EndIf
    Else
      V = max_speed
    EndIf

    If deceleration = "on" And encoder - CurrentEncoder < @DecelerateEncoder Then
      V = ((encoder - CurrentEncoder) / @DecelerateEncoder) * (max_speed - @start_speed) + @start_speed
      If V < @start_speed Then 
        V = @start_speed 
      EndIf
    EndIf

    error = right_encoder - left_encoder
    P = @kp_move_sync * error
    D = @kd_move_sync * (error - error_old)
    Motor_Left = V + P + D
    Motor_Right = V - P - D

    Motor.StartPower(@left_motor, Motor_Left)
    Motor.StartPower(@right_motor, Motor_Right)
    error_old = error

    If Math.Abs(CurrentEncoder) > encoder Then
      exit_condition = 0
    EndIf
  EndWhile

  If end_brake = "on" Then
    Motor.Stop("BC", "True")
  EndIf
EndFunction

Move Until Color

This movement ends when a specific color is detected by either sensor. Common use cases include stopping at a green zone or aligning to black lines.

MoveSync_To_Color()

The robot moves forward while continuously checking for a color match. It supports RGB and HSV detection depending on the selected color. Use this when you want to stop on black, white, green, blue, or brown zones. 

Function MoveSync_To_Color(in number max_speed, in string acceleration, in string end_brake, in string colour)
  Sensor.SetMode(1, 0)
  Sensor.SetMode(2, 0)
  Motor.ResetCount("BC")
  error_old = 0
  exit_condition = 1

  While exit_condition = 1
    left_encoder = Motor.GetCount(@left_motor)
    right_encoder = Motor.GetCount(@right_motor)
    CurrentEncoder = (Math.Abs(left_encoder) + Math.Abs(right_encoder)) / 2

    If acceleration = "on" Then
      V = (CurrentEncoder / @AccelerateEncoder) * (max_speed - @start_speed) + @start_speed
      If V > max_speed Then 
        V = max_speed 
      EndIf
    Else
      V = max_speed
    EndIf

    error = right_encoder - left_encoder
    P = @kp_move_sync * error
    D = @kd_move_sync * (error - error_old)
    Motor_Left = V + P + D
    Motor_Right = V - P - D

    Motor.StartPower(@left_motor, Motor_Left)
    Motor.StartPower(@right_motor, Motor_Right)
    error_old = error

    HSV_Read(1, val)
    If val = 3 Then 
      exit_condition = 0 
    EndIf
    HSV_Read(2, val)
    If val = 3 Then 
      exit_condition = 0 
    EndIf

  EndWhile

  If end_brake = "on" Then
    Motor.Stop("BC", "True")
  EndIf
EndFunction

Move Along Wall

This move sync variation keeps the robot aligned to a wall using a fixed speed difference between the motors.

MoveSync_Encoder_On_Wall()

The motors are adjusted to push slightly against the wall on one side (left or right). Great for guiding along borders or guiding rails. 

Function MoveSync_Encoder_On_Wall(in number encoder_cm, in number max_speed, in string side, in string acceleration, in string deceleration, in string end_brake)
  Motor.ResetCount(@left_motor)
  Motor.ResetCount(@right_motor)
  cm2degree(encoder_cm, encoder)
  exit_condition = 1

  While exit_condition = 1
    left_encoder = Motor.GetCount(@left_motor)
    right_encoder = Motor.GetCount(@right_motor)
    CurrentEncoder = (Math.Abs(left_encoder) + Math.Abs(right_encoder)) / 2

    If acceleration = "on" Then
      V = (CurrentEncoder / @AccelerateEncoder) * (max_speed - @start_speed) + @start_speed
      If V > max_speed Then 
        V = max_speed 
      EndIf
    Else
      V = max_speed
    EndIf

    If deceleration = "on" And encoder - CurrentEncoder < @DecelerateEncoder Then
      V = ((encoder - CurrentEncoder) / @DecelerateEncoder) * (max_speed - @start_speed) + @start_speed
      If V < @start_speed Then 
        V = @start_speed 
      EndIf
    EndIf

    Motor_Left = V
    Motor_Right = V
    If side = "left" Then
      Motor_Right += @speed_dif_wall
    Else
      Motor_Left += @speed_dif_wall
    EndIf

    Motor.StartPower(@left_motor, Motor_Left)
    Motor.StartPower(@right_motor, Motor_Right)

    If Math.Abs(CurrentEncoder) > encoder Then
      exit_condition = 0
    EndIf
  EndWhile

  If end_brake = "on" Then
    Motor.Stop("BC", "True")
  EndIf
EndFunction

Wall-Guided Move Until Color

Combines wall-following behavior with color detection. This allows the robot to hug a wall and stop only when a specific color is reached.

MoveSync_OnWall_To_Color()

Adjusts motor speed just like the wall-following function, and checks color at the same time. Works with brown, black, green, and white surfaces. 

Function MoveSync_OnWall_To_Color(in number max_speed, in string side, in string acceleration, in string end_brake, in string colour)
  Motor.ResetCount(@left_motor)
  Motor.ResetCount(@right_motor)
  exit_condition = 1

  While exit_condition = 1
    left_encoder = Motor.GetCount(@left_motor)
    right_encoder = Motor.GetCount(@right_motor)
    CurrentEncoder = (Math.Abs(left_encoder) + Math.Abs(right_encoder)) / 2

    If acceleration = "on" Then
      V = (CurrentEncoder / @AccelerateEncoder) * (max_speed - @start_speed) + @start_speed
      If V > max_speed Then 
        V = max_speed 
      EndIf
    Else
      V = max_speed
    EndIf

    Motor_Left = V
    Motor_Right = V
    If side = "left" Then
      Motor_Right += @speed_dif_wall
    Else
      Motor_Left += @speed_dif_wall
    EndIf

    Motor.StartPower(@left_motor, Motor_Left)
    Motor.StartPower(@right_motor, Motor_Right)

    HSV_Read(@left_sensor, val)
    If val = 3 Then e
      xit_condition = 0 
    EndIf
                
    HSV_Read(@right_sensor, val)
    If val = 3 Then 
      exit_condition = 0 
    EndIf
  EndWhile

  If end_brake = "on" Then
    Motor.Stop("BC", "True")
  EndIf
EndFunction

Turning & Rotation

The following functions handle all types of turning: arc-based movement, in-place spinning, and S-shaped shifting using both large and medium EV3 motors. Each movement is calculated based on the geometry of the robot and uses trigonometric formulas to ensure precision.

Arc_Move()

This function allows the robot to move along a circular path. It calculates two arc lengths: one for the inner wheel (L1) and one for the outer wheel (L2), based on the given radius R and rotation angle degrees. The ratio P = L1 / L2 is used to adjust motor speeds for smooth arc movement.

The conversion from centimeters to motor degrees is handled by cm2degree(), and the arc lengths are derived using: L = 2πR × (angle / 360). An additional correction (+0.2) is applied to account for mechanical offsets in wheel distance. 

Function Arc_Move(in number degrees, in number R, in string direction, in number speed)
  Motor.ResetCount("BC")
  cm2degree(1, dist)
  R1 = R * dist
  R2 = R1 + (@wheel_distance + 0.2) * dist
  L1 = 2 * Math.Pi * R1 * degrees / 360
  L2 = 2 * Math.Pi * R2 * degrees / 360
  P = L1 / L2

  If @motor_type = "large" Then
    If direction = "right" Then
      Motor.MoveSync("BC", speed, speed * P, L2, "true")
    Else
      Motor.MoveSync("BC", speed * P, speed, L2, "true")
    EndIf
  Else
    If direction = "right" Then
      Motor.MoveSync("BC", -1 * speed, speed * P, L2, "true")
    Else
      Motor.MoveSync("BC", speed * P, -1 * speed, L2, "true")
    EndIf
  EndIf
EndFunction

Arc_Move_Encoder()

This version of arc movement uses encoder feedback instead of predefined arc length. It accepts motor speeds for the left and right wheels independently and keeps running until the average encoder count reaches the target distance.

For medium motors, where MoveSync is not available, a PD controller is used to adjust motor speeds based on encoder differences. The error is calculated as: error = left_encoder × sign + right_encoder × ratio, where sign ensures proper correction depending on motor direction, and ratio keeps the curve shape. 

Function Arc_Move_Encoder(in number encoder_cm, in number left_speed, in number right_speed, in string end_brake)
  Motor.ResetCount("B")
  Motor.ResetCount("C")
  error_old = 0
  exit_condition = 1
  cm2degree(encoder_cm, encoder)

  If @motor_type = "large" Then
    Motor.MoveSync(@stop_string, left_speed, right_speed, encoder, "True")
  Else
    While exit_condition = 1
      left_encoder = Motor.GetCount(@left_motor)
      right_encoder = Motor.GetCount(@right_motor)
      CurrentEncoder = (Math.Abs(left_encoder) + Math.Abs(right_encoder)) / 2
                  
      sign = Math.Abs(left_speed * right_speed - 1) - Math.Abs(left_speed * right_speed)
      ratio = left_speed / right_speed
      error = left_encoder * sign + right_encoder * ratio
      error_old = error
                  
      P = @kp_move_sync_vir * error
      D = @kd_move_sync_vir * (error - error_old)
                  
      Motor_Left = left_speed - (P + D) * sign
      Motor_Right = right_speed - (P + D)
      Motor.StartPower(@left_motor, Motor_Left)
      Motor.StartPower(@right_motor, Motor_Right)
                  
      If CurrentEncoder >= encoder Then
        exit_condition = 0
      EndIf
    EndWhile
  EndIf

  If @debug = "off" Then
    LCD.Clear()
    LCD.Write(1, 10, Motor.GetCount(@left_motor))
    LCD.Write(1, 20, Motor.GetCount(@right_motor))
    LCD.Update()
  EndIf

  If end_brake = "on" Then
    Motor.Stop(@stop_string, "True")
  EndIf
EndFunction

Turn_In_Place()

Rotates the robot in place by running the left and right motors in opposite directions. The amount of rotation in degrees is first converted to linear distance using: distance = angle × wheel_distance × π / 360. This value is then passed to Arc_Move_Encoder() to handle exact movement using encoder feedback. 

Function Turn_In_Place(in number degrees, in string direction, in number speed, in string end_brake)
  move_tank_cm = degrees * @wheel_distance * Math.Pi / 360

  If direction = "left" Then
    If @motor_type = "large" Then
      left_speed = -1 * speed
      right_speed = speed
    Else
      left_speed = speed
      right_speed = speed
    EndIf
  Else
    If @motor_type = "large" Then
      left_speed = speed
      right_speed = -1 * speed
    Else
      left_speed = -1 * speed
      right_speed = -1 * speed
    EndIf
  EndIf

  Arc_Move_Encoder(move_tank_cm, left_speed, right_speed, end_brake)
EndFunction

S_Move()

This function makes the robot shift to the side in an S-shaped motion. The angle needed to perform each curve is calculated using the inverse cosine formula: degrees = arccos(1 - d / D) × (2D / wheel_diameter) × (180 / π), where d is the lateral shift and D is the distance between wheels.

The robot performs two consecutive arcs in opposite directions, resulting in a smooth lateral displacement. Useful for centering or aligning without changing orientation. 

Function S_Move (in number distance, in string direction, in string end_brake)
  degrees = Math.ArcCos(1 - distance / @wheel_distance) * 2 * @wheel_distance / @wheel_diameter * 180 / Math.Pi

  If @motor_type = "large" Then
    If direction = "right" Then
      Motor.MoveSync("BC", @turn_speed, 0, degrees, "True")
      Motor.MoveSync("BC", 0, @turn_speed, degrees, "True")
    Else
      Motor.MoveSync("BC", 0, @turn_speed, degrees, "True")
      Motor.MoveSync("BC", @turn_speed, 0, degrees, "True")
    EndIf
  Else
    If direction = "right" Then
      Motor.MoveSync("BC", -1 * @turn_speed, 0, degrees, "True")
      Motor.MoveSync("BC", 0, @turn_speed, degrees, "True")
    Else
      Motor.MoveSync("BC", 0, @turn_speed, degrees, "True")
      Motor.MoveSync("BC", -1 * @turn_speed, 0, degrees, "True")
    EndIf
  EndIf

  If end_brake = "on" Then
    Motor.Stop("BC", "True")
  EndIf
EndFunction

Alignment Functions

This function ensures that the robot is perfectly aligned to a wall or fixed object. It is especially useful for resetting orientation before starting a precise movement or routine. The technique works by driving both motors forward or backward and waiting for one motor to slow down due to wall contact, indicating alignment.

The function continuously checks the current speed of each motor. If one of them significantly drops (i.e., when it hits the wall), or a maximum time limit is reached, the motors stop.

For medium motors, the direction is handled differently because one motor is mounted in reverse. The direction check ensures compatibility with both large and medium motor configurations. 

Function alignment(in string direction, in number speed)
  exit_condition = 0

  If @motor_type = "large" Then
    If direction = "forward" Then
      Motor.Start("B", speed)
      Motor.Start("C", speed)
    Else
      Motor.Start("B", speed)
      Motor.Start("C", speed)
    EndIf
  Else
    If direction = "forward" Then
      Motor.Start("B", speed)
      Motor.Start("C", -1 * speed)
    Else
      Motor.Start("B", -1 * speed)
      Motor.Start("C", speed)
    EndIf
  EndIf

  Program.Delay(400) ' Initial delay to start the movement
  Time.Reset1()

  While exit_condition = 0
    If Time.Get1() > @alignment_time Then
      exit_condition = 1
    EndIf
  EndWhile

  Motor.Stop("BC", "True")
EndFunction

Sensor Reading: Color & Distance Sensors

EV3 sensors can operate in different modes, depending on what kind of data you want to gather. Below we explain the available modes and how to use them effectively in EV3 Basic for Color Sensors and Distance Sensors (Ultrasonic and Infrared).

Color Sensor Modes

    Mode 0 – Reflected Light Intensity: returns a percentage (0–100) of reflected light. Use Sensor.ReadPercent(port). Mode 1 – Ambient Light: returns a percentage of surrounding light. Use Sensor.ReadPercent(port). Mode 2 – Color Detection: returns a code (0–7) for standard LEGO colors. Use Sensor.ReadRawValue(port, 0). Mode 4 – RGB Raw Values: returns an array of 3 values (red, green, blue). Use Sensor.ReadRaw(port, 3). 
' Mode 0 – Reflected Light Intensity
Sensor.SetMode(3, 0)
value = Sensor.ReadPercent(3)
LCD.Text(1, 10, 10, 2, "Reflected: " + value)
' Mode 1 – Ambient Light
Sensor.SetMode(3, 1)
value = Sensor.ReadPercent(3)
LCD.Text(1, 10, 10, 2, "Ambient: " + value)
' Mode 2 – Color Detection
Sensor.SetMode(3, 2)
colorCode = Sensor.ReadRawValue(3, 0)

Colors[0] = "UNKNOWN"
Colors[1] = "BLACK"
Colors[2] = "BLUE"
Colors[3] = "GREEN"
Colors[4] = "YELLOW"
Colors[5] = "RED"
Colors[6] = "WHITE"
Colors[7] = "BROWN"

LCD.Text(1, 33, 40, 2, "Color: " + Colors[colorCode])
' Mode 4 – RGB Values
Sensor.SetMode(3, 4)
RGB = Sensor.ReadRaw(3, 3)
red = RGB[0]
green = RGB[1]
blue = RGB[2]

LCD.Text(1, 10, 30, 2, "R: " + red)
LCD.Text(1, 10, 50, 2, "G: " + green)
LCD.Text(1, 10, 70, 2, "B: " + blue)

Ultrasonic Sensor Modes

    Mode 0 – Distance in mm: use Sensor.ReadRawValue(port, 0). Mode 1 – Distance in tenths of an inch: use Sensor.ReadRawValue(port, 0). 
' Ultrasonic Distance in cm (mode 0)
Sensor.SetMode(4, 0)
distance_mm = Sensor.ReadRawValue(4, 0)
LCD.Text(1, 45, 55, 2, distance_mm / 10 + " cm")
' Ultrasonic Distance in inches (mode 1)
Sensor.SetMode(4, 1)
distance_tenthinches = Sensor.ReadRawValue(4, 0)
LCD.Text(1, 45, 55, 2, distance_tenthinches / 10 + " in")

Infrared Sensor Modes

    Mode 0 – Proximity: returns distance to an obstacle in cm. Use Sensor.ReadPercent(port). Mode 1 – Beacon (Direction + Distance): returns array [direction, distance]. Use Sensor.ReadRaw(port, 2). Mode 2 – Remote Buttons: returns button codes using Sensor.ReadRawValue(port, channel). 
' IR Sensor – Proximity
Sensor.SetMode(4, 0)
distance = Sensor.ReadPercent(4)
LCD.Text(1, 10, 10, 2, distance + " cm")
' IR Sensor – Beacon (Direction + Distance)
Sensor.SetMode(4, 1)
data = Sensor.ReadRaw(4, 2)
direction = data[0]
distance = data[1]
LCD.Text(1, 10, 20, 2, "Dir: " + direction)
LCD.Text(1, 10, 40, 2, "Dist: " + distance)
' IR Sensor – Remote Control Button
Sensor.SetMode(4, 2)
channel = 0 ' choose channel 0 to 3
buttonCode = Sensor.ReadRawValue(4, channel)
LCD.Text(1, 10, 60, 2, "Button: " + buttonCode)

Always remember to Sensor.SetMode() before using any sensor. The wrong mode will give incorrect results or crash your code.

HSV & RGB Detection

Color sensors on the EV3 can detect light intensity in several modes. Two of the most powerful modes are RGB (Red, Green, Blue) and HSV (Hue, Saturation, Value). RGB measures the amount of red, green, and blue light reflected by a surface, while HSV transforms these values to make it easier to classify colors based on how we perceive them visually.

What is RGB?

RGB stands for Red, Green, Blue. The sensor provides three values representing how much of each color is reflected by an object. For example, white surfaces reflect all three colors highly, while black absorbs all. RGB is helpful for detecting subtle differences in color, but may vary based on lighting conditions.

What is HSV?

HSV stands for Hue, Saturation, and Value. It's a way of interpreting RGB data in a more intuitive way:

    Hue: the actual color (e.g., red, green, blue), measured in degrees on a 0–360° scale. Saturation: how strong or pure the color is (0 = gray, 1 = fully saturated color). Value: brightness of the color (0 = black, 1 = full brightness).
This format helps in classifying colors even under inconsistent lighting by focusing on color proportions.

Read_RGB(port)

This function continuously reads RGB values from a color sensor and displays both live and maximum values for red, green, and blue. Useful for color calibration and debugging.

Function Read_RGB (in number port)
  ' Initialize values
  red = 0
  green = 0
  blue = 0
  MaxRed = 0
  MaxGreen = 0
  MaxBlue = 0

  ' Set color sensor mode to RGB (mode 4)
  Sensor.SetMode(port, 4)

  LCD.Clear()
  ' Keep reading until center button is pressed
  While Buttons.GetClicks() <> "E"
    ' Read RGB values from the chosen port
    If port = 1 Then 
      Sensor1.Raw3(red, green, blue)
    If port = 2 Then 
      Sensor2.Raw3(red, green, blue)
    If port = 3 Then
      Sensor3.Raw3(red, green, blue)
    If port = 4 Then
      Sensor4.Raw3(red, green, blue)

    ' Save maximum values
    If red > MaxRed Then
      MaxRed = red
    If green > MaxGreen Then 
      MaxGreen = green
    If blue > MaxBlue Then 
      MaxBlue = blue

    ' Display current and max RGB values
    LCD.Clear()
    LCD.Write(1,1, "R: ") : LCD.Write(20,1, red)
    LCD.Write(1,15, "G: ") : LCD.Write(20,15, green)
    LCD.Write(1,30, "B: ") : LCD.Write(20,30, blue)
    LCD.Write(1,45, "maxR: ") : LCD.Write(40,45, MaxRed)
    LCD.Write(1,60, "maxG: ") : LCD.Write(40,60, MaxGreen)
    LCD.Write(1,75, "maxB: ") : LCD.Write(40,75, MaxBlue)
    LCD.Update()
    Wait(0.3)
  EndWhile

  ' Wait so values remain visible
  Wait(5)
EndFunction

HSV_Read(port, out color)

This function reads RGB values, normalizes them, and converts to HSV. Based on hue, saturation, and value, it determines a simplified color code (e.g., red, green, blue, black, white).

Function HSV_Read (in number port, out number color)
  Sensor.SetMode(port, 4)
  red = 0
  green = 0
  blue = 0

  ' Read RGB values depending on port
  If port = 1 Then 
    Sensor1.Raw3(red, green, blue)
  If port = 2 Then
    Sensor2.Raw3(red, green, blue)
  If port = 3
    Then Sensor3.Raw3(red, green, blue)
  If port = 4 Then 
    Sensor4.Raw3(red, green, blue)

  ' Normalize values to 0-255 range
  red = red / @max_R * 255
  green = green / @max_G * 255
  blue = blue / @max_B * 255

  ' Find min and max RGB components
  max = Math.Max(Math.Max(red, green), blue)
  min = Math.Min(Math.Min(red, green), blue)

  ' Hue calculation
  If max = min Then 
    hue = 0
                
  If max = red Then 
    If green >= blue Then
      hue = 60 * (green - blue) / (max - min)
    Else
      hue = 60 * (green - blue) / (max - min) + 360
    EndIf
  If max = green Then 
    hue = 60 * (blue - red) / (max - min) + 120
  If max = blue Then 
    hue = 60 * (red - green) / (max - min) + 240

  ' Saturation and Value
  sat = If(max = 0, 0, 1 - min / max)
  val = max / 255

  ' Determine color based on HSV thresholds
  If sat >= 0.55 And val >= 0.09 Then
    If hue <= 7 Then 
      color = 5 ' red
    ElseIf hue <= 62 Then 
      color = 4 ' yellow
    ElseIf hue <= 220 Then 
      color = 3 ' green
    ElseIf hue <= 270 Then 
      color = 2 ' blue
    Else 
      color = 5 ' red
  ElseIf val <= 0.25 And val >= 0.05 Then
    color = 1 ' black
  Else
    If val > 0.25 Then 
    color = 6 ' white 
    Else 
      color = 0 ' none
  EndIf

  ' Debug display
  If @debug = "on" Then
    LCD.Clear()
    LCD.Write(20, 20, "r=" + red)
    LCD.Write(20, 40, "g=" + green)
    LCD.Write(20, 60, "b=" + blue)
    LCD.Write(20, 80, "hue=" + hue)
    LCD.Write(20, 100, "col=" + color)
    LCD.Update()
  EndIf
EndFunction

Wait & Button Functions

These functions are used for implementing delays and user interaction via button presses on the EV3 brick. They are essential for creating pauses in execution and for waiting until the user gives a signal to proceed, such as pressing the center button.

Wait()

The Wait() function halts the program for a specified number of seconds. It uses the EV3 timer system by resetting Timer5 and waiting in a loop until the desired time has passed. The time is measured in milliseconds, so the input seconds are multiplied by 1000. We are not using Program.delay() because this function repeats last code line for that specified time. 

Function Wait (in number time_in_sec)
  Time.Reset5()
  While Time.Get5() < time_in_sec * 1000
  EndWhile
EndFunction

wait_until_press()

The wait_until_press() subroutine pauses execution until the center button on the EV3 brick is pressed. It continuously checks the button state using Buttons.GetClicks() and exits the loop when the button "E" (Enter) is detected.

This is commonly used at the start of programs to ensure the user is ready before the robot begins moving or for example to stop the robot in middle of the program to measure something or to just check its position. 

Sub wait_until_press
  While Buttons.GetClicks() <> "E" 'center button 
  EndWhile
EndSub

Threading & Background Processes

This function is an example of a continuous background task that runs parallel to the main program. In EV3-G or CLEV3R environments, such background subroutines are useful for keeping time, managing manipulators, or running diagnostics while the main algorithm continues. Especially to always keep arms/claws and lifts in tension so they won't lose position, or simple to be able to grab objects.

The subroutine shown below uses Time.Reset9() to track how long the program has been running, triggering a sound alert after 2 minutes using Speaker.Tone(). It also implements a PD controller for manipulating the robot arm's claw and lift, using motors A and D. These are controlled based on error correction between the current and target positions deltaA and deltaD. 

' Starting the Thread:
Thread.Run = Thread_Sub

' Main Function
Sub Thread_Sub
  Time.Reset9() ' reset timer for 2-minute countdown
  @stop_sound = 0
  erroldA = 0
  erroldD = 0
  
  Motor.ResetCount("AD")
  
  While 1 > 0 ' Infinite loop
  
    ' Arm Manipulation Logic
    errD = deltaD - Motor.GetCount("D")
    errA = deltaA - Motor.GetCount("A")
  
    vitD = kpLift * errD + kdLift * (errD - erroldD)
    erroldD = errD
    vitA = kpClaw * errA + kdClaw * (errA - erroldA)
    erroldA = errA
  
    Motor.StartPower("D", vitD)
    Motor.StartPower("A", vitA)
  
  EndWhile
EndSub

This type of threaded subroutine ensures your robot can perform real-time tasks like adjusting manipulators or monitoring sensors, even while another main task is executing simultaneously.

How to Use the Thread

This thread is designed to work with a lift and claw system controlled by delta values. To use it properly:

    Start position of the lift and claw must will be at 0. Call System_Init before anything else to set initial delta values: 
Sub System_Init
  @DeltaA = 0
  @DeltaD = 0
  Wait(0.5)
EndSub

After that, you can use predefined procedures like Lift_Up, Claw_Close, etc. to control the manipulators. Each of them changes the delta value, and the thread continuously adjusts motor power to reach that position. 

Sub Lift_Up
  @DeltaA = 650
EndSub

Sub Claw_Close
  @DeltaD = 300
  Wait(0.3) ' use wait it you want to wait for the action to finish
EndSub

These commands are non-blocking when using the _No_Wait variant, so you can chain multiple movements together in parallel while the thread adjusts in the background.

The End

Congratulations! You have finished the EV3 lesson.

Explore more lessons via the navigation above. For any feedback do not hesitate to contact. Contact here


© 2025 - 2025 HyperLine Robotics. All Rights Reserved.