RobotC for VEX-IQ Computational Thinking and Engineering For Kids

An introduction to programming in RobotC using VEX-IQ hardware

Date: August of 2014

Properties of Storming Robots

1|Page
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Before You Start 4

SCOPE 4
BACKGROUND NEEDED: 4
DOWNLOAD FIRMWARE 4
Chatper 1 - Using the Configuration Tool 5
Chapter 2 - Debugging 7
USING THE LCD DISPLAY 7
USING SOUND 7
USING THE DEBUGGER 7
WATCH IT STEP BY STEP 8
WARM UP - CHALLENGE 10
Chapter 3 - Motors 13
3.1) KNOW YOUR DEVICE 13
3.2) MOST COMMONLY USED FUNCTIONS 13
3.3) CODE SAMPLE SEGMENT 14
Chapter 4 - Bumper Sensor 15
4.1) KNOW YOUR SENSOR 15
4.2) MOST COMMONLY USED FUNCTIONS 15
4.3 ) CODE EXAMPLE SEGMENT15
Chapter 5 - Touch LED Sensor 16
Chapter 6 - Color Sensor 17
6.1) KNOW THE SENSOR 17
TO CONFIGURE TO GRAYSCALE - SENSORVEXIQ_COLORGRAYSCALE 17
TO CONFIGURE TO COLOR HUE - SENSORVEXIQ_COLORHUE 18
TO CONFIGURE TO COLOR HUE - SENSORVEXIQ_COLOR12COLOR 18
6.2) MOST COMMONLY USED FUNCTIONS 19
6.3) CODE SAMPLE SEGMENT 20
Chapter 7 - Distance Sensor 21
7.1) KNOW THE SENSOR 21
7.2) MOST COMMONLY USED FUNCTIONS 21
7.3) CODE SAMPLE SEGMENT 23
7.4) OTHER APIS 23
Chapter 8 - Gyro Sensor 24
8.1) KNOW YOUR SENSOR 24
8.2) MOST COMMONLY USED FUNCTIONS 25
8.3) GYRO SENSOR CODE SAMPLE 26
Chapter 9 – Motor Encoder 27
2|Page
9.1) KNOW YOUR DEVICE 27
9.2) MOST COMMONLY USED FUNCTIONS 27
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

9.3) CODE SEGMENT SAMPLE 30

Chapter 10 – Feedback Control (PID) 31
Fundamentals of PID Controller 32
Using PID Algorithm. 32
TARGET POWER (TP) 33
TARGET VALUE (TVAL) 34
PROPORTIONAL FACTOR (KP) 34
INTEGRAL FACTOR (KI) 34
DERIVATIVE FACTOR (KD) 36
Steps to tune 37
STEP 1) TUNE THE KP FIRST. 37
STEP 2) TUNE THE KI. 37
STEP 3) TUNE THE KD. 38
THINGS WILL CHANGE YOUR TUNING FACTORS 38

3|Page
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Scope
There are over hundreds of APIs. You should look into the Functions Explorers and their help windows to
explore them. Most of them are listed in the TMotorTypes or RobotsInstrinsics.h.

In this tutorial, only the most commonly used APIs and basic tools are covered in this tutorial. You should
always consult the Functions Explorers. You often can right-click on a particular function or system variables to
find out what other APIs, RobotC/VEX-IQ data types, etc. available.

Background needed:
1. You should have already known some basics in programming in C/C++:
o Primitive data types: char, int, float, long, bool, unsigned…
o Some pre-processor directives: #define , #undef, #include
o Basics in control structure statements:
o if … else if …. Else
o switch
o while … do { } while
o Single array
2. Basics in working with functions:
o void func()
o void func(int, …. )
o int func(…)
o bool func(…), etc.
3. More advanced ones will be good too:
o typedef
o Simple pointers
o Simple structure
o typedef enum

Download Firmware
In order to be able to begin programming your VEX-IQ robot, you must first download the most up to date
firmware from the VEX website. Go to http://www.vexrobotics.com/vexiq/software/firmware, and follow the
instructions to get started. (You must also download RobotC firmware before programming. Simply open
RobotC and select Firmware Download from the top of the screen.)

This tutorial was written based on Version 4.25, and VEX IQ Firmware Update Utility for Windows dated
July of 2014. You must follow their installation process of this firmware because it may actually need to update
the firmware of the sensors and motor devices as well.

4|Page
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

The following preprocessor directives will be inserted at the top of your program:
#pragma config(Motor, motor9, LM,tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor, motor10,RM,tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor, motor11,ARM,tmotorVexIQ, PIDControl, encoder)

You may also do these yourself:

#define LM motor9
#define RM motor10
… etc.

setMotorEncoderUnits(encoderCounts);
SensorType[LM] = tmotoVexIQ;
setMotorBrakeMode(LM, motorBrake);
…etc.

5|Page
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

The following preprocessor directives will be inserted at the top of your program:
#pragma config(Sensor, port1, Leye, sensorVexIQ_ColorGrayscale)
#pragma config(Sensor, port2, Reye, sensorVexIQ_ColorGrayscale)
#pragma config(Sensor, port3, Lping, sensorVexIQ_Distance)
#pragma config(Sensor, port4, Rping, sensorVexIQ_Distance)
#pragma config(Sensor, port5, Gyro, sensorVexIQ_Gyro)
#pragma config(Sensor, port6, DiscoLight, sensorVexIQ_Color12Color)
#pragma config(Sensor, port7, FrontBumper, sensorVexIQ_LED)
#pragma config(Sensor, port8, BackBumper, sensorVexIQ_Touch)

You may also do these yourself:

#define Reye port1
#define Leye port2
… etc.

SensorType[Reye] = sensorVexIQ_ColorGrayscale;
SensorType[Lping] = sensorVexIQ_Distance;
SensorType[Rping] = sensorVexIQ_Distance;
SensorType[Gyro] = sensorVexIQ_Gyro;
SensorType[DiscoLight] = sensorVexIQ_Color12Color;
SensorType[FrontBumper] = sensorVexIQ_LED;
SensorType[BackBumper] = sensorVexIQ_Touch;

6|Page
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Using the LCD Display

The LCD measures 46 x 128 pixels (i.e. 0<= x <= 127, 0<= y <=45 )

You may display data during execution:

displayTextLine(1, "Hello ");
displayTextLine(3,”%d: %s”, 10, “Storming Robots”);

You should look up all the display functions from the Function explorers.

Using Sound
Play a sound to signal a certain stage of execution can also be very informative too if you use wisely.
playSound(TSounds);

where Tsounds can be in many values such as soundTada, soundSiren2, etc.. Right click on the function in
the IDE, the IDE will open RobotCIntrinsic.c where you will find the sounds available.

You can play with playSoundfile(…) and playNote(…), but these usually are not very useful in real time
debugging.

Using the Debugger

RobotC comes with probably most powerful debugger available for microcontroller.
You program must be compiled with no errors and downloaded to the controller in order to watch the screen
debugger:

7|Page
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Watch it step by step

Set breakpoint Step into each expressions. Watch the change of the variable here.

8|Page
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

9|Page
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Warm Up - challenge

Challenge 1: Write a single program to do the following:

- Display your name.
- Play a sound. Wait for one second.
- Display your school name on the next line.
- Play another sound, Wait for one second.
- Display your favorite event, followed by another sound. Wait for another one second.
- End the program.

Challenge 2: Write a program to create a variable called “ct”. Create a loop so that your screen will display the
following:

1
2
3
4
5
6
7
8

Challenge 3:
Draw a small rectangle with coordinates with top left point at (10, 20) and bottom right point at (25, 5), .e.g
drawRect(10,20, 25, 5). Make it expand to the max size of screen, then shrink back with dimension of 1 pixel
width and height.

Challenge 4: Given the radius. Generate Circumference and Area of a circle. Only 2 decimal places. Do it in
one’s increment four times. E.g.
R Cir Area
================
7.00 43.96 314.15
14.00 65.94 153.86
21.00 131.95 1385.44
28.00 175.93 2463.01

10 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Challenge 5: Given the width and length of a rectangle. Generate the area of the biggest circle to fit inside this
rectangle. E.g.

W L CirA
2 53.14

Challenge 6: Draw the rectangle, as well as the painted complete circle fit inside the rectangle.

W L CirA
50 60 1962.50

Challenge 7: Given the measurement of base and height of a right angle triangle, find out the measurement of
the hypotenuse of the triangle. (Pythagorean Theorem)

B H Hyp

50 60 1962.50

11 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Challenge 8 : (for the advanced)

1. Write a function that outputs a right-side-up triangle of height n and width 2n-1; the output *
for n = 6 would be: ***
*****
*******
2. Write a program to calculate a gcd with two numbers using Euclid Algorithm. This is how *********
it works: **********
*
N M R= N % M
300 64 44 = 300 % 64
64 44 20 = 64 % 44
44 20 4 = 44 % 20
20 4 0 = 20 % 4

3. Write a program to calculate a square root of a number using Newton’s method. This is how it works:

e.g. user wishes to find out square root of 3:

N N/M M Average of N/M and
M
3 3 1 2
1.5 2 1.75
1.71429 1.75 1.73214
1.73196 1.7314 1.73205
1.73205 1.73205 1.73205

Your get your answer when:

1
| M (current) – M (previous) | <= M * 10000

12 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

3.1) Know your device

Type : sensorVexIQ_Motor

You may set the ports to many other type of devices. Look into the TMotorTypes.h at your RobotC installation
directory.

Instead of using the configuration setup wizard, you may do inside your code, e.g.:
SensorType[motor9] = sensorVexIQ_Motor

Note: In the configuration wizard, if you should check the “reversed” box for your a motor port, it will be in a
mirrored position on your chassis. Depending on how your mount your motors, this may avoid confusion by
ensuring that both motors drive in a forward direction when positive speed values are passed to them and vice
versa.

There are other options such as set up for motor feedback control. This is outside the scope of this tutorial.

3.2) Most Commonly Used Functions

ROBOTC for VEX Robotics 4.0 allows for direct control of the VEX IQ's motors using Natural Language
commands. These commands can be used to control multiple motors, individual motors, set motor targets,
stop motors, and more.
Try to be resourceful and use the function window on the left of the screen, which will give you the parameters
needed for each function.
There are two common parameters used in the motor control commands; the motorPort and speed
parameters.
 motorPort values can be assigned by the port number (motor1) or by the name assigned to it in the
Motors and Sensors Setup window (ex. Leftmotor). *Default value is motor1.
 speed values range from -100 to +100. -100 is full power reverse, +100 is full power forward, and 0 is
stopped. Default value is +50.

To move the motors:

setMotor(motor7, 50); //This command sets the motor connected to port 7 to 50%
speed in the forward direction
stopMotor(motor7); //Stops the motor at port 7
setMotorReversed (motor7, bool reverse);
13 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

To make a point turn:

4. setMotor(motor7, 50); /* This block of code will turn the robot in place for 4
seconds*/
setMotor(motor12, -50);
5.

stopMotor(motor7); //Stops the motor at port 7

3.3) Code Sample Segment

RobotC comes with a good amount basic motor movements. Use their samples will be sufficient.

14 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

4.1) Know your sensor

Type : sensorVexIQ_Touch

Before you can use your sensors, you must configure them. Using the Motors and Sensors Setup window is a
quick and easy way to do so. Create a name for each of your sensors and identify their respective port
locations. However, remember: all “pragma” statements must be at the top of your file.

Instead of using the configuration setup wizard, you may do inside your code, e.g.:
SensorType[ port3 ] = sensorVexIQ_Touch

4.2) Most Commonly Used Functions

Bool getBumperValue (nDeviceIndex)

Parameter Explanation Data Type

Return 1= pressed bool
0 = released
nDeviceIndex Port number that the sensor is plugged into tSensors
Note:
Watch out for “Denouncing” Effect. To count # of pressed, your code must code for the
delay or capture a sequence of press and release as a single “pressed”.

4.3 ) Code Example Segment

//This code will make the robot run forward at a speed of 50
//until the 'bumperSensor' is pressed
//Repeat the loop until the Bumper Switch is pressed (returns 1)

repeatUntil(getBumperValue(port8) == 1)
{
//Set both left and right motors to run at 50% speed (continuously)
setMotorSpeed(motor1, 50);
setMotorSpeed(motor6, 50);
}
//Stop the motors after the movement is complete
setMotorSpeed(motor1, 0);

15 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

setMotorSpeed(motor6, 0);

This sensor serves not so much as a sensor but output device. This is more for fun elements. The samples
from RobotC has already well identified how to use it. Therefore, we will skip this topic .

Type: sensorVexIQ_LED

bool getTouchLEDValue(tSensors port1...port12);

void setTouchLEDColor(tSensors port1...port12, TSimpleColors color );
void setTouchLEDHue(tSensors port1...port12, const unsigned short HueValue);
void setTouchLEDRGB(tSensors port1...port12, red, green, blue);
Unsigned short setTouchLEDRGB (nDeviceIndex);

16 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

6.1) Know the sensor

The color sensor is an input device that can measure colors in a variety of ways. It can measure simple colors
(Red, Blue, Orange, etc), overall color hue, independent red, blue and green channels, ambient light, and even
the proximity of objects. The values returned by the sensor depend on which command is being used to
control it (below). The default port for the color sensor is port #9.

Values returned will vary depending on the surface being detected, ambient light, and other environmental
factors (example: brighter ambient light may cause the sensor to return higher values than average.)

For those familiar with the NXT light sensor, the methodology is very similar. You must take the sensor
readings over dark and light, then average those two numbers to get your threshold.

Types: sensorVexIQ_ColorNotActive
sensorVexIQ_ColorGrayscale
sensorVexIQ_ColorHue
sensorVexIQ_Color12Color

To configure to Grayscale - sensorVexIQ_ColorGrayscale

Configures the color sensor to return the grayscale value of a detected object.

1. 3 ... 359
Darkest Brightest

Actual values returned will vary depending on the surface being detected, ambient light, and other
environmental factors (example: brighter ambient light may cause the sensor to return higher average values
than normal.) *Note that due to the nature of our typical robotics challenges, we will normally focus on using
Color Greyscale mode.

17 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

To configure to Color Hue - sensorVexIQ_ColorHue

Configures the color sensor to return values between 0 and 255, with each value corresponding to a specific
shade of the color spectrum.

359
255

To configure to Color Hue - sensorVexIQ_Color12Color

Configures the color sensor to return one of 12 different color names depending on the color of the object
detected. If no color is detected, colorNone will be returned instead.
 colorNone
 colorRedViolet
 colorRed
 colorDarkOrange
 colorOrange
 colorDarkYellow
 colorYellow
 colorLimeGreen
 colorGreen
 colorBlueGreen
 colorBlue
 colorDarkBlue
 colorViolet

18 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

6.2) Most Commonly Used Functions

Unsigned short getColorGrayscale(nDeviceIndex);

Explanation Data Type

Return Function returns 0 to 359 long
Parameter nDeviceIndex: port # tSensors port1…12

long getColorValue(nDeviceIndex);

Explanation Data Type

Return Function returns 0 to 255 long
Parameter nDeviceIndex: port # tSensors port1…12

long getColorHue(nDeviceIndex);

Explanation Data Type

Return Function returns 0 to 255 long
This is different than normal "hue" readings
which range from 0-359.
Parameter nDeviceIndex: port # tSensors port1…12

long getColorValue(nDeviceIndex);

Explanation Data Type

Return Function returns raw color value long
Parameter nDeviceIndex: port # tSensors port1…12

TSimpleColors getColorName(nDeviceIndex);

Parameter Explanation Data Type

Return Function returns an enumeration type to TSimpleColors
represent different colors by names. Such as
colorYellow, colored, etc.
Look it up from the Help
Parameter nDeviceIndex: port # tSensors port1…12

19 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

6.3) Code Sample Segment

//This is a simple line tracking program using a color sensor plugged into port 9
that is set to Grayscale mode.
int threshold = 150; //Found by taking dark and light readings, then calculating
their average
while(true)
{
if(getColorGrayscale(port9) < threshold)
//If robot sees dark, turn toward light
{
setMotor(motor1,50);
setMotor(motor2,0);
}
else //if robot sees light, turn toward dark
{
setMotor(motor1,0);
setMotor(motor2,50);
}
}

20 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

7.1) Know the sensor

Type : sensorVexIQ_Distance

The distance sensor measures its distance from an object by sending out high frequency sound waves. It
measures the amount of time a sonar wave takes to reflect off of an object and return to the sensor. The
distance sensor also has the ability to detect three different object distances at the same time.
The closest object to the distance sensor can be measured with the getDistanceValue command. The
getDistanceSecondStrongest command can be used to get the distance of the second closest object to the
sensor, and the largest object can be detected with the getDistanceMostReflected command.
Note that the distance sensor returns all values in millimeters for maximum precision. The default port for the
distance sensor is port #7.

May set your sensor type: SensorType[ port# ] = sensorVexIQ_Distance

7.2) Most Commonly Used Functions

short getDistanceValue(tSensors, nDeviceIndex)

Parameter Explanation Data Type

Return Type Returns the value in millimeters. short

The value returned will be for the distance to the

strongest signal by default

nDeviceIndex nDeviceIndex: port # tSensors

short getDistanceStrongest(tSensors, nDeviceIndex)

Parameter Explanation Data Type

Return Type Returns the value of the object the distance short
sensor determines to be the strongest (usually
the closest object).
nDeviceIndex nDeviceIndex: port # tSensors

21 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

short getDistanceSecondStrongest(tSensors, nDeviceIndex)

Parameter Explanation Data Type

Return Type Returns the value of the object that the distance short
sensor determines to be the second strongest
(typically the second closest object).

nDeviceIndex nDeviceIndex: port # tSensors

short getDistanceMostReflective(tSensors, nDeviceIndex)

Parameter Explanation Data Type

Return Type Returns the value of the object that the distance short
sensor determines to be the most reflective
(usually the largest object).

nDeviceIndex nDeviceIndex: port # tSensors

From VexIQ Manual

22 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

7.3) Code Sample Segment

while(getDistanceStrongest(port10) > 100)
{
setMotor(motor1,50); //robot will drive forward until it sees an object
within 100 mm
setMotor(motor2,50);
}
stopAllMotors();

7.4) Other APIs

Although the following functions are not commonly used, I found them to be indeed very useful. However,
you will need to do some experiments to polish the usage.

unsigned short getDistanceStrongest( tSensors port1...port12)

unsigned short getDistanceSecondStrongest( tSensors port1...port12)
unsigned short getDistanceMostReflective( tSensors port1...port12)
unsigned short getDistanceMinRange( tSensors port1...port12)
unsigned short getDistanceMaxRange( tSensors port1...port12)

void setDistanceBrightnessThreshold( tSensors port1...port12, const unsigned short nBrightnessValue)

void setDistanceFilterFactor( tSensors port1...port12, const unsigned short nFilterFactor)
void setDistanceTransmitPower( tSensors port1...port12, const unsigned short nTransmitPower)
void setDistanceMinRange( tSensors port1...port12, const unsigned short nMinDistanceInMM)
void setDistanceMaxRange( tSensors port1...port12, const unsigned short nMaxDistanceInMM)
void setDistanceObjectFilterMode( tSensors port1...port12, TDistanceFilterMode nFilterMode)
void setDistanceSNRThreshold( tSensors port1...port12, const unsigned short nSNRValue)

typedef enum
{
strongestSignal = 0,
secondStrongestSignal = 1,
mostReflective =2
} TDistanceFilterMode;

23 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

8.1) Know Your Sensor

The Gyro Sensor measures the rotational angle of the robot. It constantly keeps track of the robot's angular
position and its direction. For simplification, it only tracks rotation in one axis based off of a “zero” point.
With the VEX IQ Gyro, counter-clockwise rotation will increase (+) the Gyro's value in degrees. Clockwise
rotation will decrease (-) the Gyro's value. In order to read the Gyro's value the getGyroDegrees command is
used. This command returns the current rotational value of the sensor in units of degrees.

Type: sensorVexIQ_Gyro

24 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

8.2) Most Commonly Used Functions

void resetGyro(nDeviceIndex)

Parameter Explanation Data Type

Return Function returns no value. See Usage below void
nDeviceIndex nDeviceIndex: port # tSensors port1…
port12
Note:
o Resets the value of the Gyro Sensor and use its current position as 'zero degrees’
o This command is useful when the robot needs to turn a specific number of degrees from
its current position

long getGyroDegrees(nDeviceIndex)

Explanation Data Type

Return  Returns the accumulated value of the long
Gyro Sensor in degrees
 Direction can be determined by positive
and negative values

nDeviceIndex nDeviceIndex: port # tSensors port1… port12

float getGyroDegreesFloat(nDeviceIndex)

Explanation Data Type

Return Returns a floating point decimal value for float
greater precision as opposed to
getGyroDegrees
nDeviceIndex nDeviceIndex: port # tSensors port1… port12

Long getGyroRate(nDeviceIndex)

Explanation Data Type

Return Returns the rate of movement in degrees long
per second
nDeviceIndex nDeviceIndex: port # tSensors port1… port12

25 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

8.3) Gyro Sensor Code Sample

//This program will reset the gyro's heading and then turn the robot 90 degrees
from its current position
//Resets the gyro on port 4 to 0 degrees
resetGyro(port4);
//Keep looping until the gyro sensor reads greater than 90 degrees from its current
position
repeatUntil(getGyroDegrees(port4) > 90)
{
//Point turn to the left
setMotorSpeed(motor1, -50);
setMotorSpeed(motor6, 50);
}
//Stop the motors at the end of the turn
setMotorSpeed(motor1, 0);
setMotorSpeed(motor6, 0);

26 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

9.1) Know your device

The VEX IQ Smart Motor allows you to program your robot to move precise distances by utilizing a built- in
encoder. In addition, RobotC incorporates a rich set of functions to allow for greater target control. Because
the VEX motor has a microcontroller to process requests, measure speed and direction, monitor current, and
control the motor, most of the difficult work has been done for you. Your only task is to convert distance of
travel desired for your robot to encoder degrees and plug it into the corresponding functions.

By default, the rotation of the VEX motor is measured in degrees (360 degrees per rotation). Therefore,
assuming that your wheel is attached directly to the motor, you can easily convert centimeters traveled to
rotations of the motor:
1 𝑟𝑜𝑡𝑎𝑡𝑖𝑜𝑛 𝑜𝑓 𝑚𝑜𝑡𝑜𝑟 = 𝑐𝑖𝑟𝑐𝑢𝑚𝑓𝑒𝑟𝑒𝑛𝑐𝑒 𝑜𝑓 𝑡𝑖𝑟𝑒
360 𝑑𝑒𝑔𝑟𝑒𝑒𝑠 = 𝜋 ∗ 𝑑𝑖𝑎𝑚𝑒𝑡𝑒𝑟 𝑜𝑓 𝑡𝑖𝑟𝑒 (𝑖𝑛 𝑐𝑚)
360
= 1 𝑐𝑚
𝜋 ∗ 𝑑𝑖𝑎𝑚𝑒𝑡𝑒𝑟

360
The quantity represents degrees the motor rotates when the tire travels one cm. We can use
𝜋∗𝑑𝑖𝑎𝑚𝑒𝑡𝑒𝑟
this conversion factor multiplied by desired centimeters in order to tell our robot how many degrees to rotate
the motor in order to reach the target.

9.2) Most Commonly Used Functions

void resetMotorEncoder(tMotor nMotorIndex)

Parameter Explanation Data Type

Return Type Function returns no value void
Resets the value of the motor encoder of
the motor to 0.
nMotorIndex Port number that the motor is plugged into tMotor ,motor1 …12
Note:

27 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

void setMotorTarget(tMotor nMotorIndex, float nPosition, int nSpeed)

Parameter Explanation Data Type

Return Function returns no value void
nMotorIndex Port number that the motor is plugged into tMotor
nPosition Target absolute value to which the motor float
will travel to.
nSpeed Desired speed %(- 100 to +100) int
Note:
 This command tells the motor to move an absolute distance.
 nPosition takes values in degrees (unless the motor encoder units are set to a different
mode by the user)
 The resetMotorEncoder command should be used before setMotorTarget in order to ensure
that the motor is starting from zero degrees

Must use waitUntilMotorStop(nMotorIndex) command to ensure that the motor reaches its
target (see sample code below)

void moveMotorTarget(tMotor nMotorIndex, float nPosition)

Parameter Explanation Data Type

Return Type Function returns no value void
nMotorIndex Motor port # tMotor motor1…12
nPosition Target incremental (not absolute) value to float
which the motor will travel.
Note:
 This command tells the robot to move a relative distance. It will add (or subtract) distance
from any previously specified targets, unlike setMotorTarget, which uses absolute distance.
 Setting either the target or the speed to a negative number will cause the motor to move
backwards
 The motor will automatically stop when target is reached

28 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Other useful functions:

 setMotorBrakeMode ( nMotorInded, TMotorBrakeModes ) //
 setMotorEncoderUnits(TMotorEncoderUnitModes nEncoderUnitMode) ;
where :
typedef enum
{
encoderDegrees = 0,
encoderRotations = 1,
encoderCounts = 2, //Raw Encoder Count Mode
} TMotorEncoderUnitModes;

typedef enum TMotorBrakeModes

{
motorCoast = 0,
motorBrake = 1,
motorHold = 2,
#endif
} TMotorBrakeModes;

29 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

9.3) Code Segment Sample

#pragma config(Motor, motor1, tmotorVexIQ, PIDControl, encoder)

#pragma config(Motor, motor6, tmotorVexIQ, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard
!!*//
//This code will move a robot 20 cm.

#define StillMoving(m) ( !getMotorZeroVelocity(nMotorIndex) )

//Robot's tire of diameter 6.37 cm is directly connected to the motor

task main()
{
float cir =PI * 6.37;
float encpercm = 360/cir;
float target = encpercm * 20.0;

resetMotorEncoder(motor1);
resetMotorEncoder(motor6);
setMotorTarget(motor1, target, 50);
setMotorTarget(motor6, target, 50);
while ( StillMoving(motor1) || StillMoving(motor6) )
sleep(10);
}

30 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

This section will use line tracing as an example for using feedback control.

First of all, let’s look at some typical ad-hoc algorithms used:

Possible ad-hoc methods:

1) 2 light sensors (should build a states table (or states machine) whenever you use more than one device
to create many possible actions)
2) A light array of 6 sensors: e.g.
‘1’ == see black .
a. 001100 On the center of the line
b. 000011 To the left of the line
c. 110000 To the right of the line

Possible weighted system methods:

1) With A light array of 5 sensors. Build a binary weighted map, eg.:

The full range of weighted values is shown below. We assign a numerical value to each one.
Binary Value Weighted Value
00001 4
00011 3
00010 2
00110 1
00100 0
01100 -1
01000 -2
11000 -3
10000 -4
00000 -5 or 5 (depending on the previous value)

Use this weighted values to adjust the turning. The range of possible values for the measured position is
-5 to 5.
Now, this method is also considered to be feedback control system.

31 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

PID stands for Proportional – Integral – Derivative and is a control loop feedback mechanism widely used in
industrial control systems. Basically, it is a way to calculate an error value as the difference between a
measured process variable and a desired setpoint. This is often referred as “Closed Loop” System vs. “Open
Loop”. “Closed Loop” system means output is self-adjusted with signal feedback, while “Open Loop” system
generates output without feedback.

These factors help the system to :

- calculate how much to correct in real time execution
- help the system to continuously adjust itself in order to approach the point where it will stop correcting

The goal is to minimize the error by adjusting the process through the use of manipulated variables .

To simplify, let’s use one of our typical activities – line tracing. Our goal is going to be to trace a line smoothly
using only one color sensor.

This section will focus on this topic.

Ad-hoc algorithm PID Algorithm

Basic variables used:

Target power (tp) set a desired power level for our Usually start low such as 50 or even 30
motors if there is no error depending on your system.
Target value (tval) Set a based ideal value. Anything The target location, in the case of line
above or below this value will be tracing, it will be the threshold value
considered as “error”. between white and black line.
“P” value (Kp) Set a factor value which works like a It tells us how far the robot is from the line.
knob to tune present error, Proportional is the first one to use. For some

32 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

system, it is also used to calculate “I” and “D”

factor as well.

“I” value (ki) Set a factor value which works like a It tells us if the robot has been on the line in
knob to tune accumulative errors the last few moments or not. The goal is to
over time. watch the elapse time in between the time to
bring the bot to the ideal target location (ie..
the threshold)
“D” value (kd) Set a factor value which works like a It tells a prediction of future errors.
knob to tune the possible future This “D” means rate of change of error from
error. the last reading done for the control output.

Effects
Term Expression Effect
Proportional Kp * error It reduces a large part of the error based on present time error.
Reduces the final error in a system. Cumulative of a small error over time
Integral Ki * sumError would help us further reduce the error. However, you must extra precaution of
this one. We will go through this later in this chapter.
Derivative Kd * dError / dt Counteracts the Kp and Ki terms when the output changes quickly.
Note: Kp, Ki, and Kd will determine the the magnitude of each turn.

currError = tVal – currReading

pErr = currError * Kp

sumErr = sumErr + currErr

iErr = sumErr * Ki

dErr = (currErr – prevErr ) * Kd

totalErr = pErr + iErr + dErr

leftpower = tp + totalErr
rightpower = tp - totalErr

Let’s take each of these factors and parameters and see how to code them.

Target power (tp)

Should always start with something smaller first, say 30, in order to ensure that we don’t stray too far from the
line.

33 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Target Value (tval)

Assuming the ideal light reading (threshold) to be average of dark and light. E.g.:
int dark = 40;
int light = 260;
int tval = 150;
You do not have to use the average, but some threshold you deem to be more desirable The idea is that this is
the threshold value which you deem to be the ideal value, so anything over or below that will be considered to
be “error”. If you look at PID algorithm online, they refer this to setPoint.

Proportional factor (Kp)

First is to do a best estimate of the Kp. This is how:

Get maximum error = value of light – tVal = 110 (base on example above)

Kp is essentially the factor that you must multiply with the error to get the needed turn. A good rule of thumb
for calculating Kp is to take the target power level for your motors and divide by the maximum possible error.

Kp = 30/110 = 0.27

Now we can say “for every 1 unit of change in the error, we will increase the power of one motor by 0.27”. The
other motor’s power gets decreased by 0.27.

In pseudo code:
Kp = 0.27
tVal = 150
Tp = 30
while (true)
{
currValue = getcolorsensorval
currErr = currValue – tVal
turn = Kp * currErr
lpwr = Tp + turn;
rpwr = Tp – turn;
}
*Note that both Kp and Tp can and should be fine-tuned, based on the performance of the code.

Integral factor (Ki)

34 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Our “I”, or integral, term is going to keep track of the sum of the errors. Each time we get a color sensor
reading and calculate an error, we will add that error to an integral variable:
integral = integral + error

Next, we will create another proportionality constant, called kI, and multiply that by our integral. This product
is then added to Kp*error to get a new value for the totalErr:

TotalErr = (Kp*error) + (kI*integral)

One way to think about this integral term is that it is the controller’s “memory”. The integral is the cumulative
history of the error and gives the controller a method to fix errors that persist for a long time.

In pseudo code:
Kp = 0.27
Ki = 0.1
tVal = 150
Tp = 30
integralErr = currErr = 0
while (true)
{
currValue = getcolorsensorval
currErr = currValue – tVal
integralErr = integralErr + currErr
turn = Kp * currErr + Ki * integralErr
lpwr = Tp + turn;
rpwr = Tp – turn;

*When fine-tuning your code, you may find that the integral is increasing too quickly. If the line is a spiral, you
may even add data overflow. Therefore, you may use some mechanism to do a reset it the line may indeed
go spiral. Possible ways to do this:
1) set some kind data most upper and lowest bound.
2) reset once in a while
3) use a dynamic Ki value based

35 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Derivative factor (kd)

Our “D”, or derivative, term is going to look into the future by assuming that the next change in the error is the
same as the last change in the error. This means that the next error is expected to be the current error plus the
change in the error between the two preceding sensor readings. If we were to plot our error on an xy plane,
this change in error between two consecutive points would be the slope of the line, which is what a derivative
is.
Why is the derivative so useful? If the current error is larger than the previous error, then the D term tries to
correct the error. If the current error is smaller than the previous error, then the D term tries to stop the
controller from correcting the error. The latter is particularly useful. If the error is getting close to zero, then
we are approaching the point where we want to stop correcting. Since the system probably takes a while to
respond to changes in the motors’ power, we want to start reducing the motor power before the error has
actually gone to zero, otherwise we will overshoot.

Pseudo code for final PID controller:

Kp = ?
kI = ?
kD = ?
threshold = 150
Tp = 30
IntegralErr = lastErr = derivative = 0
prevTime = 0
Loop forever
{
Lightvalue = getcolorsensorval
currErr = Lightvalue – tVal
integral = integral + currErr
duration = nVexIQRawTime - prevTime
derivative = (currErr – lastErr)/duration
lastError = currErr
prevTime = nVexIQRawTime

turn =( Kp*currErr) + (Ki*integralErr) + (Kd*derivative)

LM = Tp + turn
RM = Tp – turn
}

Sensors:
Sensors are the first requirement in the process of line following. When selecting the type of sensors, there are
three things you need to keep in mind – response time, sensitivity and ambient light protection. There is
good tutorial about how to choose the right sensors at
http://www.societyofrobots.com/member_tutorials/book/export/html/350 .

36 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

Step 1) Tune the Kp first.

1) Start with simple calculation:

Tp / (largestErr – targetValue) * dur

Where
Tp = target power
LargestErr = highest Error. In line tracing, it will be the value on full white
targetValue = the set point, i.e. the target value. In line tracing, it will be the value of threshold
dur = the duration in second for the robot to go from “LargestErr” to targetValue.

There are two more parameters which affect kp : battery power level and motor current. Unfortunately,
this is not something easy to tell unless you have done a good amount of data collection.

2) Simply put :
a. if robot wobbles  tune Kp  and vice versa.
b. If robot turns too slow, tune Kp 
e.g. Start with 1.0, too sluggish… change to 3.0, wobbles too much…
change to 2.0… too much…. Change to 1.5..etc.
3) Tune the Kp value till the robot smoothly follows a curvy line even. Your robot should go with no
acceleration on straight lines, but speeds up the turn on curves.
4) But then, depending the size of your chassis and location of the light sensors, your robot may very likely
to struggle around 90 degrees turn.

Step 2) Tune the Ki.

1) Start with 0.0
2) In the line tracing scenario, you should allow Ki to increase during the duration when error occurs.
However, you should reset it once it go back to the set point, i.e threshold. So, the code will be
something like this:

If (! At threshold)
ki = ki + 0.1;
else
ki = 0;

But, there are times when the response time is not fast enough, your integral value may get really high
at times. Then, your robot will easily be off line even when the kp alone works well by itself.

37 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

3) So, considering that usually the “kp” takes care of the line tracing pretty well, except 90 degree inner
turns. See the diagram below…

Went off at the

90o turn just fine.
90o turn

Therefore, I find the following will work fine as well:

If (see dark)
ki = ki + 0.1;
else
ki = 0;

You really need to test it out with your robot to see which works better.

Very important point, because of the fact that this takes accumulative errors, you must make sure to
minimum noise. Two things you should at least do it yourself:
a) Shield the light sensor from ambient light
b) Keep the distance between the light sensor to the surface consistent.

Step 3) Tune the Kd.

1) Start with 0.01
2) To be updated

Things will change your tuning factors

1. Power source - If the power source depletes to really low, or you just change your batteries to a set new
ones or ones which produce much higher current, the Kp will no longer work as well as it used to.
Therefore, you need to record your batteries level and/or current when you are satisfied with the tuning
Kp. Does it mean that you need to create various power source levels scenarios in order to come up
with a formula? YES, if you want to a robust robot which runs even after you change batteries!
2. Chassis Size
3. Location of the light sensors
4. Ambient light

38 | P a g e
RobotC for VEX-IQ Computational Thinking and Engineering For Kids

5. Response time of the light sensors

6. Single wheel drive vs 4-wheels drive
7. Frictions from the track the bot runs on.

39 | P a g e