Post on 01-Sep-2018
Introduction to MechatronicsDiscover robots with the Lego Mindstorms
Loïc Cuvillon, Bernard Bayle
Télécom Physique StrasbourgStrasbourg University
Introduction to Mechatronics 1 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Part I
Introduction to Mobile Robotics
Introduction to Mechatronics 2 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 3 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 4 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 5 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Mobile robots classification
Mobile robotsRobots with a moving base, by opposition with robotic manipulators.Classification based on the locomotion type.
Wheeled robots
Introduction to Mechatronics 6 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Mobile robots classification
Mobile robotsRobots with a moving base, by opposition with robotic manipulators.Classification based on the locomotion type.
Legged robots
Introduction to Mechatronics 6 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Mobile robots classification
Mobile robotsRobots with a moving base, by opposition with robotic manipulators.Classification based on the locomotion type.
Legged robots
Introduction to Mechatronics 6 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Mobile robots classification
Wheeled robots, legged robots+
flying robots, undersea robots.
Introduction to Mechatronics 7 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Wheeled mobile robots applications
A few industrial applications
Automatic Guided Vehicles (AGV)FMC Technologieshttp://www.fmcsgvs.com
Introduction to Mechatronics 8 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Wheeled mobile robots applications
More and more general public applications
ToysTribot, WowWee
http://www.wowwee.com
Automatic vacuum cleanersDirtyDog, iRobot
http://www.irobot.com
Introduction to Mechatronics 8 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Wheeled mobile robots applications
Education and research
Khepera II , K-teamhttp://www.k-team.com
1 h autonomy, 1 m/s max7x3 cm, 80g (payload<250g) Multi-robot navigation
SRIhttp://www.ai.sri.com/centibots/index.html
Introduction to Mechatronics 8 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Wheeled mobile robots applications
A few High-Tech applications
Spatial exploration robotsSojourner mission to Mars
http://mars.jpl.nasa.gov/MPF/rover/sojourner.html
Introduction to Mechatronics 8 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Wheeled mobile robots applications
A few High-Tech applications
Inspectionhttp://crasar.csee.usf.edu
Introduction to Mechatronics 8 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Issues
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 9 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Issues
Issues
TechnologyIssues:
mechanical architectureembedded systemactuatorssensors
NavigationIssues:
localizationmappingplanningcontrol
Introduction to Mechatronics 10 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 11 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Mechanical architecture
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 12 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Mechanical architecture
Mobile robots mechanical architecture
A large amount of systemsseveral types of wheelsarticulated platformscaution: all the architectures arenot kinematically consistent
Lego Tribot: a simple differential drivemobile robot
simple to build: 2 fixed wheels,with 2 independent actuatorsdifferential drive locomotionnonholonomic, nonlinear. . . not sosimple
Introduction to Mechatronics 13 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Mechanical architecture
Mobile robots mechanical architecture
A large amount of systemsseveral types of wheelsarticulated platformscaution: all the architectures arenot kinematically consistent
Lego Tribot: a simple differential drivemobile robot
simple to build: 2 fixed wheels,with 2 independent actuatorsdifferential drive locomotionnonholonomic, nonlinear. . . not sosimple
Introduction to Mechatronics 13 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Mechanical architecture
Lego TriBot modelling
O
y
xx
y
CIR
O′
L
ρ
ω
θ
v
ϕr
ϕl
vr
vl
Wheels velocities (!anglesconvention) :
vr = −r ϕr = (ρ+ L)ωvl = r ϕl = (ρ− L)ω
and then:
v =vr + vl2
ω =−r(ϕr + ϕl)
2LDifferential kinematics:
x = v cos θy = v sin θθ = ω
Introduction to Mechatronics 14 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Embedded system
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 15 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Embedded system
Lego TriBot embedded system
NXT BlockThe core of the robot, which enables to get measurements and to control theactuators.
Refer to the first part of the talk for more details. . .
NXT InputsNXT Outputs
Sound Sensor
Touch Sensor
Light Sensor
Servo Motors
Ultrasonic Sensor
with position encoders
(contact switch)
(pressure in dB) (distance in cm)
(light intensity in %)
Buttons
LCD screen
Speaker
with BluetoothNXT processor
Introduction to Mechatronics 16 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Actuators
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 17 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Actuators
LEGO Mindstorms Servo-motor
Encoder wheel andwith optical sensor
Gear train (1:48)
Running_modesON: power control
open loop controlreference: % of the max power
ON & REGULATED:speed regulationuse of position sensor signalsclosed loop with PID controller
ON & BRAKE: electronic brakingOFF: coasting (free motion)
Introduction to Mechatronics 18 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Sensors
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 19 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Sensors
Ultrasonic Telemeters
http://www.pages.drexel.edu/ kws23/tutorials/ultrasonic/ultrasonic.html
Properties:alternate emission/receptionsounds not heard by humanear (ultrasound=20kHz to200kHz)minimal measurementdistance and variablemaximal frequency=f(maximaldistance)different drawbacks(directivity, reflectivity,ambient conditions sensitivity)
Introduction to Mechatronics 20 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Sensors
Lego TriBot Ultrasonic Telemeter
Introduction to Mechatronics 21 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 22 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Localization
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 23 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Localization
Dead reckoning
OdometryComputation of the configuration by velocities integration:
x(t) =! t
0x(τ )dτ, y(t) =
! t
0y(τ )dτ, θ(t) =
! t
0θ(τ )dτ.
The TriBot example, with a Te sampling period:
x(k + 1) = x(k) + v(k)Te cos θ(k)y(k + 1) = y(k) + v(k)Te sin θ(k)θ(k + 1) = θ(k) + ω(k)Te
RemarkOdometry: current configuration relative to the initial configuration.
Introduction to Mechatronics 24 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Mapping
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 25 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Mapping
Environment mapping
Occupancy grid: discrete map of the environmentCell decomposition, with probability of collision from sensors measurements.The simplest case: binary grid, without filtering, trusting the odometrylocalization.
Introduction to Mechatronics 26 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 27 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Configuration space
DefinitionConfiguration space = set of all the accessible configurations of the robot:
robot represented as a point: obstacles augmentationrobot with complex shapes and kinematics: more difficult problem. Overpessimistic obstacle augmentation can close doors !
Introduction to Mechatronics 28 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Roadmaps
Visibility roadmapGraph capturing the environment topology from the polygonal obstaclesvertices.
link initial point and goal to visible vertices
Introduction to Mechatronics 29 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Roadmaps
Visibility roadmapGraph capturing the environment topology from the polygonal obstaclesvertices.
apply the same to the new vertices
Introduction to Mechatronics 29 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Roadmaps
Visibility roadmapGraph capturing the environment topology from the polygonal obstaclesvertices.
search for a path in the graph
Introduction to Mechatronics 29 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Control
Outline
1 Mobile robotsClassification and applicationsIssues
2 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
3 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 30 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Control
Path following problem
ProblemFind ω such that d(P, C) decreases to zero, with v imposed.
x′
y ′
Ox
O′
C
θr
Or
xr
yry
Pd
a
θeIt can be showna that:
s =v cos θe − aω sin θe
1− dc(s) ,
d = v sin θe + aω cos θe,θe = ω − sc(s).
a|d c(s)|<1 under some adequate initial conditions of the
Introduction to Mechatronics 31 / 98
Mobile robots Wheeled Mobile Robots Technology Navigation
Control
Path following control law
Distance equation:d = v sin θe + aω cos θe
It comes that:ω = −
v sin θea cos θe
−v
cos θek(d , θe)d ,
with k(d , θe) ! 0 such that k(d , ±π
2 ) = 0 gives:
d = −vak(d , θe)d .
ConsequenceIf a, v et k(d , θe) > 0: |d | decreases along any path.
RestrictionsNo control of the orientation.
Can be solved at the planning step.
Introduction to Mechatronics 32 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Part II
Hardware and Programming under Linux
Introduction to Mechatronics 33 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 34 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 35 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
References
NXC Programming
[NXC07] J.C. Hansen, NXT Power Programming, Variant Press, 2007
[NXC] J.C Hansen, Not eXactly C Programmer’s Guide,http://bricxcc.sourceforge.net/nbc/
[NXCT] D. Benedettelli. , Programming LEGO NXT Robots using NXC,http://bricxcc.sourceforge.net/nbc/nxcdoc/NXC_tutorial.pdf
NXT Bluetooth and USB communication
[BLUE] J. Schultz, Lego, Bluetooth and Linux, http://www.cs.uleth.ca/benkoczi/3720/data/NXT_Bluetooth_handout-jeremy.pdf
[NXTLIBC] Lego Mindstorms NXT Bluetooth library in C,http://www.quietearth.us/nxtlibc.htm
Introduction to Mechatronics 36 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 37 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
Why the Lego Mindstorms?
fun (look back on childhood)cheap (for the content)and not the least: highly valuable for education
computer sciencescontrol systems theoryrobotics
Introduction to Mechatronics 38 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
Lego Mindstorms Overview
NXT InputsNXT Outputs
Sound Sensor
Touch Sensor
Light Sensor
Servo Motors
Ultrasonic Sensor
with position encoders
(contact switch)
(pressure in dB) (distance in cm)
(light intensity in %)
Buttons
LCD screen
Speaker
with BluetoothNXT processor
Introduction to Mechatronics 39 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
NXT brick
Battery levelNXT brick name(configurable)
USB 2.0 connector
B : ON, invisible B< : ON, visibleB<> : ON, connected
usb : connected, working]−[ : connected, not working
On/Enter button
Clear/Go back buttonNavigation buttons
S4S1 S2 S3Sensor port
OUT_A OUT_B OUT_CServo outputs
Loudspeaker
USB indicator
Bluetooth indicator
Introduction to Mechatronics 40 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
NXT brick Menus
On/off
Connections
Visability
Searchto bluetooth device)(search and connect
Motor rotation
Sound dBA
Ultrasonic cm
Sound files
Software File(bytecode executableupload from computer)
(LCD display distance from sensor)
Delete Files
Volume
NXT version(firmware version)
NXT ProgramSettingsBluetoothMy files View (testing)
NXT Program Example :
= "Forward until an object detection, then turn left. Repeat"
NXT files
savedin
Introduction to Mechatronics 41 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
NXT-G (LabView)
advance graphical interface for programming NXTfunctional block and control flow
Introduction to Mechatronics 42 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
Some fun NXT Projects
Rubik’s Cube Solverby Daniele Benedettelli
NXTway-GS ENSPS (model by Y.Yamamoto)
Introduction to Mechatronics 43 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 44 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
Hardware specification
S1 S2 S3 S4
−single and double H−bridge(OUT_A and OUT_BC)
−48MHz, 64KB RAM−256KB FLASH−I2C interface with AVR proc.and digital sensor (ultrasonic)
−class 2−10 meters range
−high speed communication lineon Port S4 (for future use)
−A/D converter for analogic sensor(light,sound,touch )
−PWM signal generator
LB1930 & LB1836
AT91SAM7S256Amtel 32 bit ARM Processor
Bluetooth Chip
USB 2.0 connector
8bit AVR Co−Processor
RS485 UART
Introduction to Mechatronics 45 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
LEGO Mindstorms Servo-motor
Encoder wheel andwith optical sensor
Gear train (1:48)
Running_modesON: power control
open loop controlreference: % of the max power
ON & REGULATED:speed regulationuse of position sensor signalsclosed loop with PID controller
BRAKE: electronic brakingOFF: coasting (free motion)
Introduction to Mechatronics 46 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
LEGO Mindstorms Servo-motor driving
O V
9 V
90% duty cycle
O V
9 V10% duty cycle
Power controlPWM (Pulse Width Modulation) ofa 8 kHz signal
90% duty cycle ≈ 8.1 V20% duty cycle ≈ 1.8 V
+Vcc
0V
In2
In2In1
M
In1
+−
In1=High, In2=Low In1=Low, In2=High
+Vcc
0V
In1 In2
In2In1
M +−
Direction and brakingH-bridge with DC motorsIN1 IN2 ActionH L ForwardL H ReverseL L Brake
Introduction to Mechatronics 47 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
Electronic braking
e
i
U
R
Lτ = Ki
ω = Ke
e : back e.m.fτ : motor torqueω : motor angular speed
Braking1 motor motion: e = 02 short circuit current i ≃ − e
R
→ torque τ in opposition to themotion
Introduction to Mechatronics 48 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
Hardware specification
I2C: Inter-IntegratedCommunicationuse 3 lines (data,clock,ground)UART: Universal AsynchronousReceiver Transmitter(serial line/port)
Note:each sensor port (S1-4) can beused with digital or analog sensor.6 wires by port: 3 for I2C + 3 foranalog signals
Introduction to Mechatronics 49 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
NXT Firmware
Processor
Boot loader/boot code
LEGO firmwarewith CodeByte interpreter
Executables (.rxe)
HARD
SOFT
Boot code/loaderinitialization of the hardwareload the firmware
Firmware : a basic operating systemmulti-thread (thread: light task)provides an API for I/O(displays the menus)interprets ByteCode executables(.rxe)
firmware and files stored inFLASH memory
Introduction to Mechatronics 50 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 51 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Compilation and download to the NXT
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 52 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Compilation and download to the NXT
NBC/NXC
NBC: Next Byte Codesname of an assembly languagename of an open source assembler available under MPL licenseproduces executable bytecode for the native LEGO firmware
NXC: Not eXactly Chigh level programming language: C-like syntaxcompiler built upon nbc.nxc (source)→ .rxe (ByteCode executable)
Introduction to Mechatronics 53 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Compilation and download to the NXT
Compilation and Execution
Compilation under linuxnbc compiler available from [NXC]compilation of a source file *.nxc and upload to the brick:nbc -EF -d toto.nxc
Execution after upload.rxe listed in the brick Menu : Software Filesstart/stop the progam with the NXT buttons
Introduction to Mechatronics 54 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Compilation and download to the NXT
Compilation and Execution
Cross−compilation process
1−Source editing
2−Compilation (nbc)
USB cable
3−Upload of .rxe file4−Execution on the NXT(menu Program Files)
nbc −EF −d toto.nxc
toto.nxc
Introduction to Mechatronics 55 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Compilation and download to the NXT
communication tool : NeXTTool
NeXTToolrunning on PC to communicate with the NXT brickvia USB or BT (BlueTooth if configured)list of functions: NexTTool -h
Examplesto download a file via USB cable :NeXTTool /COM=usb -download=toto.rxe
to obtain info on the brick (free memory, BT address):NeXTTool /COM=usb -deviceinfo
to play a musical tone of 500Hz:NeXTTool /COM=usb -playtone=500
Introduction to Mechatronics 56 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 57 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Not Exactly C
C like syntax butNo pointers !
new types: Arrays and stringno parameter passed by address
Multi-tasking : more than one program (task) on executiontime-sharing operating system
Introduction to Mechatronics 58 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Types
bool, byte, unsigned char, char 8 bitunsigned int, short, int 16 bitunsigned long, long 32bit
float 32 bits (in recent version of NXC)string Array of charactersstruct User defineArrays Array of any type
Type conversionsno explicit conversion - no cast operator (type).please remind that an operation on integers give an integers :float y;y=3/10; differs from y=3.0/10// -> y = 0.0 // -> y=0.3
be also careful with order of calculus for int:int y; int x=7;y=x*3/10 differs from y=x/10*3// -> y=2 // -> y=0
Introduction to Mechatronics 59 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Arraysno pointers→ no dynamic allocation with malloc
byte myvector[2]; //2 byte array
int myarray[]; // int array (no dimension)
ArrayInit(myarray,0,10);// allocation of 10 null entries (equiv. to malloc)
myarray[0]= 15;
Introduction to Mechatronics 60 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
String type
Stringarray of bytesdeclaration:
string msg1, test=’’toto’’;msg1="yes";
manipulation functions:msg=NumToStr(i);
//convert number in stringmsg=StrCat(str1,str2);
//concatenationn=StrToNum(str);
//convert string to numbery=StrLen(str);
//return length of the string
Introduction to Mechatronics 61 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Program structure: Functions
parameter passed by valuereturn for non-void function
int foo (int x){ x++;
return x;}
task main{
int x,y=1;x=foo(y);
//y still equal to 1}
parameter passed by reference
void foo (int &x) // !!{x++;}
task main{int y=1;foo(y);
//y now equal to 2foo(2);
//ERROR (not a variable)}
Introduction to Mechatronics 62 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Control structures
Test Expression
= equal!= not equaltrue Truefalse False
Expr True if Expr!=0&& logical AND between conditions|| logical OR between conditions
if, if . . . elseif (x==1){x=2; z=3;}
elsey=4;
forfor (i=1; i<10; i++)
{;}
whilewhile (x<10){x++ ; // something needed !!}
do.. whiledo
{ x++ ;}while (x<10)
Introduction to Mechatronics 63 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Control structures
switchswitch (x){
case 1 :z=3;break;
case 2 :// some codebreak;
default:// some codebreak;
}
repeat and until control structure available but not C standard
Introduction to Mechatronics 64 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Program structure: Main Task
source.nxc :
long time; //global var.
task main (){
int x;...
}
Task main ()entry point of the programequivalent to the C function main()no parameter, no return value
Introduction to Mechatronics 65 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Program structure: Periodic Task
source.nxc :
long time; //global var.
task main (){
while(1){
...Wait(500);//wait, sleep 500 ms
}}
Periodic Taskan infinite Taska timing function to set a period
Introduction to Mechatronics 66 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Program structure: Tasks and Multi-tasking
task odometrie(){ while(true)
{...Wait(500);
}}
task main (){
start odometrie;
while(true){ OnFwd(OUT_A,50);
if (x==1)stop odometrie;
...Wait(1000);
}}
multithreading in NXT=concurrent execution of tasksin time sharingat least one entry task called”main”
APIstart task1 or StartTask(task1) ;stop task2; and StopAllTasks();precedes(task1,task2,..);launch the task after end of main
Introduction to Mechatronics 67 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Program structure: Tasks and Multi-threading
Time sharing of the processor for periodic tasks (with sleep)
t
t
t
0 50 100 150
+
Task main
Task odometrie
=global cpu usage
(Equitable) Time sharing for non-periodic tasks
0 50 100 150
t
Introduction to Mechatronics 68 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC syntax and multi-tasking
Preprocessor
Preprocessor directiveinclude files#include ’’foo.h’’#include <foo.h> //error no libray path
macro and define (recommended!)#define TIMEOUT 200 //0.2 ms timeout#define NORMAL_SPEED 50
Introduction to Mechatronics 69 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 70 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
API functions
TimeWait(2000);// sleep for 2s
x=CurrentTick();// system time in ms (since the power on)
Math and Trigonometry Functions-abs(), sqrt()-cos(), sin(), acos() in radiansand return a float
x=cos(3.14159); //returns -1
SoundPlayTone (440,500);
// frequency=440 Hz, duration 500 ms
Introduction to Mechatronics 71 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
LCD Screen API
Screen Resolution100x64 pixels8 lines : LCD_LINE1,...LCD_LINE8char size : 6x8 pixeldefault mode : overwriting without clearing of thescreen
Screen functions:with X and Y, the column in pixel and Y the line(LCD_LINEx)
Display a string variable:TextOut(X,Y,string);
display a numeric variable:NumOut(X,Y,var);
clear the screen:ClearScreen();
Introduction to Mechatronics 72 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
LCD Screen API
task main (){int i=1;TextOut(0,LCD_LINE1,"toto:");
// display toto on line 1NumOut(20,LCD_LINE1,i);
//display i on line 1 and column 20Wait(3000);ClearScreen();LineOut(40,40,60,60);
//plot a line from (40,40) to (60,50) coordinatesWait(3000);
}
Introduction to Mechatronics 73 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
Parameterspower in percent: [-100,+100] (negative = reverse)port name to select motor: OUT_A, OUT_B, OUT_Cor multiple selection : OUT_AB, OUT_AC, OUT_BC,OUT_ABC
Motor functions:power control: OnFwd(port,power);
speedcontrol: OnFwdReg(port,power,mode);
2 synchronized motor control:OnFwdSync(OUT_AC,power,sterring)
rotation control:RotateMotor(port,power,angle);
brake: Off(port)
stop without braking: Float(port)
Introduction to Mechatronics 74 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
OnFwd : Unregulated power controlno regulation (speed can vary according to the load)reference = power in % , output selection= OUT_A, OUT_BC, OUT_ABC. . .
task main(){OnFwd(OUT_AC,85); //motor AC fwd at 85% of powerWait(1500); // run for 1.5 sOnRev(OUT_A,90); //motor A reverse at 90%,
//C still going fwd at 85%Float(OUT_C); //motot C power off, free decelerationWait(1500);Off(OUT_ABC); //motor ABC power off with braking
}
Introduction to Mechatronics 75 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
OnFwdReg : Speed Regulated modePID regulation of the motor speed (power will increase if load increases)reference = speed in % of maximal speed
task motor_status(){while(1)NUMOUT(0,LCD_LINE1,MotorActualSpeed(OUT_A));
//diplay not speed but power!!}
task main() {start motor_status;
OnFwdReg(OUT_A, 40, OUT_REGMODE_IDLE);//same as OnFwd, no regulation
Wait(5000);OnFwdReg(OUT_A,40,OUT_REGMODE_SPEED); //speed regulationWait(5000);Off(OUT_A);StopAllTasks();
}Introduction to Mechatronics 76 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
OnFwdSync: Synchronized modeonly for a pair of motors (OUT_xy)synchronized the rotation (useful to go in straight line for mobile robot)if one motor slow down, the other adaptslast parameter: steering or turn percentage
task main() {OnFwdSync(OUT_AC, 50, 0); //Motors A and C same speedWait(2000);OnFwdSync(OUT_AC, 40 , 50); //motor A stopWait(2000);OnFwdSync(OUT_AC, 40 , 100);
//motor A speed opposite of CWait(2000);Off(OUT_AC);
}
Introduction to Mechatronics 77 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
RotateMotor: Position-based moderegulation of rotation position in degree
#define degree 180
task main() {RotateMotor(OUT_AC,56, degree);
//rotation of 180 degree at 56% of power}
Introduction to Mechatronics 78 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Input API : Sensors
Configuration of the sensors (input ports)1 a port name: S1, S2, S3 or S4;2 a TYPE (TOUCH,SOUND. . . ) with SetSensorType(port,TYPE)3 a MODE (RAW, BOOL. . . ) with SetSensorMode(port,MODE)
TYPE DescriptionSENSOR_TYPE_TOUCH touch sensor
SENSOR_TYPE_LIGHT_ACTIVE light sensor with LED onSENSOR_TYPE_LIGHT_INACTIVE light sensor with LED offSENSOR_TYPE_SOUND_DB sound pressure in dBSENSOR_TYPE_LOWSPEED I2C sensor (Ultrasonic)
MODE DescriptionSENSOR_MODE_RAW value between 0 < x < 1024SENSOR_TYPE_BOOL 0 if x<512, else 1
SENSOR_TYPE_PERCENT value between 0 < x < 100SENSOR_TYPE_EDGE count number of transitionSENSOR_TYPE_TOUCH count number of level change
Introduction to Mechatronics 79 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Input API : Analog Sensors
Fast initialisation of Analog Sensorsport where sensor is connected : S1, S2, S3, S4.touch sensor (mode bool):
SetSensorTouch(port);
light sensor (mode percent):SetSensorLight(port);
sound sensor (mode percent):SetSensorSound(port);
Read analog sensors:function SensorValue(port)
var1=SensorValue(S1);
or the macros SENSOR_1, SENSOR_2, SENSOR_3and SENSOR_4:
var1=SENSOR_1;
Introduction to Mechatronics 80 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Input API : Numeric Sensors
Initialisation of Numeric Sensorsfor the sonar sensorset numeric communication (i2c) on the port:
SetSensorLowspeed(port);
Read numeric sensors:specific function for each sensorsonar sensor (value in centimeters):
dist=SensorUS(port);
Rotation count of motors:motor selected by port: OUT_A, OUT_B, OUT_Cread the motor position in degree:long position=MotorRotationCount(port);
reset the motor position (and stop motor):ResetRotationCount(port);
Introduction to Mechatronics 81 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Input API : Sensors and rotation
task main(){
bool cont; int dist;
SetSensorTouch(S1);SetSensorLowspeed(S2);ResetRotationCount(OUT_A);
while(1){
cont=SENSOR_1; // or cont=SensorValue(S1);dist=SensorUS(S2);if (cont==1){ TextOut(0,LCD_LINE1,"Pressed"); PlayTone(440,500); }
else TextOut(0,LCD_LINE1,"Released");
if (MotorRotationCount(OUT_A) > 180)TextOut(0,LCD_LINE2,"Half-rotation!"); }
}
Introduction to Mechatronics 82 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
One example : "Move forward but avoid obstacles"
Moteur A et B en avant, 80% de la vitesse
{OnFwd(OUT_AB, 80);
{ Off(OUT_AB); RotateMotor(OUT_A, 80, 360); }}
Si distance (par ultrason) < 15 cm
Rotation Moteur A, vitesse 80%, 360
Arrêt moteur A et B
Un pied
if ( SensorUS(4) < 15 )
while (true)
Introduction to Mechatronics 83 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
One example : "Move forward but avoid obstacles"
Implementation 1
#define NEAR 15 //cm
task main(){SetSensorLowspeed(S4);
while(true){OnFwd(OUT_AC,50);
while(SensorUS(S4)>NEAR);//do nothing: waitOff(OUT_AC);OnRev(OUT_C,40); //turn
//angle linked to wait timeWait(800);}
}
Implementation 2
#define NEAR 15 //cm
task main(){SetSensorLowspeed(S4);
while(true){
OnFwd(OUT_AC,50);
if (SensorUS(S4)<NEAR){ Off(OUT_AC);
OnRev(OUT_C,40);Wait(800); }
}}
Introduction to Mechatronics 84 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
One example : third implementation
#define NEAR 15 //cm
mutex motor_mutex;
task obstacle_test(){ while(1)
{if (SensorUS(S4)<NEAR){Acquire(motor_mutex);Off(OUT_AC);OnRev(OUT_C,40);Wait(800);Release(motor_mutex);}
}}
task move(){
while(true){ Acquire(motor_mutex);OnFwd(OUT_AC,50);Release(motor_mutex); }}
task main(){SetSensorLowspeed(S4);start obstacle_test; start move; }
Implementation 3: Multiple tasksrunning in concurrency (problem)mutex: mutual exclusion =prevent the 2 tasks to controlmotors simultaneouslya task block on Acquire(), ifanother has already Acquire themutex→ must wait for theRelease
Introduction to Mechatronics 85 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 86 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth Protocol
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 87 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth Protocol
Bluetooth
wireless protocolfor electronics (phone headset), computer peripherals (WiiMote,keyboard)2.4 GHz short-range radio frequency bandwidth : unlicensed frequency
Classclass 1: 100 mW ≈ 100metersclass 2: 2.5 mW ≈ 10 meters (LEGO NXT)
Search and Pairingsearch service available to find new Bluetooth devicepairing: trust communication by learning of a common passkey-input of the passkey on both device-key used to encrypt communication
Introduction to Mechatronics 88 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 89 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
Pairing between PC and NXT -1
Get NXT Bluetooth addresseach Bluetooth as an unique address on 6 bytes (MAC address)
1 command hcitool : give name and Bluetooth address of available device>hcitool scanScanning ...
00:16:53:04:B3:46 NXT2 NeXTTool, while NXT connected by USB>NeXTTool /COM=usb -deviceinfoBrick name = NXT12Bluetooth Address = 00:16:53:0B:2E:D4Bluetooth signal strength = 0,0,0,0Free memory = 31716
Introduction to Mechatronics 90 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
Pairing between PC and NXT -2
Pairing under ubuntu Linux 9.101 On Linux: right click on the Bluetooth-applet > set up new device2 select NXT and in PIN option select custom PIN ’1234’(but a new one can be set)
3 click Forward, Linux will initiate a connection to the NXT4 On NXT: a pop-up ask for the passkey, validate the default ’1234’ (orgive the new one)
5 it is a one time operation, NXT now appears in Bluetooth-applet >preferences > known devices
Introduction to Mechatronics 91 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
NXTlibC
A simple library to perform Direct CommandRun on the PC to interact with the NXT brick via Bluetoothcompilation:gcc -lm -lbluetooth -lnxtlibc test.c -o test
API of NXTlibC : samplesnxt_bluetooth_initialize() , nxt_bluetooth_done()get_battery_level()play_tone()...message_write() and message_read() (see next section)
Introduction to Mechatronics 92 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
play_tone.c with NXTlibC
#include <stdio.h>#include <nxtlibc.h>
#define MAC "00:16:53:02:f3:31"
int main (void){int ret;// * * * * * Connection Ãa la brique * * * * * //nxt_bluetooth_initialize(MAC);
play_tone(500, 2000);
// * * * * * Verification de la batterie * * * * * //ret = get_battery_level();
nxt_bluetooth_done();
return 0; }
Introduction to Mechatronics 93 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
NXTlibC
Message ExchangePC = masterNXT = slave20 mailboxes available on the NXT
mailboxes 0-9 reserved for Master inputmailboxes 10-19 reserved for NXT output
one mailbox can store up to 5 messages of 64 bytes each? behaviour if mailbox full
Introduction to Mechatronics 94 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
NXTlibC
Message Exchange1 PC writes its message in mailbox A (with nxtlibc functions)2 NXT check the mailbox A (with nxc functions)3 If needed, NXT writes its response in mailbox B4 PC checks the answer in mailbox B
message_write(3,size,message)PC −NxtLibC
1−
4−
2−3−
O
10
13
19
3
9
mailboxesNXT−NXC
string buf, res;ReceiveMessage(3,true,buf)
SendMessage(10+3,res)
message_read(10+3,0,....,reponse)
Introduction to Mechatronics 95 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
message.c with NXTlibC
#include <stdio.h>#include <nxtlibc.h>#define MAC "00:16:53:02:f3:31"
int main (void){unsigned char return_inbox[1], return_size[1];unsigned char message[]="home";unsigned char *data;
// * * * * * Connection Ãa la brique * * * * * //nxt_bluetooth_initialize(MAC);
message_write(3,5, message);getchar(); //or sleep to let NXT some time to answermessage_read(13, 0, 1, return_inbox, return_size,
(unsigned char**)&data);
printf("++message: %s\n",data);
nxt_bluetooth_done();
return 0; }
Introduction to Mechatronics 96 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
message.nxc
task main() {
string buffer, message="none";char result;
while(1) {// read mailbox 3result = ReceiveMessage(3, true, buffer);if (result == 0) //if something is read{ message = "sweet";// send message back to master in mailbox 13SendMessage(3+10, message);}
Wait(500);}
}
Introduction to Mechatronics 97 / 98
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication between NXT
Outline
4 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
5 NXT Programming with NBC/NXCCompilation and download to the NXTNXC syntax and multi-taskingNXC I/O API
6 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 98 / 98