Introduction to Mechatronics

Transcription

1 Introduction to Mechatronics Discover robots with the Lego Mindstorms Loïc Cuvillon, Bernard Bayle Ecole Nationale Supérieure de Physique de Strasbourg Strasbourg University March 13, 2009 Introduction to Mechatronics 1 / 97

2 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 does not sponsor, authorize or endorse this document. Introduction to Mechatronics 2 / 97

3 Part I Hardware and Programming under Linux Introduction to Mechatronics 3 / 97

4 Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 4 / 97

5 Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 5 / 97

6 References NXC Programming [NXC07] J.C. Hansen, NXT Power Programming, Variant Press, 2007 [NXC] J.C Hansen, Not exactly C Programmer s Guide, [NXCT] D. Benedettelli., Programming LEGO NXT Robots using NXC, NXT Bluetooth and USB communication [LINXT] Lego Mindstorms NXT brick in Linux via USB, [BLUE] J. Schultz, Lego, Bluetooth and Linux, benkoczi/3720/data/nxt_bluetooth_handout-jeremy.pdf [NXTLIBC] Lego Mindstorms NXT Bluetooth library in C, Introduction to Mechatronics 6 / 97

7 References LEGO resources and documentation [LEGO] Bluetooth Developer Kit and Hardware Developer Kit, Introduction to Mechatronics 7 / 97

8 Overview Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 8 / 97

9 Overview Lego Mindstorms Overview NXT Inputs NXT Outputs LCD screen Servo Motors with position encoders Buttons Touch Sensor (contact switch) NXT processor with Bluetooth Sound Sensor (pressure in db) Ultrasonic Sensor (distance in cm) Light Sensor (light intensity in %) Introduction to Mechatronics 9 / 97

10 Overview NXT brick bluetooth indicator B : ON, invisible B< : ON, visible B<> : ON, connected usb indicator Servo outputs usb : connected, working ] [ : connected, not working OUT_A OUT_B OUT_C USB 2.0 connector battery level NXT brick name (configurable) loudspeaker On/Enter button Clear/Go back button Naviguation buttons Sensor port S1 S2 S3 S4 Introduction to Mechatronics 10 / 97

11 Overview NXT brick Menus My files View (testing) Bluetooth Settings NXT Program NXT files Motor rotation On/off Delete Files saved in Sound files Sound dba Connections Volume Software File (bytecode executable upload from computer) Ultrasonic cm (LCD display distance from sensor) Visability NXT version (firmware version) NXT Program Example : Search (search and connect to bluetooth device) = "Forward until an object detection, then turn left. Repeat" Introduction to Mechatronics 11 / 97

12 Overview NXT-G (LabView) advance graphical interface for programming NXT functional block and control flow Introduction to Mechatronics 12 / 97

13 Overview Some fun NXT Projects Rubik s Cube Solver by Daniele Benedettelli NXTway GS by Y. Yamamoto Introduction to Mechatronics 13 / 97

14 NXT Brick Hard/Firm-ware Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 14 / 97

15 NXT Brick Hard/Firm-ware Hardware specification LB1930 & LB1836 single and double H bridge (OUT_A and OUT_BC) USB 2.0 connector AT91SAM7S256 Amtel 32 bit ARM Processor 48MHz, 64KB RAM 256KB FLASH I2C interface with AVR proc. and digital sensor (ultrasonic) 8bit AVR Co Processor PWM signal generator A/D converter for analogic sensor (light,sound,touch ) Bluetooth Chip class 2 10 meters range RS485 UART high speed communication line on Port S4 (for future use) S1 S2 S3 S4 Introduction to Mechatronics 15 / 97

16 NXT Brick Hard/Firm-ware Hardware specification I2C: Inter-Integrated Communication need 3 lines (data,clock,ground) UART: Universal Asynchronous Receiver Transmitter (serial line/port) Note: -each sensor port (S1-4) can be used with digital or analog sensor. -6 wires by port: 3 for I2C + 3 for analog signals Introduction to Mechatronics 16 / 97

17 NXT Brick Hard/Firm-ware NXT Firmware Boot code/loader initialization of the hardware load the firmware Running executables RAM 64K Firmware : a basic operating system Files (sound,.rxe...) Firmware Boot Recovery (SAM BA) Memory FLASH 256K ROM Executables (.rxe) LEGO firmware with CodeByte interpreter Boot loader/boot code Processor multi-thread (thread: light task) provides an API for I/O (displays the menus) interprets ByteCode executables (.rxe) Software firmware and files stored in FLASH memory BOOT CODE stored in RAM allow to update firmware Introduction to Mechatronics 17 / 97

18 Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 18 / 97

19 NBC/NXC basics Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 19 / 97

20 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

21 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 Cross compilation process 1 Source editing 3 Upload of.rxe file 2 Compilation USB cable 4 Execution Introduction to Mechatronics 21 / 97

22 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

23 NXC structure and statements Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 23 / 97

24 NXC structure and statements Statements 1 comments: //This is a comment /*This is a comment*/ variables bool, byte, unsigned char, char unsigned int, short, int unsigned long, long string struct Arrays declaration of variable 8 bit 16 bit 32bit Array of byte User define Array of any type int i; int x=0x10; // 16 in hexadecimal int y=10; // 10 string msg= toto ; Introduction to Mechatronics 24 / 97

25 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 array int myarray[]; // int array (no dimension) ArrayInit(myarray,0,10); // alocation of 10 null entries myarray[0]= 15; conditions = equal!= not equal true false Expr True if Expr!=0 && logical AND between conditions logical OR between conditions Introduction to Mechatronics 25 / 97

26 NXC structure and statements Control structures if, if... else if (x==1) {x=2; z=3;} else y=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

27 NXC structure and statements Control structures switch switch (x) { case 1 : z=3; break; case 2 : // some code break; default: // some code break; } repeat and until control structure available but not C standard Introduction to Mechatronics 27 / 97

28 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 music start motion; //in parallel } // execution API multithreading in NXT =concurrent execution of tasks number of task and function must be < 256 at least an init task called main needed precedes(task1,task2,..); launch the task after end of main start task1 or StartTask(task1) ; StopAllTasks(); Introduction to Mechatronics 28 / 97

29 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 2 foo(2); // ERROR (one-variable) } Introduction to Mechatronics 29 / 97

30 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

31 NXC I/O API Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 31 / 97

32 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

33 NXC I/O API LCD API task main () { int i=1; TextOut(0,LCD_LINE1,"toto:"); // display toto on line 1 NumOut(20,LCD_LINE1,i); //display i on line 1 and column 20 Wait(3000); ClearScreen(); LineOut(40,40,60,60); //plot a line from (40,40) to (60,50) coordinates Wait(3000); } Introduction to Mechatronics 33 / 97

34 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 power Wait(1500); // run for 1.5 s OnRev(OUT_A,90); //motor A reverse at 90%, //C still going fwd at 85% Float(OUT_C); //motot C power off, free deceleration Wait(1500); Off(OUT_ABC); //motor ABC power off with braking } Introduction to Mechatronics 34 / 97

35 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 regulation Wait(5000); Off(OUT_A); StopAllTasks(); } Introduction to Mechatronics 35 / 97

36 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 regulation Wait(2000); OnFwdSync(OUT_AC, 40, 50); //motor A stop Wait(2000); OnFwdSync(OUT_AC, 40, 100); //motor A speed opposite of C Wait(2000); Off(OUT_AC); } Introduction to Mechatronics 36 / 97

37 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

38 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 bytes if (result==ldr_success) { int i=3; result=write(handle,i); } CloseFile(handle); } Introduction to Mechatronics 38 / 97

39 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 Description SENSOR_TYPE_TOUCH SENSOR_TYPE_LIGHT_ACTIVE light sensor with LED on SENSOR_TYPE_LIGHT_INACTIVE light sensor with LED off SENSOR_TYPE_SOUND_DB sound pressure in db SENSOR_TYPE_TOUCH I2C sensor (Ultrasonic) MODE Description SENSOR_MODE_RAW value between 0 < x < 1024 SENSOR_TYPE_BOOL 0 if x<512, else 1 SENSOR_TYPE_PERCENT value between 0 < x < 100 SENSOR_TYPE_EDGE count number of transition SENSOR_TYPE_TOUCH count number of level change Introduction to Mechatronics 39 / 97

40 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 : percent SetSensorLowSpeed(S2); // I2C sensor (Ultrasonic,..) on port 2 } Introduction to Mechatronics 40 / 97

41 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

42 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: wait Off(OUT_AC); OnRev(OUT_C,40); //turn //angle linked to wait time Wait(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

43 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 =prevent the 2 tasks to control motors simultaneously a task block on Acquire(), if another has already Acquire the mutex must wait for the Release Introduction to Mechatronics 43 / 97

44 Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 44 / 97

45 Bluetooth Protocol Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 45 / 97

46 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

47 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

48 Bluetooth Protocol Bluetooth NXT Communication Protocol NXT Direct Command (also for USB communication) direct command interpreted by the NXT Slave firmware and translated into functions any Bluetooth device (phone, PC, NXT,..) can send command via Bluetooth packets NXT Direct Command Telegram length of the datagram (without the 2 Length bytes) command type 0x00: Direct command, response required 0x80: Direct command, no response 0x02: Means a response datagram from slave command code (Ex: 0x0B=GetBatteryLevel 0x03=PlayTone) parameters of the command, or return value byte 1 byte 2 byte 3 byte 4 byte 5 byte 6 byt.. Length, LSB Length, MSB Command Type Command Parameters... Introduction to Mechatronics 48 / 97

49 Bluetooth communication with PC Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 49 / 97

50 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 scan Scanning... 00:16:53:04:B3:46 NXT 2 LiNXT -i, while NXT connected by USB >sudo./linxt -i Device information: NXT name: NXT02 Bluetooth address: 00:16:53:02:f3:31 Introduction to Mechatronics 50 / 97

51 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 program 2 use NXT search and Linux Bluetooth-applet: Linux: right click on the Bluetooth-applet > preferences set a Bluetooth name for the PC dongle NXT: menu Bluetooth > search select your PC name and validate validate the default passkey Linux: 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

52 Bluetooth communication with PC Programming of Direct Commands Socket a socket is an abstraction for an inter-process communication (over network) 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

53 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_rc int status; nxtsocket = socket(af_bluetooth, SOCK_STREAM, BTPROTO_RFCOMM); addr.rc_family = AF_BLUETOOTH; addr.rc_channel = (uint8_t) 1; str2ba(btaddress, &addr.rc_bdaddr);// convertit un string en adresse status = connect(nxtsocket, (struct sockaddr *)&addr, sizeof(addr) ) //connection } Introduction to Mechatronics 53 / 97

54 Bluetooth communication with PC play_tone.c void nxt_tone(void) { /**0x06 0x00 : length =6 bytes 0x00 : direct command with response 0x03 : Playtone commande 0x5B 0x1C : parameter ( Hz) 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 response if (reply[2]!= 0x02) fprintf(stderr, "not a response datagramm"); 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

55 Bluetooth communication with PC play_tone.c int main (void) { int blevel; char btaddress[18] = "00:16:53:02:f3:31"; //NXT adress init_bluetooth(btaddress); //open connection nxt_tone(); //direct command close(nxtsocket); return 0; } Introduction to Mechatronics 55 / 97

56 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

57 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 Ãă 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, ); nxt_bluetooth_done(); return 0; } Introduction to Mechatronics 57 / 97

58 Bluetooth communication between NXT Outline 1 Lego Mindstorms Hardware Overview NXT Brick Hard/Firm-ware 2 NXT Programming with NBC/NXC NBC/NXC basics NXC structure and statements NXC I/O API 3 Bluetooth communication Bluetooth Protocol Bluetooth communication with PC Bluetooth communication between NXT Introduction to Mechatronics 58 / 97

59 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) Master Channel 0 Nxt Piconet Up to 3 slaves Channel 1 3 Introduction to Mechatronics 59 / 97

60 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

61 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 connection TextOut(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

62 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 connection TextOut(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

63 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 A wait for response status, send full speed command on motor A with speed regulation Introduction to Mechatronics 63 / 97

64 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

65 Mobile robots Wheeled Mobile Robots Technology Navigation Part II Introduction to Mobile Robotics Introduction to Mechatronics 65 / 97

66 Mobile robots Wheeled Mobile Robots Technology Navigation Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 66 / 97

67 Mobile robots Wheeled Mobile Robots Technology Navigation Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 67 / 97

68 Mobile robots Wheeled Mobile Robots Technology Navigation Classification and applications Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 68 / 97

69 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

70 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

71 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

72 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

73 Mobile robots Wheeled Mobile Robots Technology Navigation Classification and applications Wheeled mobile robots applications A few industrial applications Automatic Guided Vehicles (AGV) FMC Technologies Introduction to Mechatronics 71 / 97

74 Mobile robots Wheeled Mobile Robots Technology Navigation Classification and applications Wheeled mobile robots applications More and more general public applications Toys Tribot, WowWee Automatic vacuum cleaners DirtyDog, irobot Introduction to Mechatronics 71 / 97

75 Mobile robots Wheeled Mobile Robots Technology Navigation Classification and applications Wheeled mobile robots applications Education and research Khepera II, K-team 1 h autonomy, 1 m/s max 7x3 cm, 80g (payload<250g) Multi-robot navigation SRI Introduction to Mechatronics 71 / 97

76 Mobile robots Wheeled Mobile Robots Technology Navigation Classification and applications Wheeled mobile robots applications A few High-Tech applications Spatial exploration robots Sojourner mission to Mars Introduction to Mechatronics 71 / 97

77 Mobile robots Wheeled Mobile Robots Technology Navigation Classification and applications Wheeled mobile robots applications A few High-Tech applications Inspection Introduction to Mechatronics 71 / 97

78 Mobile robots Wheeled Mobile Robots Technology Navigation Issues Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 72 / 97

79 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

80 Mobile robots Wheeled Mobile Robots Technology Navigation Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 74 / 97

81 Mobile robots Wheeled Mobile Robots Technology Navigation Mechanical architecture Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 75 / 97

82 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 are not kinematically consistent Lego Tribot: a simple differential drive mobile robot simple to build: 2 fixed wheels, with 2 independent actuators differential drive locomotion nonholonomic, nonlinear... not so simple Introduction to Mechatronics 76 / 97

83 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 are not kinematically consistent Lego Tribot: a simple differential drive mobile robot simple to build: 2 fixed wheels, with 2 independent actuators differential drive locomotion nonholonomic, nonlinear... not so simple Introduction to Mechatronics 76 / 97

84 Mobile robots Wheeled Mobile Robots Technology Navigation Mechanical architecture Lego TriBot modelling y Wheels velocities (!angles convention) : CIR ω ρ v r = rϕ r = (ρ + L)ω v l = r ϕ l = (ρ L)ω θ and then: y ϕ l O v g v v d v = vr + v l 2 ω = r( ϕr + ϕ l) 2L L ϕ r Differential kinematics: O x x ẋ = v cos θ ẏ = v sin θ θ = ω Introduction to Mechatronics 77 / 97

85 Mobile robots Wheeled Mobile Robots Technology Navigation Embedded system Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 78 / 97

86 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 the actuators. Refer to the first part of the talk for more details... NXT Inputs NXT Outputs LCD screen Servo Motors with position encoders Buttons Touch Sensor (contact switch) NXT processor with Bluetooth Sound Sensor (pressure in db) Ultrasonic Sensor (distance in cm) Light Sensor (light intensity in %) Introduction to Mechatronics 79 / 97

87 Mobile robots Wheeled Mobile Robots Technology Navigation Actuators Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 80 / 97

88 Mobile robots Wheeled Mobile Robots Technology Navigation Actuators LEGO Mindstorms Servo-motor Running_modes Encoder wheel and with optical sensor Gear train (1:48) ON: power control open loop control reference: % of the max power ON & REGULATED: speed regulation use of position sensor signals closed loop with PID controller ON & BRAKE: electronic braking OFF: coasting (free motion) Introduction to Mechatronics 81 / 97

89 Mobile robots Wheeled Mobile Robots Technology Navigation Actuators LEGO Mindstorms Servo-motor driving 9 V O V 9 V O V 90% duty cycle 10% duty cycle Power control PWM (Pulse Width Modulation) of a 8 khz signal 90% duty cycle 8.1 V 20% duty cycle 1.8 V In1 In1 +Vcc In2 + M In2 0V In1=High, In2=Low In1 In1 +Vcc M + 0V In1=Low, In2=High In2 In2 Direction and braking H-bridge with DC motors IN1 IN2 Action H L Forward L H Reverse L L Brake Introduction to Mechatronics 82 / 97

90 Mobile robots Wheeled Mobile Robots Technology Navigation Actuators Electronic braking e : back e.m.f U i R L e τ = Ki ω = Ke τ : motor torque ω : motor angular speed Braking 1 motor motion: e 0 2 short circuit current i e R τ in opposition Introduction to Mechatronics 83 / 97

91 Mobile robots Wheeled Mobile Robots Technology Navigation Sensors Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 84 / 97

92 Mobile robots Wheeled Mobile Robots Technology Navigation Sensors Ultrasonic Telemeters Properties: alternate emission/reception sounds not heard by human ear (ultrasound=20khz to 200kHz) minimal measurement distance and variable maximal frequency=f(maximal distance) different drawbacks (directivity, reflectivity, ambient conditions sensitivity) kws23/tutorials/ultrasonic/ultrasonic.html Introduction to Mechatronics 85 / 97

93 Mobile robots Wheeled Mobile Robots Technology Navigation Sensors Lego TriBot Ultrasonic Telemeter Introduction to Mechatronics 86 / 97

94 Mobile robots Wheeled Mobile Robots Technology Navigation Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 87 / 97

95 Mobile robots Wheeled Mobile Robots Technology Navigation Localization Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 88 / 97

96 Mobile robots Wheeled Mobile Robots Technology Navigation Localization Dead reckoning Odometry Computation of the configuration by velocities integration: x(t) = Z t 0 ẋ(τ)dτ, y(t) = Z t 0 The TriBot example, with a T e sampling period: ẏ(τ)dτ, θ(t) = Z t 0 θ(τ)dτ. x(k + 1) = x(k) + v(k)t e cos θ(k) y(k + 1) = y(k) + v(k)t e sin θ(k) θ(k + 1) = θ(k) + ω(k)t e Remark Odometry: current configuration relative to the initial configuration. Introduction to Mechatronics 89 / 97

97 Mobile robots Wheeled Mobile Robots Technology Navigation Mapping Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 90 / 97

98 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 odometry localization. Introduction to Mechatronics 91 / 97

99 Mobile robots Wheeled Mobile Robots Technology Navigation Planning Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 92 / 97

100 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. Over pessimistic obstacle augmentation can close doors! Introduction to Mechatronics 93 / 97

101 Mobile robots Wheeled Mobile Robots Technology Navigation Planning Roadmaps Visibility roadmap Graph capturing the environment topology from the polygonal obstacles vertices. link initial point and goal to visible vertices Introduction to Mechatronics 94 / 97

102 Mobile robots Wheeled Mobile Robots Technology Navigation Planning Roadmaps Visibility roadmap Graph capturing the environment topology from the polygonal obstacles vertices. apply the same to the new vertices Introduction to Mechatronics 94 / 97

103 Mobile robots Wheeled Mobile Robots Technology Navigation Planning Roadmaps Visibility roadmap Graph capturing the environment topology from the polygonal obstacles vertices. search for a path in the graph Introduction to Mechatronics 94 / 97

104 Mobile robots Wheeled Mobile Robots Technology Navigation Control Outline 4 Mobile robots Classification and applications Issues 5 Wheeled Mobile Robots Technology Mechanical architecture Embedded system Actuators Sensors 6 Navigation Localization Mapping Planning Control Introduction to Mechatronics 95 / 97

105 Mobile robots Wheeled Mobile Robots Technology Navigation Control Path following problem Problem Find ω such that d(p, C) decreases to zero, with v imposed. y y y r It can be shown a that: x θ e ṡ = v cos θe aω sin θe, 1 dc(s) O a P d x r ḋ = v sin θ e + aω cos θ e, θ e = ω ṡc(s). θ r O r O x a d c(s) <1 under some adequate initial conditions of the problem C Introduction to Mechatronics 96 / 97

106 Mobile robots Wheeled Mobile Robots Technology Navigation Control Path following control law Distance equation: It comes that: ḋ = v sin θ e + aω cos θ e v sin θe ω = v k(d, θ e)d, a cos θ e cos θ e with k(d, θ e) 0 such that k(d, ± π 2 ) = 0 gives: ḋ = 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