Upload
phan-van-tu
View
27
Download
3
Embed Size (px)
Citation preview
Introduction to MechatronicsDiscover robots with the Lego Mindstorms
Loïc Cuvillon, Bernard Bayle
Ecole Nationale Supérieure de Physique de StrasbourgStrasbourg University
March 13, 2009
Introduction to Mechatronics 1 / 97
You are free to copy, to modify, to adapt and distribute this document.Latex sources of this document should be available on the author’s webpage.
LEGO and Mindstorms NXT are trademarks of the LEGO Group, which doesnot sponsor, authorize or endorse this document.
Introduction to Mechatronics 2 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Part I
Hardware and Programming under Linux
Introduction to Mechatronics 3 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 4 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 5 / 97
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
[LINXT] Lego Mindstorms NXT brick in Linux via USB,http://sourceforge.net/projects/linxt/
[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 6 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
References
LEGO resources and documentation
[LEGO] Bluetooth Developer Kit and Hardware Developer Kit,http://mindstorms.lego.com/eng/Overview/NXTreme.aspx
Introduction to Mechatronics 7 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 8 / 97
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 %)
LCD screen
Buttons
NXT processorwith Bluetooth
Introduction to Mechatronics 9 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
NXT brick
battery level
NXT brick name(configurable)
USB 2.0 connectorbluetooth indicatorB : ON, invisible
B< : ON, visible
B<> : ON, connected
usb indicatorusb : connected, working
]−[ : connected, not working
loudspeaker
On/Enter button
Clear/Go back buttonNaviguation buttons
S4S1 S2 S3Sensor port
OUT_A OUT_B OUT_CServo outputs
Introduction to Mechatronics 10 / 97
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 11 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
NXT-G (LabView)
advance graphical interface for programming NXT
functional block and control flow
Introduction to Mechatronics 12 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Overview
Some fun NXT Projects
Rubik’s Cube Solverby Daniele Benedettelli
NXTway GSby Y. Yamamoto
Introduction to Mechatronics 13 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 14 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
Hardware specification
S1 S2 S3 S4
−PWM signal generator
8bit AVR Co−Processor
−A/D converter for analogic sensor
(light,sound,touch )
USB 2.0 connector
−high speed communication line
on Port S4 (for future use)
RS485 UARTBluetooth Chip−class 2−10 meters range
Amtel 32 bit ARM Processor
−48MHz, 64KB RAM
−256KB FLASH
−I2C interface with AVR proc.and digital sensor (ultrasonic)
−single and double H−bridge
(OUT_A and OUT_BC)
LB1930 & LB1836
AT91SAM7S256
Introduction to Mechatronics 15 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
Hardware specification
I2C: Inter-IntegratedCommunication need 3 lines(data,clock,ground)
UART: Universal AsynchronousReceiver Transmitter(serial line/port)
Note:
-each sensor port (S1-4) can be usedwith digital or analog sensor.-6 wires by port: 3 for I2C + 3 foranalog signals
Introduction to Mechatronics 16 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXT Brick Hard/Firm-ware
NXT Firmware
Memory
Boot Recovery(SAM−BA)
ROM
FLASH
RAM
64K
Firmware
(sound, .rxe...)
Files
Running executables
256K
Processor
Boot loader/boot code
LEGO firmwarewith CodeByte interpreter
Executables (.rxe)
Software
Boot code/loader
initialization of the hardware
load the firmware
Firmware : a basic operating system
multi-thread (thread: light task)
provides an API for I/O
(displays the menus)
interprets ByteCode executables(.rxe)
firmware and files stored inFLASH memory
BOOT CODE stored in RAMallow to update firmware
Introduction to Mechatronics 17 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 18 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NBC/NXC basics
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 19 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NBC/NXC basics
NBC/NXC
NBC: Next Byte Codes
name of an assembly language
name of an open source assembler available under MPL license
produce executable code for the native LEGO firmware
NXC: Not eXactly C
high level programming language: C-like syntax
compiler built upon nbc
produce ByteCode executables (.rxe)
NBC and NXC available for Linux, MAC OS, Windows
custom Firmwares to replace LEGO firmwares are available : LeJOS(JAVA-VM) for Java programming
Introduction to Mechatronics 20 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NBC/NXC basics
Compilation and Execution
Compilation under linux
nbc compiler available from [NXC]
options of nbc available with ”nbc -help”
compilation of a source file *.nxc
nbc toto.nxc -O toto.rxe
Run execution
toto.rxe available in Program Files folder via menus after upload
start/stop via buttons
3−Upload of .rxe file
USB cable
1−Source editing
2−Compilation
4−Execution
Cross−compilation process
Introduction to Mechatronics 21 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NBC/NXC basics
USB Upload
Detection of the NXT brick
”lsusb”: listing of connected USB device
Bus 002 Device 004: ID 0694:0002 Lego Group
via NXC
need complex configuration of linux device manager (udev)
upload command
nbc -d -S=usb toto.rxe
via LiNXT (prefered method, [LINXT])
LiNXT: Perl script with automatic search of the brick
command to upload, download and get NXT information
to get help ”linxt -h”
to upload a file (as root user):
linxt -u toto.rxe
Introduction to Mechatronics 22 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC structure and statements
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 23 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC structure and statements
Statements 1
comments:
//This is a comment/ * This is a comment * /
variablesbool, byte, unsigned char, char 8 bit
unsigned int, short, int 16 bitunsigned long, long 32bit
string Array of bytestruct User defineArrays Array of any type
declaration of variable
int i;int x=0x10; // 16 in hexadecimalint y=10; // 10string msg=’’toto’’;
Introduction to Mechatronics 24 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC structure and statements
Statements 2
assignment
x=y+2;x+=y; // equivalent to x=x+y;
arrays (no pointers)
byte myvector[2]; // 2 byte arrayint myarray[]; // int array (no dimension)ArrayInit(myarray,0,10); // alocation of 10 null entriesmyarray[0]= 15;
conditions= equal!= not equal
truefalse
Expr True if Expr!=0&& logical AND between conditions|| logical OR between conditions
Introduction to Mechatronics 25 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC structure and statements
Control structures
if, if . . . else
if (x==1){x=2; z=3;}
elsey=4;
for
for (i=1; i<10; i++){
;}
while
while (x<10){x++ ; // something needed !!}
do.. while
do{ x++ ;}
while (x<10)
Introduction to Mechatronics 26 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC structure and statements
Control structures
switch
switch (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 27 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC structure and statements
Program structure: Tasks
task music (){ PlayTone(440,3000); }
task motion (){
while(true){ OnFwd(OUT_A,50);
Wait(1000); }}
task main (){ // motion
start music; //and musicstart motion; //in parallel
} // execution
multithreading in NXT=concurrent execution of tasks
number of task and function mustbe < 256
at least an init task called ”main”needed
API
precedes(task1,task2,..);launch the task after end of main
start task1 or StartTask(task1) ;
StopAllTasks();
Introduction to Mechatronics 28 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC structure and statements
Program structure: Functions
parameter passed by value
return for non-void function
void foo (int x){x++;}
task main{
int y=1;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
(one-variable)}
Introduction to Mechatronics 29 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC structure and statements
Preprocessor
Preprocessor directive
include 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 30 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 31 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
some functions of the API
Timings
wait(2000);// sleep for 2s
x=CurrentTick();// sys time in ms
Math Functions
-Cos, Sin, Abs, Acos in degree-returns 100 times the value
x=cos(60); //returns 50
Strings
msg=numToStr(i)// convert number in string
msg=StrCat(str1,str2)// concatenation
Sound
Play tone (440,500);//440: frequency//500: duration
Introduction to Mechatronics 32 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
LCD 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 33 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
Unregulated mode
no 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 34 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
Regulated mode
PID 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 regulat ionWait(5000);Off(OUT_A);StopAllTasks();
}
Introduction to Mechatronics 35 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
Synchronized mode
only 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 adapts
last parameter: steering or turn percentage
task main() {OnFwdSync(OUT_AC, 50, 0); //same as OnFwd, no regulationWait(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 36 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Output Control API : Servo-motor
Position-based mode
regulation 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 37 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Advance Output API : Write File
Position-based mode
wite written in flash memory
return value of the API: LDR_FILEEXIST, LDR_FILEISFULL,. . .
read file API : OpenFileRead(), Read()
task main() {byte handle;int result;result=CreateFile("test_write.rxe",256,handle);
//file of 256 bytesif (result==LDR_SUCCESS)
{ int i=3;result=Write(handle,i);
}CloseFile(handle);}
Introduction to Mechatronics 38 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Input API : Sensors
Configuration of the input ports
1 a TYPE (TOUCH,SOUND. . . ) with SetSensorType(port,TYPE)2 a MODE (RAW, BOOL. . . ) with SetSensorMode(port,MODE)
TYPE DescriptionSENSOR_TYPE_TOUCH
SENSOR_TYPE_LIGHT_ACTIVE light sensor with LED onSENSOR_TYPE_LIGHT_INACTIVE light sensor with LED off
SENSOR_TYPE_SOUND_DB sound pressure in dBSENSOR_TYPE_TOUCH 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 transition
SENSOR_TYPE_TOUCH count number of level change
Introduction to Mechatronics 39 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Input API : Sensors
Default configuration of the input ports
high-level functions with the default mode : SetSensorXXXX(port)
task main() {SetSensorType(S1,SENSOR_TYPE_TOUCH);SetSensorMode(S1,SENSOR_MODE_BOOL)SetSensorTouch(S1); //idem than the 2 previous line
SetSensorSound(S3);//Sound sensor on port 3, default pourcent?
SetSensorLight(S4); // port 4, default mode : percentSetSensorLowSpeed(S2);
// I2C sensor (Ultrasonic,..) on port 2}
Introduction to Mechatronics 40 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
Input API : Sensors and rotation
Reading
analog sensor: SensorValue(SX) or the equivalent macro SENSOR_X
for I2C ultrasonic sensor: SensorUS(port)
rotation count on servo_motor: MotorRotationCount(OUT_X)
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");else TextOut(0,LCD_LINE1,"Release");if (MotorRotationCount(OUT_A) > 180)
TextOut(0,LCD_LINE2,"Half-rotation!"); } }
Introduction to Mechatronics 41 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
One example : 3 implementation
Move forward but avoid obstacles
#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);
}
}
#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 42 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
NXC I/O API
One example : 3 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; }
Multiple tasks
running in concurrency
mutex: mutual exclusion =preventthe 2 tasks to control motorssimultaneously
a task block on Acquire(), ifanother has already Acquire themutex → must wait for theRelease
Introduction to Mechatronics 43 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 44 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth Protocol
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 45 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth Protocol
Bluetooth
wireless protocol
for electronics (phone headset), computer peripherals (WiiMote,keyboard)
2.4 GHz short-range radio frequency bandwidth : unlicensed frequency
Class
class 1: 100 mW ≈ 100meters
class 2: 2.5 mW ≈ 10 meters (LEGO NXT)
Search and Pairing
search service available to find new Bluetooth device
pairing: trust communication by learning of a common passkey-input of the passkey on both device-key used to encrypt communication
Introduction to Mechatronics 46 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth Protocol
Bluetooth piconet
Master and slaves devices (max: 7 slaves)
on master by piconet (slaves synchronized on his clock)
slaves can not initiate a data packet transfer
slaves can send a response packet right after the master one(synchronous communication)
slaves can not communicate between them
Introduction to Mechatronics 47 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth Protocol
Bluetooth NXT Communication Protocol
NXT Direct Command (also for USB communication)
direct command interpreted by the NXT Slave firmware and translatedinto functions
any Bluetooth device (phone, PC, NXT,..) can send command viaBluetooth packets
NXT Direct Command Telegram
length of the datagram (without the 2 Length bytes)command type
0x00: Direct command, response required0x80: Direct command, no response0x02: Means a response datagram from slave
command code (Ex: 0x0B=GetBatteryLevel 0x03=PlayTone)
parameters of the command, or return value
byte 1
Length, LSB Length, MSBCommand
TypeCommand
byte 2 byte 3 byte 4 byte 5 byte 6 byt..
Parameters..................
Introduction to Mechatronics 48 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 49 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
Pairing between PC and NXT -1
Get NXT Bluetooth address
each 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 NXT
2 LiNXT -i , while NXT connected by USB
>sudo ./linxt -iDevice information:NXT name: NXT02Bluetooth address: 00:16:53:02:f3:31
Introduction to Mechatronics 50 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
Pairing between PC and NXT -2
Pairing under ubuntu Linux
1 set the key in the pop-up when using the following ”connect” program2 use NXT search and Linux Bluetooth-applet:
Linux: right click on the Bluetooth-applet > preferencesset a Bluetooth name for the PC dongleNXT: menu Bluetooth > searchselect your PC name and validatevalidate the default passkeyLinux: a pop-up ask for the passkey, give the same
3 it is a one time operation, NXT now appears in Bluetooth-applet >preferences > known devices
Introduction to Mechatronics 51 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
Programming of Direct Commands
Socket
a socket is an abstraction for an inter-process communication (overnetwork)
a socket can be bind to an IP or Bluetooth address
then we can read, write on the socket data between 2 process
Compilation with gcc
gcc -o executable_name -lm -lbluetooth source_name.c
to link with Bluetooth library: -lbluetooth (math: -lm)
Example: play_tone.c
opening of a socket bind to NXT Bluetooth address
write of the direct command PlayTone 0x03
read of the response (status)
Introduction to Mechatronics 52 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
play_tone.c
#include <stdio.h>#include <stdlib.h>#include <sys/socket.h> //socket lib#include <sys/types.h>#include <bluetooth/bluetooth.h> //bluettoh lib#include <bluetooth/rfcomm.h>#define max_message_size 59
int nxtsocket; //on declare la socket
void init_bluetooth(char * btAddress) {struct sockaddr_rc addr={0};//structure du type sockaddr _rcint status;
nxtsocket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_R FCOMM);
addr.rc_family = AF_BLUETOOTH;addr.rc_channel = (uint8_t) 1;str2ba(btAddress, &addr.rc_bdaddr);// convertit un stri ng en adresse
status = connect(nxtsocket, (struct sockaddr * )&addr, sizeof(addr) );//connection }
Introduction to Mechatronics 53 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
play_tone.c
void nxt_tone(void) {/ ** 0x06 0x00 : length =6 bytes
0x00 : direct command with response0x03 : Playtone commande0x5B 0x1C : parameter 1 - 7259 (200-14000Hz)0x88 0x13 : parameter 2 - 0x1388=5000 (ms) ** /
char cmd[8]={0x06, 0x00, 0x00, 0x03,0x5B ,0x1C ,0x88,0x13 };char reply[max_message_size];
write(nxtsocket, cmd, 8); //send PlayTone command
read(nxtsocket, reply, 5); //read response
// check responseif (reply[2] != 0x02) fprintf(stderr, "not a response datag ramm");if (reply[3] != 0x03) fprintf(stderr, "not the reply to play tone");if (reply[4] != 0x00) fprintf(stderr, "status not ok");
}
Introduction to Mechatronics 54 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
play_tone.c
int main (void) {int blevel;char btaddress[18] = "00:16:53:02:f3:31"; //NXT adressinit_bluetooth(btaddress); //open connectionnxt_tone(); //direct commandclose(nxtsocket);return 0;
}
Introduction to Mechatronics 55 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication with PC
NXTlibC
A simple library to perform Direct Command
wrap the previous code in nice function
compilation:gcc -lm -lbluetooth -lnxtlibc test.c -o test
API of NXTlibC : samples
nxt_bluetooth_initialize() , nxt_bluetooth_done()
get_battery_level(),play_tone()
set_touch_sensor(), get_touch_value()
message_write(), message_read() (see next section)
Introduction to Mechatronics 56 / 97
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();
// *** all motor at 10% of power in regulate mode ** //ret = _set_output_state(ALL_WHEEL,10,
MODE_MOTORON|MODE_BRAKE|MODE_REGULATED,REGULATION_MODE_MOTOR_SPEED|REGULATION_MODE_MOTOR_SYNC,0x00, MOTOR_RUN_STATE_RUNNING, 1000000);
nxt_bluetooth_done();
return 0; }
Introduction to Mechatronics 57 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication between NXT
Outline
1 Lego Mindstorms HardwareOverviewNXT Brick Hard/Firm-ware
2 NXT Programming with NBC/NXCNBC/NXC basicsNXC structure and statementsNXC I/O API
3 Bluetooth communicationBluetooth ProtocolBluetooth communication with PCBluetooth communication between NXT
Introduction to Mechatronics 58 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication between NXT
Connection between NXT
Via the Bluetooth menu
search for others NXT
connect it on one of the free channel 1, 2 or 3 (up to 3 slave)
MasterChannel 0
Up to 3 slavesChannel 1−3
Nxt Piconet
Introduction to Mechatronics 59 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication between NXT
NXC Bluetooth API
Via the Bluetooth menu
10 mailbox on each NXT to exchange messages
if there is no read program while sending = message lost
direct commands available to control the slave
Example of message exchange
master : check slave connection, send message to mailbox 5 on slave,read again message in mailbox 1 on slave,
slave : check master connection, read its mailbox 5,put an message in its mailbox 1
Introduction to Mechatronics 60 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication between NXT
Message exchange
//MASTER#define INBOX 1#define OUTBOX 5
sub BTCheck(int conn){if (!BluetoothStatus(conn)==NO_ERR){
TextOut(5,LCD_LINE2,"Error");Wait(1000);Stop(true); }
}
task main(){string in,out="hello from master!";
BTCheck(1); //check slave connectionTextOut(10,LCD_LINE1,"Master Test");
SendRemoteString(BT_CONN,OUTBOX,out);
while (1){ ReceiveRemoteString(INBOX, true, in);TextOut(10,LCD_LINE3,in); }
}
Introduction to Mechatronics 61 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication between NXT
Message exchange
//SLAVE#define INBOX 5#define OUTBOX 1
sub BTCheck(int conn){if (!BluetoothStatus(conn)==NO_ERR){
TextOut(5,LCD_LINE2,"Error");Wait(1000);Stop(true);}
}
task main(){string in,out="thanks, master!";
BTCheck(0); //check slave connectionTextOut(10,LCD_LINE1,"Slave Test");
while(1){ ReceiveRemoteString(INBOX, true, in);SendResponseString(OUTBOX,out); // Not SendRemote !!TextOut(10,LCD_LINE3,in); }
}
Introduction to Mechatronics 62 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication between NXT
NXC Bluetooth API
Example of Direct Command
master : check slave connection,send play_tone command,wait for response status,send reset position on motor Await for response status,send full speed command on motor A with speed regulation
Introduction to Mechatronics 63 / 97
Lego Mindstorms Hardware NXT Programming with NBC/NXC Bluetooth communication
Bluetooth communication between NXT
NXC Bluetooth API
//MASTER
sub BTCheck(int conn){if (!BluetoothStatus(conn)==NO_ERR){
TextOut(5,LCD_LINE2,"Error");Wait(1000);Stop(true); }
}
task main(){BTCheck(1);
RemotePlayTone(BT_CONN, 4000, 100);until(BluetoothStatus(BT_CONN)==NO_ERR);Wait(1100);
RemoteResetMotorPosition(BT_CONN,OUT_A,true);until(BluetoothStatus(BT_CONN)==NO_ERR);
RemoteSetOutputState(BT_CONN, OUT_A, 100,OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED,OUT_REGMODE_SPEED, 0, OUT_RUNSTATE_RUNNING, 0)
}
Introduction to Mechatronics 64 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Part II
Introduction to Mobile Robotics
Introduction to Mechatronics 65 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 66 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 67 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 68 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Mobile robots classification
Mobile robots
Robots with a moving base, by opposition with robotic manipulators.
Classification based on the locomotion type.
Wheeled robots
Introduction to Mechatronics 69 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Mobile robots classification
Mobile robots
Robots with a moving base, by opposition with robotic manipulators.
Classification based on the locomotion type.
Legged robots
Introduction to Mechatronics 69 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Mobile robots classification
Mobile robots
Robots with a moving base, by opposition with robotic manipulators.
Classification based on the locomotion type.
Legged robots
Introduction to Mechatronics 69 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Mobile robots classification
Wheeled robots, legged robots+
flying robots, undersea robots.
Introduction to Mechatronics 70 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Classification and applications
Wheeled mobile robots applications
A few industrial applications
Automatic Guided Vehicles (AGV)FMC Technologies
http://www.fmcsgvs.com
Introduction to Mechatronics 71 / 97
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 71 / 97
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 navigationSRI
http://www.ai.sri.com/centibots/index.html
Introduction to Mechatronics 71 / 97
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 71 / 97
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 71 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Issues
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 72 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Issues
Issues
Technology
Issues:
mechanical architecture
embedded system
actuators
sensors
Navigation
Issues:
localization
mapping
planning
control
Introduction to Mechatronics 73 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 74 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Mechanical architecture
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 75 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Mechanical architecture
Mobile robots mechanical architecture
A large amount of systems
several types of wheels
articulated platforms
caution: all the architectures arenot kinematically consistent
Lego Tribot: a simple differential drivemobile robot
simple to build: 2 fixed wheels,with 2 independent actuators
differential drive locomotion
nonholonomic, nonlinear. . . not sosimple
Introduction to Mechatronics 76 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Mechanical architecture
Mobile robots mechanical architecture
A large amount of systems
several types of wheels
articulated platforms
caution: all the architectures arenot kinematically consistent
Lego Tribot: a simple differential drivemobile robot
simple to build: 2 fixed wheels,with 2 independent actuators
differential drive locomotion
nonholonomic, nonlinear. . . not sosimple
Introduction to Mechatronics 76 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Mechanical architecture
Lego TriBot modelling
O
~y
~x
x
y
CIR
O′
L
ρ
ω
θ
v
vd
vg
ϕr
ϕl
Wheels velocities (!anglesconvention) :
vr = −r ϕr = (ρ + L)ω
vl = r ϕl = (ρ − L)ω
and then:
v =vr + vl
2
ω =−r(ϕr + ϕl)
2L
Differential kinematics:
x = v cos θ
y = v sin θ
θ = ω
Introduction to Mechatronics 77 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Embedded system
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 78 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Embedded system
Lego TriBot embedded system
NXT Block
The 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 %)
LCD screen
Buttons
NXT processorwith Bluetooth
Introduction to Mechatronics 79 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Actuators
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 80 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Actuators
LEGO Mindstorms Servo-motor
Encoder wheel andwith optical sensor
Gear train (1:48)
Running_modes
ON: power controlopen loop controlreference: % of the max power
ON & REGULATED:speed regulationuse of position sensor signalsclosed loop with PID controller
ON & BRAKE: electronic braking
OFF: coasting (free motion)
Introduction to Mechatronics 81 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Actuators
LEGO Mindstorms Servo-motor driving
O V
9 V
90% duty cycle
O V
9 V10% duty cycle
Power control
PWM (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 braking
H-bridge with DC motorsIN1 IN2 ActionH L ForwardL H ReverseL L Brake
Introduction to Mechatronics 82 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Actuators
Electronic braking
e
i
U
R
Lτ = Ki
ω = Ke
e : back e.m.f
τ : motor torque
ω : motor angular speed
Braking
1 motor motion: e 6= 02 short circuit current i ≃ − e
R
→ τ in opposition
Introduction to Mechatronics 83 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Sensors
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 84 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Sensors
Ultrasonic Telemeters
http://www.pages.drexel.edu/ kws23/tutorials/ultraso nic/ultrasonic.html
Properties:alternate emission/reception
sounds not heard by humanear (ultrasound=20kHz to200kHz)
minimal measurementdistance and variablemaximal frequency=f(maximaldistance)
different drawbacks(directivity, reflectivity,ambient conditions sensitivity)
Introduction to Mechatronics 85 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Sensors
Lego TriBot Ultrasonic Telemeter
Introduction to Mechatronics 86 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 87 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Localization
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 88 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Localization
Dead reckoning
Odometry
Computation of the configuration by velocities integration:
x(t) =
Z t
0x(τ)dτ, y(t) =
Z t
0y(τ)dτ, θ(t) =
Z 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
Remark
Odometry: current configuration relative to the initial configuration.
Introduction to Mechatronics 89 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Mapping
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 90 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Mapping
Environment mapping
Occupancy grid: discrete map of the environment
Cell decomposition, with probability of collision from sensors measurements.
The simplest case: binary grid, without filtering, trusting the odometrylocalization.
Introduction to Mechatronics 91 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 92 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Configuration space
Definition
Configuration space = set of all the accessible configurations of the robot:
robot represented as a point: obstacles augmentation
robot with complex shapes and kinematics: more difficult problem. Overpessimistic obstacle augmentation can close doors !
Introduction to Mechatronics 93 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Roadmaps
Visibility roadmap
Graph capturing the environment topology from the polygonal obstaclesvertices.
link initial point and goal to visible vertices
Introduction to Mechatronics 94 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Roadmaps
Visibility roadmap
Graph capturing the environment topology from the polygonal obstaclesvertices.
apply the same to the new vertices
Introduction to Mechatronics 94 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Planning
Roadmaps
Visibility roadmap
Graph capturing the environment topology from the polygonal obstaclesvertices.
search for a path in the graph
Introduction to Mechatronics 94 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Control
Outline
4 Mobile robotsClassification and applicationsIssues
5 Wheeled Mobile Robots TechnologyMechanical architectureEmbedded systemActuatorsSensors
6 NavigationLocalizationMappingPlanningControl
Introduction to Mechatronics 95 / 97
Mobile robots Wheeled Mobile Robots Technology Navigation
Control
Path following problem
Problem
Find ω such that d(P, C) decreases to zero, with v imposed.
~x′
~y ′
O~x
O′
C
θr
Or
~xr
~yr
~y
P
d
a
θe
It 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 adequateinitial conditions of the problem
Introduction to Mechatronics 96 / 97
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 θe
a cos θe−
vcos θe
k(d , θe)d ,
with k(d , θe) > 0 such that k(d , ±π
2 ) = 0 gives:
d = −vak(d , θe)d .
Consequence
If a, v et k(d , θe) > 0: |d | decreases along any path.
Restrictions
No control of the orientation.
Can be solve at the planning step.
Introduction to Mechatronics 97 / 97