2 réponses à ce sujet

[luc]

Bonjour,

j'ai un boe-bot, lorsque je le fait tourner sur du carrelage, il ne respecte pa les 90° de consigne que je lui donne.

alors j'ai monté une boussole élétronique (CMP03). la boussole fonctionne bien, mais lorsque je la coordone avec la commande de mes servo moteur sa bug,

si je met ce test

IF angle > consigneCap AND angle < consigneCap+180 THEN
DEBUG 2,0,1, "Gauche"
GOSUB gauche
ELSE
DEBUG 2,0,1, "Droite"
'GOSUB droite linge en commentaire
ENDIF

et si je place le robot à droit de la consigne le robot ce position sur la consigne

maintent si je retire la ligne en commentaire le robot va à droit alors qu'il devrait aller à gauche

IF angle > consigneCap AND angle < consigneCap+180 THEN
DEBUG 2,0,1, "Gauche"
GOSUB gauche
ELSE
DEBUG 2,0,1, "Droite"
GOSUB droite
ENDIF

merci pour votre aide, ca fait 24h que je séche, je pense que le PB vient du IF ... THEN ... ELSE .... ENDIF, mais je ne comprent car c'est tellement simple à utiliser

voici le code complet

' =========================================================================
'
' File....... asservicement_posisiton.BS2
' Purpose.... Core I2C routines for BS2/BS2e/BS2sx
' Author..... Luc Salace
' E-mail..... luc.salace@gmail.com
' Started....
' Updated.... 26 janvier 2007
'
' {$STAMP BS2}
' {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

SDA PIN 8 ' I2C serial data line
SCL PIN 9 ' I2C serial clock line


' -----[ Constants ]-------------------------------------------------------

Ack CON 0 ' acknowledge bit
Nak CON 1 ' no ack bit

CMP03 CON %11000000 ' adresse du CMP03 $C0
errCapPermise CON 2


vfp13 CON 830 ' vitesse servo moteur
vbp13 CON 670
vfp12 CON 650
vbp12 CON 850

' -----[ Variables ]-------------------------------------------------------

slvAddr VAR Byte ' I2C slave address
devNum VAR Nib ' device number (0 - 7)
addrLen VAR Nib ' bytes in word addr (0 - 2)
wrdAddr VAR Word ' word address

i2cData VAR Byte ' data to/from device
i2cWork VAR Byte ' work byte for TX routine
i2cAck VAR Bit ' Ack bit from device

angle VAR Byte ' resultat de la lecture de l'angle
consigneCap VAR Byte ' consigne du cap
errCap VAR Byte

pulseCount VAR Byte ' compteur servo moteur

i VAR Byte
' -----[ EEPROM Data ]-----------------------------------------------------


' -----[ Initialization ]--------------------------------------------------

Reset:
#IF ($STAMP >= BS2P) #THEN
#ERROR "Use I2COUT and I2CIN!"
#ENDIF


' -----[ Program Code ]----------------------------------------------------

Main:
DO

consigneCap = 1*$FF/8
GOSUB GoCap
PAUSE 3000
consigneCap = 2*$FF/8
GOSUB GoCap
PAUSE 3000
consigneCap = 3*$FF/8
GOSUB GoCap
PAUSE 3000
consigneCap = 4*$FF/8
GOSUB GoCap
PAUSE 3000
consigneCap = 5*$FF/8
GOSUB GoCap
PAUSE 3000
consigneCap = 6*$FF/8
GOSUB GoCap
PAUSE 3000
consigneCap = 7*$FF/8
GOSUB GoCap
PAUSE 3000

LOOP
END


' -----[ Subroutines ]-----------------------------------------------------
GoCap:
DO
GOSUB ReadBoussole
IF angle > consigneCap THEN
errCap = angle - consigneCap
ELSE
errCap = consigneCap - angle
ENDIF

IF errCap < errCapPermise THEN
RETURN
ENDIF

DEBUG 2,0,0,"Consigne :", DEC3 consigneCap, " Compas :", DEC3 angle, " errCap :", DEC3 errCap
i = i +1
IF consigneCap < 180 THEN

IF angle > consigneCap AND angle < consigneCap+180 THEN
DEBUG 2,0,1, "Gauche"
GOSUB gauche
ELSE
DEBUG 2,0,1, "Droite"
'GOSUB droite
ENDIF
ELSEIF consigneCap >= 180 THEN

IF angle < consigneCap AND angle > consigneCap-180 THEN
DEBUG 2,0,1, "Droite"
GOSUB droite
ELSE
DEBUG 2,0,1, "Gauche"
'GOSUB gauche
ENDIF
ENDIF

LOOP




ReadBoussole:
devNum = 0 ' device address %000
slvAddr = CMP03 | (devNum << 0) ' setup slave ID
addrLen = 1
wrdAddr = 1 ' addresse compas sur un octet $00->$FF
GOSUB Read_Byte
angle = i2cData
DEBUG 2,0,2, "cmp :", DEC3 i, " angle :", DEC3 angle
RETURN

' =====[ subroutine commande servo moteur ]============================
'commande moteur
avance: ' Left turn, about 90-degrees.
DEBUG "Avance un peu", CR
FOR pulseCount = 0 TO 73
PULSOUT 13, vfp13
PULSOUT 12, vfp12
PAUSE 20
NEXT
RETURN

gauche: ' Left turn, about 90-degrees.
PULSOUT 13, vbp13
PULSOUT 12, vfp12
RETURN

droite:
PULSOUT 13, vfp13-40
PULSOUT 12, vbp12+10
RETURN

arriere: ' Back up.
DEBUG "Demi tour, droite", CR
FOR pulseCount = 0 TO 73
PULSOUT 13, vbp13+40
PULSOUT 12, vfp12-10
PAUSE 20
NEXT
RETURN


' =====[ High Level I2C Subroutines]=======================================

' Random location write
' -- pass device slave address in "slvAddr"
' -- pass bytes in word address (0, 1 or 2) in "addrLen"
' -- word address to write passed in "wrdAddr"
' -- data byte to be written is passed in "i2cData"

Write_Byte:
GOSUB I2C_Start ' send Start
i2cWork = slvAddr & %11111110 ' send slave ID (write)
GOSUB I2C_TX_Byte
IF (i2cAck = Nak) THEN Write_Byte ' wait until not busy
IF (addrLen > 0) THEN
IF (addrLen = 2) THEN
i2cWork = wrdAddr.BYTE1 ' send word address (1)
GOSUB I2C_TX_Byte
ENDIF
i2cWork = wrdAddr.BYTE0 ' send word address (0)
GOSUB I2C_TX_Byte
ENDIF
i2cWork = i2cData ' send data
GOSUB I2C_TX_Byte
GOSUB I2C_Stop
RETURN


' Random location read
' -- pass device slave address in "slvAddr"
' -- pass bytes in word address (0, 1 or 2) in "addrLen"
' -- word address to write passed in "wrdAddr"
' -- data byte read is returned in "i2cData"

Read_Byte:
GOSUB I2C_Start ' send Start
IF (addrLen > 0) THEN
i2cWork = slvAddr & %11111110 ' send slave ID (write)
GOSUB I2C_TX_Byte
IF (i2cAck = Nak) THEN Read_Byte ' wait until not busy
IF (addrLen = 2) THEN
i2cWork = wrdAddr.BYTE1 ' send word address (1)
GOSUB I2C_TX_Byte
ENDIF
i2cWork = wrdAddr.BYTE0 ' send word address (0)
GOSUB I2C_TX_Byte
GOSUB I2C_Start
ENDIF
i2cWork = slvAddr | %00000001 ' send slave ID (read)
GOSUB I2C_TX_Byte
GOSUB I2C_RX_Byte_Nak
GOSUB I2C_Stop
i2cData = i2cWork
RETURN


' -----[ Low Level I2C Subroutines ]---------------------------------------

' *** Start Sequence ***

I2C_Start: ' I2C start bit sequence
INPUT SDA
INPUT SCL
LOW SDA

Clock_Hold:
DO : LOOP UNTIL (SCL = 1) ' wait for clock release
RETURN


' *** Transmit Byte ***

I2C_TX_Byte:
SHIFTOUT SDA, SCL, MSBFIRST, [i2cWork\8] ' send byte to device
SHIFTIN SDA, SCL, MSBPRE, [i2cAck\1] ' get acknowledge bit
RETURN


' *** Receive Byte ***

I2C_RX_Byte_Nak:
i2cAck = Nak ' no Ack = high
GOTO I2C_RX

I2C_RX_Byte:
i2cAck = Ack ' Ack = low

I2C_RX:
SHIFTIN SDA, SCL, MSBPRE, [i2cWork\8] ' get byte from device
SHIFTOUT SDA, SCL, LSBFIRST, [i2cAck\1] ' send ack or nak
RETURN


' *** Stop Sequence ***

I2C_Stop: ' I2C stop bit sequence
LOW SDA
INPUT SCL
INPUT SDA
RETURN

[Matthieu]

si tu peux , ajoute un afficheur LCD , ou led pour savoir se que tu as en entrée ..... avec tes cerveau connecter ....

Sa peux étre des parasites sur l'allim ....

[luc]

j'ai trouvé, mais j'ai pas tous compris

il faut ajouter une pause, en dessous de 170 milisecondes, j'ai le problème. mais je ne sais pas dir si cela viens du bus i2c ou ou le faites que je n'attend pas que le robot soit stabiliser avant de solicité la boussole ???

j'en prifiter pour corriger un autre bug 180° c'est $FF/2 et non 180 ;-)

si quelqu'un peu m'expliquer pourquoi la pause ?

GoCap:
DO
PAUSE 170 ' indispensable, sinon on solicite le capteur alors que le robot est en mouvment
GOSUB ReadBoussole
IF angle > consigneCap THEN
errCap = angle - consigneCap
ELSE
errCap = consigneCap - angle
ENDIF

IF errCap < errCapPermise THEN
RETURN
ENDIF

DEBUG 2,0,0,"Consigne :", DEC3 consigneCap, " Compas :", DEC3 angle, " errCap :", DEC3 errCap
i = i +1
IF consigneCap < ($FF/2) THEN

IF angle > consigneCap AND angle < consigneCap+($FF/2) THEN
DEBUG 2,0,1, "Gauche"
GOSUB gauche
ELSE
DEBUG 2,0,1, "Droite"
GOSUB droite
ENDIF

ELSE

IF angle < consigneCap AND angle > consigneCap-($FF/2) THEN
DEBUG 2,0,1, "Droite"
GOSUB droite
ELSE
DEBUG 2,0,1, "Gauche"
GOSUB gauche
ENDIF

ENDIF

LOOP