Lego 4LavA - TU Dortmund

Lego 4 LavAEntwicklung einer Lego Mindstorms

Experimentierplattform fur FPGA-basierte,konfigurierbare Multiprozessorsysteme

Nico StuteBachelorarbeit Abschlussvortrag

Lehrstuhl fur Informatik 12TU Dortmund

[email protected]

26. September 2011

Lego 4 LavA 1

Inhalt

Ziele der Arbeit

Anforderungen

Sensorinterface

Aktorinterface

Errata

Prototyp

Demo

Lego 4 LavA 2

Inhalt

⇒

Lego 4 LavA 3

Inhalt

⇒

Lego 4 LavA 3

Ziele der Arbeit

• Implementierung eines konfigurierbaren Sensor/Aktor-Interfaces inVHDL

• Einbindung des Interfaces in LavA

• Entwicklung der Hardware um Lego Sensoren/Aktoren anzusteuern

• Schreiben einer Steuerbibliothek (C/C++)

• Bau eines Lego-Demonstrators zur Evaluierung

Lego 4 LavA 4

Anforderungen

• LEGO

• I2C-Interface• Analogeingang• H-Brucke• Quadratureingang

• LavA

• Wishbone-Interface• C-Bibliothek

Lego 4 LavA 5

Anforderungen

• LEGO

• I2C-Interface• Analogeingang• H-Brucke• Quadratureingang

• LavA

• Wishbone-Interface• C-Bibliothek

Lego 4 LavA 5

Sensorinterface

Programm

Software

VHDL

Hardware

ReadSensor()

I2CWrite() I2CRead()

Write() Read()

WBWrite() WBRead()

WB-Adapter

Taktteiler

I2C-Modul

AD 9V 4,3V

Sensor

Lego 4 LavA 6

Sensorinterface - Hardware

FILE: REVISION:

DRAWN BY: PAGE OF

TITLE

82K

C

E

B BC547C

C

E

B BC547C

PCF8591

AIN0 1

AIN1 2

AIN2 3

AIN3 4

A0 5

A1 6

A2 7

VSS 8

VDD16

AOUT15

VREF14

AGND13

EXT12

OSC11

SCL10

SDA9

Vss

Vss

Vss

1K

Vcc

10K

C

E

B BC547C

C

E

B BC547C

3910K

Vss

value=9VVcc

value=5V

Vccvalue=5V

Vccvalue=5V

10K

2 1

2 1

VssVcc

value=5V

Vss

Vccvalue=5V

82K

27

SDAO

SCL

SDAI

9V

SDA / AINSCL

AIN / 9V

100n

F

10K

5V

GND

5V

GNDGND

AINAIN

FPG

A

Sens

or

Lego 4 LavA 7

Sensorinterface - VHDL

scl

sdastart addr rw ack data ack stop

4.1 Entwurf und Implementierung

Stoppbedingung (stop), Byte lesen (read), Byte schreiben (write), Bestatigung lesen(readack), Bestatigung schreiben (writeack)). Die Zustande read und write werdenjeweils achtmal in Folge wiederholt, um ein komplettes Byte zu verarbeiten. Die Unter-zustande teilen eine Taktperiode in vier gleiche Zeitschritte ein (low (l), steigende Flanke(re), high (h), fallende Flanke (fe)).

start

read

write

stop

writeack

readackl

l

l

l

re

re

re

re

init

h

hh

h

h

h re

reend

l

lfe

fe

fe

fefe

fe

Abbildung 4.2: Zustandsautomat de I2C-Moduls

when wr i t e =>case subs ta t e i s

when l =>s c l <= ’0 ’ ; −− lowsda <= data i ( count ) ; −− e in Bit s ch re i b ensubs ta t e <= re ;count <= count − 1 ;

when re =>s c l <= ’1 ’ ; −− s t e i g ende Flankesubs ta t e <= h ;

when h => −− highsubs ta t e <= f e ;

when f e =>s c l <= ’0 ’ ; −− f a l l e n d e Flankesubs ta t e <= l ;i f ( count = −1) then −− nach 8 Bit

s t a t e <= readack ; −− ack l e s encount <= 7 ;

end i f ;end case ;

Abbildung 4.3: Quellcodeauszug fur den Zustand Byte schreiben

15

Lego 4 LavA 8

Sensorinterface - Software

uns igned char read LEGO ( uns igned char c , v o l a t i l e i n t ∗ addr ){uns igned char i ;I 2 C s t a r t ( addr ) ;I 2C w r i t e (LEGO, addr ) ;I 2C w r i t e ( c + 0x42 , addr ) ;I 2C s t op ( addr ) ;

I 2 C s t a r t ( addr ) ;I 2C w r i t e (LEGO | READ, addr ) ;i = I 2C r ead (0 , addr ) ;I 2C s t op ( addr ) ;r e t u r n ( i ) ;}

uns igned char r e a d s e n s o r ( uns igned char type , uns igned char c , v o l a t i l e i n t ∗ addr ){i f ( type == LEGO){

r e t u r n ( read LEGO ( c , addr ) ) ;}r e t u r n ( read PCF ( c , addr ) ) ;

}

Lego 4 LavA 9

AktorinterfaceProgramm

Software

VHDL

Hardware

SetPwm() ReadQuad()

Write() Read()

WBWrite() WBRead()

WB-Adapter

Taktteiler

PWM-Modul Quad-Modul

9V 4,3V

Aktor

Lego 4 LavA 10

Aktorinterface - Hardware

FILE: REVISION:

DRAWN BY: PAGE OF

TITLE

13Output 3

15Sense B

14Output 4

12Input 4

6 Enable A

5 Input 1

2 Output 1

8

GND

1 Sense A

3 Output 2

7 Input 2

L298 MW15

10Input 3

11Enable B

4

Vs (power)

9

Vss (logic)

U?

Vdd

Vcc9V

Vcc5V

1K

Vdd

Vcc5V

Vcc5V

Vdd

1K1K

Vdd

Vcc5V

Vcc5V

Vdd

1K

5VGND

PWM1PWM2

AB

M1M2

BA

5VGND

PWM1PWM2AB

M1M2

BA

GND5V

GND5V

FPG

AAk

tor

FPG

AAk

tor

Lego 4 LavA 11

Aktorinterface - VHDL

• PWM

• 8 Bit• Bremsen, Links-, Recht-, Freilauf

• Quadratur

• 16 Bit• Reset bei Lesezugriff• Fehlererkennung

WB-Adapter

Taktteiler

PWM-Modul Quad-Modul

Lego 4 LavA 12

Aktorinterface - Software

v o i d s e t s p e e d ( i n t speed , s t r u c t MOTOR motor ){i f ( speed < 0){

set PWM(REV, ( uns igned char ) −speed , motor ) ;}e l s e{

set PWM(FWD, ( uns igned char ) speed , motor ) ;}

}

i n t read QUAD( s t r u c t MOTOR motor ){i n t r e s u l t = read ( motor . addr ) & 0x0000FFFF ;i f ( r e s u l t > 32768){

r e s u l t = r e s u l t − 65536 ;}r e t u r n ( r e s u l t ) ;

}

Lego 4 LavA 13

Errata

• Firmewarebug im Ultraschallsensor

• Reverse Engineering der Firmware

• 8 Bit AD-Wandler

• MAX127 (8 x 12 Bit)• ADS7828 (8 x 12 Bit)

• Helligkeitssensor 1.0 blockiert denI2C-Bus

• Farbsensor 2.0 verwendet anderesInterface

• Jumper zum Trennen des I2C-Busses• SDA an AD-Wandler

FILE: REVISION:

DRAWN BY: PAGE OF

TITLE

82K

C

E

B BC547C

C

E

B BC547C

PCF8591

AIN0 1

AIN1 2

AIN2 3

AIN3 4

A0 5

A1 6

A2 7

VSS 8

VDD16

AOUT15

VREF14

AGND13

EXT12

OSC11

SCL10

SDA9

Vss

Vss

Vss

1K

Vcc

10K

C

E

B BC547C

C

E

B BC547C

3910K

Vss

value=9VVcc

value=5V

Vccvalue=5V

Vccvalue=5V

10K

2 1

2 1

VssVcc

value=5V

Vss

Vccvalue=5V

82K

27

SDAO

SCL

SDAI

9V

SDA / AINSCL

AIN / 9V

100n

F

10K

5V

GND

5V

GNDGND

AINAIN

FPG

A

Sens

or

Lego 4 LavA 14

Errata




• MAX127 (8 x 12 Bit)• ADS7828 (8 x 12 Bit)




FILE: REVISION:

DRAWN BY: PAGE OF

TITLE

82K

C

E

B BC547C

C

E

B BC547C

PCF8591

AIN0 1

AIN1 2

AIN2 3

AIN3 4

A0 5

A1 6

A2 7

VSS 8

VDD16

AOUT15

VREF14

AGND13

EXT12

OSC11

SCL10

SDA9

Vss

Vss

Vss

1K

Vcc

10K

C

E

B BC547C

C

E

B BC547C

3910K

Vss

value=9VVcc

value=5V

Vccvalue=5V

Vccvalue=5V

10K

2 1

2 1

VssVcc

value=5V

Vss

Vccvalue=5V

82K

27

SDAO

SCL

SDAI

9V

SDA / AINSCL

AIN / 9V

100n

F

10K

5V

GND

5V

GNDGND

AINAIN

FPG

A

Sens

or

Lego 4 LavA 14

Errata




• MAX127 (8 x 12 Bit)• ADS7828 (8 x 12 Bit)




FILE: REVISION:

DRAWN BY: PAGE OF

TITLE

82K

C

E

B BC547C

C

E

B BC547C

PCF8591

AIN0 1

AIN1 2

AIN2 3

AIN3 4

A0 5

A1 6

A2 7

VSS 8

VDD16

AOUT15

VREF14

AGND13

EXT12

OSC11

SCL10

SDA9

Vss

Vss

Vss

1K

Vcc

10K

C

E

B BC547C

C

E

B BC547C

3910K

Vss

value=9VVcc

value=5V

Vccvalue=5V

Vccvalue=5V

10K

2 1

2 1

VssVcc

value=5V

Vss

Vccvalue=5V

82K

27

SDAO

SCL

SDAI

9V

SDA / AINSCL

AIN / 9V

100n

F

10K

5V

GND

5V

GNDGND

AINAIN

FPG

A

Sens

or

Lego 4 LavA 14

Errata




• MAX127 (8 x 12 Bit)• ADS7828 (8 x 12 Bit)




FILE: REVISION:

DRAWN BY: PAGE OF

TITLE

82K

C

E

B BC547C

C

E

B BC547C

PCF8591

AIN0 1

AIN1 2

AIN2 3

AIN3 4

A0 5

A1 6

A2 7

VSS 8

VDD16

AOUT15

VREF14

AGND13

EXT12

OSC11

SCL10

SDA9

Vss

Vss

Vss

1K

Vcc

10K

C

E

B BC547C

C

E

B BC547C

3910K

Vss

value=9VVcc

value=5V

Vccvalue=5V

Vccvalue=5V

10K

2 1

2 1

VssVcc

value=5V

Vss

Vccvalue=5V

82K

27

SDAO

SCL

SDAI

9V

SDA / AINSCL

AIN / 9V

100n

F

10K

5V

GND

5V

GNDGND

AINAIN

FPG

A

Sens

or

Lego 4 LavA 14

Errata




• MAX127 (8 x 12 Bit)• ADS7828 (8 x 12 Bit)




FILE: REVISION:

DRAWN BY: PAGE OF

TITLE

82K

C

E

B BC547C

C

E

B BC547C

PCF8591

AIN0 1

AIN1 2

AIN2 3

AIN3 4

A0 5

A1 6

A2 7

VSS 8

VDD16

AOUT15

VREF14

AGND13

EXT12

OSC11

SCL10

SDA9

Vss

Vss

Vss

1K

Vcc

10K

C

E

B BC547C

C

E

B BC547C

3910K

Vss

value=9VVcc

value=5V

Vccvalue=5V

Vccvalue=5V

10K

2 1

2 1

VssVcc

value=5V

Vss

Vccvalue=5V

82K

27

SDAO

SCL

SDAI

9V

SDA / AINSCL

AIN / 9V

100n

F

10K

5V

GND

5V

GNDGND

AINAIN

FPG

A

Sens

or

Lego 4 LavA 14

Errata




• MAX127 (8 x 12 Bit)• ADS7828 (8 x 12 Bit)




FILE: REVISION:

DRAWN BY: PAGE OF

TITLE

82K

C

E

B BC547C

C

E

B BC547C

PCF8591

AIN0 1

AIN1 2

AIN2 3

AIN3 4

A0 5

A1 6

A2 7

VSS 8

VDD16

AOUT15

VREF14

AGND13

EXT12

OSC11

SCL10

SDA9

Vss

Vss

Vss

1K

Vcc

10K

C

E

B BC547C

C

E

B BC547C

3910K

Vss

value=9VVcc

value=5V

Vccvalue=5V

Vccvalue=5V

10K

2 1

2 1

VssVcc

value=5V

Vss

Vccvalue=5V

82K

27

SDAO

SCL

SDAI

9V

SDA / AINSCL

AIN / 9V10

0nF

10K

5V

GND

5V

GNDGND

AINAIN

FPG

A

Sens

or

Lego 4 LavA 14

Prototyp

• Digilent Nexys 2

• Spartan 3E 1200

• 2 NXT Motoren mit Quadraturencoder

• Gyroskop und Beschleunigungssensor von HiTechnic

Lego 4 LavA 15

Prototyp

IPC IPC IPC

UART Timer Timer

Timer Outport Outport

I2C PWM PWM

WB WB WB

MBlite MBlite MBlite

Sensor Sensor

Aktor

Aktor

Timer

IO

PWM

PWM

I2C

WB

ZPU

Sensor Sensor

IO

Aktor

Aktor

Lego 4 LavA 16

Prototyp

IPC IPC IPC

UART Timer Timer

Timer Outport Outport

I2C PWM PWM

WB WB WB

MBlite MBlite MBlite

Sensor Sensor

Aktor

Aktor

Timer

IO

PWM

PWM

I2C

WB

ZPU

Sensor Sensor

IO

Aktor

Aktor

Lego 4 LavA 16

Prototyp


• Beschleunigungssensor

- Rauschen- Ruckkopplung

• Massentragheit und Spiel der Motoren• geschachtelter PID-Regler

- Uberschwingen- zu langsam

Lego 4 LavA 17

Prototyp

• 8 Bit AD-Wandler• Beschleunigungssensor




Lego 4 LavA 17

Prototyp





Lego 4 LavA 17

Prototyp



• Massentragheit und Spiel der Motoren

• geschachtelter PID-Regler


Lego 4 LavA 17

Prototyp





Lego 4 LavA 17

Prototyp





Lego 4 LavA 17

Demo

Demo

Lego 4 LavA 18

Referenzen

Lego MINDSTORMS Developer Fileshttp://mindstorms.lego.com/en-us/support/files/default.aspx

LavA Projekthttp://ess.cs.uni-dortmund.de/DE/Research/Projects/LavA/index.html

Wishbone bei OpenCoreshttp://opencores.org/opencores,wishbone

I 2C -Bus Spezifikation 3.0http://www.nxp.com/acrobat download/usermanuals/UM10204 3.pdf

Lego 4 LavA 19

Fragen

Fragen?

Lego 4 LavA 20

Documents

Lego 4LavA - TU Dortmund