Robotc Programming: Making Your Robot Move
Robotc Programming: Making Your Robot Move
FTC Programming
Agenda
What is RobotC
Diagram your Robot
RobotC Setup and Configuration
How to display text
Your first program
Reading the joystick
Moving your robot
Servos
Timing
Sensors
Encoders
Useful Links
Questions
FTC Programming
What is RobotC
Industry Standard C
Programming Language
+
Programming Development
Environment
+
High Performance Firmware
+
Training and Curriculum Support
FTC Programming
FTC Programming
FTC Programming
FTC Programming
FTC Programming
FTC Programming
RobotC Setup
The first time you fire up the ROBOTC IDE, there are a few quick things you will want to
do before you begin programming a FTC robot.
1. Set menu level from basic to Expert.
a. Window, Menu Level, Expert
2. Set platform type to LEGO Mindstorms NXT + TETRIX.
a. Robot, Platform Type, LEGO Mindstorms NXT + TETRIX
FTC Programming
Firmware
Download latest firmware to NXT brick.
* Note that this only needs to be done the first time you setup a new NXT brick with ROBOTC and when you upgrade
your version of ROBOTC.
** Also note that the battery level must be high enough on the NXT before it will allow you to download new firmware.
a. Make sure NXT is connected via USB and turned on.
b. Open Link Setup dialog
i. Robot, NXT Brick, Link Setup
c. Select NXT in top left corner and press F/W Download button.
d. Select .rfw file in default firmware directory in popup and press Open.
i. For ROBOTC v2.03, the firmware file should be: NXT798.rfw.
e. After a few moments, the Link Setup dialog box will display some verbose
information in the Message Log and your NXT should beep several times and
restart. When complete, you should see a series of messages similar to below
in the Message Log:
FTC Programming
Exercise
RobotC Setup and Configuration
Robot C Setup
Link via USB, Bluetooth, or Samantha to NXT
Tetrix Ranger
FTC Programming
Get to Configuration
Window
FTC Programming
FTC Programming
Motor Setup
FTC Programming
Sensor Setup
FTC Programming
FTC Programming
Exercise
Setup the Ranger Configuration in RobotC
FTC Programming
Task Main
Task main is used to tell the NXT where the beginning of your programs is.
The beginning and end of task main is denoted with curly braces { }
When the program execution reaches the end of the main task, all robot
activity stops.
#include "JoystickDriver.c" // Tells ROBOTC to include the driver file for the joystick.
task main()
// All programs must have this task
{
while(true)
{
motor[rightMotor] = joystick.joy1_y1;
motor[leftMotor] = joystick.joy1_y2;
}
}
FTC Programming
Variables
Variables are the robots way of storing values for later use. They function as
containers or storage for values. Values such as sensor reading can be placed in a
variable and retrieved at a later time for convenient use. A variable is simply a place to
store a value.
Useful types:
Integer, or int values are numbers with no fractional or decimal component.
Floating point (float) numbers are so called because the decimal point floats around
in the value, allowing decimal places to be used.
Strings (string): Text in ROBOTC is always a string. In ROBOTC, the word Hello
is really a collection of letters H, e, l, l, o strung together to form a
single value.
Boolean (bool) values represent truth or logic values, in the form of true or
false.
Use variable names that make sense.
What is more readable?
a = b / c;
Or
speed = distance / time;
FTC Programming
Conditional Statements
If statements are pretty self explanatory. If a, then b.
The syntax (grammar of programming) of an if statement is
If(4 < 100) {
Do stuff;
}
If else statements run if the if statement was false. These are useful
for when there are multiple cases of an instance.
else if (4 == 100) {
Do stuff;
}
Else statements follow the if before it. If the if statement was false
then the else statement will run.
else {
Do stuff;
}
FTC Programming
Boolean Logic
==
<
>
<=
>=
&&
||
equals
less than
greater than
less than or equal to
greater than or equal to
and
or
Loops
For loops run a certain number of times
(in this case, 10).
Be careful of infinite loops (i--)
For (int i = 0; i < 10; i++;)
{
code that will repeat;
}
FTC Programming
Loops (cont.)
While statements are used when you dont know how many times
the code will run.
While (true)
{
do stuffffffffs;
}
Do While statements always run at least once, and then follow the
while loop.
Do
{
more stuffffffssss;
}
While(true)
FTC Programming
Comments
Two ways to comment code:
Comment your code, next year when you read the code youll know what you
did and why.
// Move motor C forward with 100% power
task main()
{
int motorspeed;
/* Motor C forward with 100% power
Do this for 3 seconds */
motorspeed = 100;
motor[motorC] = motorspeed;
wait1Msec(3000);
}
FTC Programming
FTC Programming
sFormatString Example
Read raw and normal light sensor data on the NXT
If you want to print Raw: 333 Normal: 96
sFormatString will look like
Raw: %d Normal: %d
Param1 and Param2 are variables, in this case the raw and
normalized light sensor data. Generally, these are constructed
in the beginning of the code with names like rawLightData and
normalLightData.
Line 0 NXT status line (do not touch) Battery Status, Bluetooth enabled, etc
Line 1 FCS Mode (Waiting, Auto, Teleop)
Line 2 BLANK
Line 3 External Battery Voltage (Textrix 12 Volt battery)
Line 4 NXT Battery Voltage
Line 5 FMS Msgs count
Line 6 "Teleop FileName:"
Line 7 NXT Teleop file name
Items in bold are refreshed every 200ms. DO NOT OVERWRITE THESE LINES!
FTC Programming
FTC Programming
Exercise
Using the example on the previous slide,
compile and run the code on the virtual world
robot.
FTC Programming
Joystick Controller
FTC Programming
FTC Programming
Tank Drive
Forward
Right
FTCBackward
Programming
task main()
{
while (true) {
tank(joystick.joy1_y1, joystick.joy1_y2);
}
}
powY = y;
if (x < 0) // if x negative, turning left; otherwise, turning right
{
powLeftMotor = (powY * (128 + (2 * x))/128); // left motor reduced for right turn
powRightMotor = powY;
// right motor not changed
}
else
{
powRightMotor = (powY * (128 - (2 * x))/128); // right motor reduced for left turn
powLeftMotor = powY;
// left motor not changed
}
motor[motorC] = powLeftMotor;
motor[motorB] = powRightMotor;
}
task main()
{
while (true) {
arcade(joystick.joy1_x1, joystick.joy1_y1);
}
}
FTC Programming
A logarithmic scale to get fine grain control at lower speeds and quickly
scale the power up at the end of the range.
motorValue = (joystickValue^2 / Max joystickValue^2) * max motorOutput
FTC Programming
Motor Control
100
90
Motor Output
80
70
60
Simple
Logarithmic
50
40
30
20
10
0
1
13
25
37
49
61
73
85
Joystick Value
FTC Programming
97 109 121
FTC Programming
Exercise
Move your Robot
Write code to move the 12V tetrix motors with your
joystick.
Compile and download it to your NXT
Run the code and move your joystick to see the motors
turn.
FTC Programming
What is a Servo?
There are two types of servos:
Standard Servos and
Continuous Rotation Servos
(almost like a super low
powered motor).
An example of when to use a
standard servo would be the
balance bridges from the FTC
2010 game. Many teams had a
metal arm that would lower the
bridge so that the robot could
cross it. (Think moving in arcs)
FTC Programming
Servos
servoChangeRate[servo#] = changeRate;
Specifies the rate at which an individual servo value is changed. A value of zero inidcates servo
will move at maximum speed. The change rate is a useful variable for "smoothing" the movement
of the servos and preventing jerky motion from software calculated rapid and wide changes in the
servo value. The default value is a change rate of 10 positions on every servo update which
occurs. (updates occur every 20 milliseconds)
FTC Programming
Servos Sample
#include "JoystickDriver.c"
task main()
{
while(true)
{
if (joy1Btn(1))
// If Joy1-Button 1 is pressed:
{
servoTarget[1] = 255; // Turn servo clockwise
}
else if (joy1Btn(3))
// If Joy1-Button 3 is pressed:
{
servoTarget [1] = 0; // Turn servo counter clockwise
}
else
{
servoTarget [1] = 127; // Center servo
}
}
}
FTC Programming
Exercise
Move your Servo
Write code to move the servo using a joystick button
joystick.
Compile and download it to your NXT
Run the code and press the joystick to see the servo
turn.
FTC Programming
Timing
Functions to pause the program for a desired amount time:
wait1Msec(nMSec);
wait10Msec(nTenMSec);
Program execution will wait for the specified number of clock units. Units can be in either 1millisecond or 10-millisecond counts. The maximum interval that can be specified is either 32.767
seconds or 327.67 seconds depending on which function is used.
There are four timers (T1, T2, T3 and T4) the user can program. These four timers can be individually
be reset to zero within a program. Theses timers are useful for measuring elapsed time of events.
ClearTimer(theTimer);
Timers start counting as soon as the NXT is powered on. A user's program should reset a timer
before using it, so use this function to reset the value of the specified timer to zero.
time1[theTimer], time10[theTimer], time100[theTimer]
These three arrays hold the current value of the respective timers. Each of the timer values can be
retrieved in units of 1, 10 and 100 milliseconds depending on which array is used. For example,
time1[T1] retrieves the value of timer T1 in units of 1-msec and time10[T1] retrieves the value
using a 10-msec tick.
FTC Programming
Timing Sample
#include "JoystickDriver.c"
task main()
{
ClearTimer(T1);
// Resets Timer T1 to 0
while(time1[T1] < 5000) // Loop for 5 seconds
{
// do something in loop
wait1Msec(500); // Wait second
}
}
FTC Programming
Types of Sensors
Light Sensor
Detects amount of light (grayscale); two modes: with flashlight
and without
Touch Sensor
Detects if the sensor hit something
Ultrasonic/Sonar Sensor
Detects how far an object is from the sensor
Gyro
Detects angle based off initialized 0
Compass
Detects True North
FTC Programming
Sensors
SensorType[]
The SensorType array is used to specify what type of sensor is connected
to a certain port. Most users should not have to use this functionality and
should use the Motors and Sensor Setup instead.
Example: SensorType[sonarSensor] = sensorSonar;
SensorRaw[]
This array value will return the "raw" (un-normalized) value of a sensor.
Usually this is the raw A-D converted value, which is an analog value
between 0 to 1023.
SensorValue[]
This array value returns the value of the sensor in a normalized fashion.
Rather than returning a raw value of 0 to 1023, ROBOTC will interpret the
data from the "SensorType" and return a more accurate representation of
the sensor's data. An example of this is the Light Sensor, which will return a
percentage value from 0 to 100.
FTC Programming
Sensor Sample
#include "JoystickDriver.c"
task main()
{
wait1Msec(50);
//leftMotor is stopped
//rightMotor is stopped
FTC Programming
Exercise
FTC Programming
FTC Programming
FTC Programming
Encoder Sample
#include "JoystickDriver.c"
task main()
{
int rotations = 1440.0 * 20.0 / 12.6;
nMotorEncoder[motorB] = 0;
// Reset the Motor Encoder of Motor B.
nMotorEncoderTarget[motorB] = rotations; // Set the target to 5 rotations.
motor[motorB] = 75;
motor[motorC] = 75;
FTC Programming
after
FTC Programming
Example :
moveForward(int num_inches)
{
int rotations_ticks;
float wheel_diameter;
wheel_diameter = 4.0;
// 1440.0 is the number of ticks per wheel rotation, if wheels are geared verify this value
// (num_inches / wheel perimeter), calculates the number of wheel rotations
rotation_ticks = 1440.0 * num_inches / (wheel_diameter * 3.14159);
nMotorEncoder[motorB] = 0;
wait1Msec(10);
motor[motorB] = 75;
// Motor B is run at a power level of 75.
motor[motorC] = 75;
// Motor C is run at a power level of 75.
while (nMotorEncoder[motorB] < rotations_ticks) // wait for motor to reach a specific # of ticks
{
wait1Msec(50);
}
motor[motorB] = 0; // Motor B and C are stopped at a power level of 0.
motor[motorC] = 0;
}
task main()
{
moveForward(30); // call function
}
FTC Programming
Functions
Good set of functions to write for your robot:
FTC Programming
Exercise
Putting it all together
FTC Programming
Classes: http://www.robotc.net/education/training/nxt/
Webinars: http://www.robotc.net/education/webinars/
Older videos & tutorials: http://www.education.rec.ri.cmu.edu/content/events/ftc/robotc/index.htm
Tetrix for FTC
http://www.tetrixrobotics.com/ftc/
FTC Programming
FTC Programming
Acknowledgments
1.
2.
3.
4.
FTC Programming
Questions
FTC Programming