probleme de mixage 3 axes vers 4 moteurs?!

Toutes les infos, questions et idées sur la partie électronique ( et électrique ! ) et les
différents capteurs utilisés ( sauf vidéo et éclairages, voir le forum
ci-dessous ).

Modérateurs : YODA, Louky

Avatar de l’utilisateur
fefenin
Messages : 196
Inscription : Mer Déc 05, 2007 7:32 pm
Localisation : Figeac,lot (46)

probleme de mixage 3 axes vers 4 moteurs?!

Messagepar fefenin » Ven Oct 31, 2008 2:40 pm

hello tout le monde

la boite de jonction est terminée,
les fils moteurs souder dedans... reste plus qu'a remplir d'huile et tester.

juste un probleme comment mixer les 3 axes de ma manette playstation (1 a 256 valeurs pour chaque) vers les quatres moteurs du sub:

je veux pouvoir faire avant arriere tourner a gauche et droite et travelling droit et gauche

les deux premier mixage sont fait..

auriez vous des formules info etc... sur le pourquoi du comment du pourquoi comment?

pour infos, je n'est pas modifier la manette play mais j'ai juste interpreter les signaux d'origine grace a un pic 18f1320 qui doit ensuite envoyer les info par rs485 a un autre pic 18f458 dans le sous marin

je ne sais pas si le plus judicieux est de faire le mixage a la surface ou dans le sub???

merci a vous d'avance

et bonnes activités!!

fefenin
fefenin (exploboticien!)

retrouvez les infos locales de figeac sur :
http://www.telefigeac.com

Avatar de l’utilisateur
Louky
Messages : 899
Inscription : Sam Oct 22, 2005 6:33 am
Localisation : Var

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar Louky » Sam Nov 01, 2008 11:56 am

salut !

Il faudrait un peu plus d'infos sur ta config :
tes 4 moteurs sont à 45° ?
sur ta manette, comment sont les 3 axes ?

sinon voici pour info ce que j'utilise sur le p'tipi ( 2 moteurs H et un manche X,Y )

le bout de code dans le main :

Code : Tout sélectionner

   // calcul manche X et Y pour moteurs D et G
   // la variation des valeurs manche est de +-32 autour du neutre
   X=((signed int16)(MANCHE_X)) - ((signed int16)(NEUTRE_X)) ;
   Y=((signed int16)(MANCHE_Y)) - ((signed int16)(NEUTRE_Y)) ;

   // moteur G
   T=Y+X ;
   MOT_G=calcul_mot( T )   ;
   // moteur D
   T=Y-X;
   MOT_D=calcul_mot( T )   ;


et la fonction de troncature :

Code : Tout sélectionner

/*--------------------------------------------------------------------*/
// controle et tronque le min / max pour le calcul des consignes moteurs
unsigned char calcul_mot( signed int16 tmp)
{
if( tmp > (signed int16)(MAX_MANCHE) )   tmp=(signed int16)(MAX_MANCHE) ;
if( tmp < -((signed int16)(MAX_MANCHE))) tmp=-((signed int16)(MAX_MANCHE)) ;
return((unsigned char)(((signed int16)(P_NEUTRE))+tmp) );
}


ça permet d'avoir une loi de variation assez simple et agréable au pilotage.

dans le cas de 4 moteurs, la loi de variation devrait être un peu plus complexe ...

sinon, pour le mixage, perso, je préfère le faire en surface car il est plus simple de faire des modifs.
j'essaye de faire le soft "fond" le plus simple possible afin que toute "l'intelligence" soit concentré en surface. De cette façon il n'y a pas besoin d'ouvrir l'engin pour faire des modifs de logiciel.
c'est par exemple le cas pour la mise à l'echelle des capteurs, ça permet de faire l'étallonage depuis le soft de surface.
dans le cas du p'tipi, la carte fond recoit directement une valeur de vitesse à pour chaque moteur.

Avatar de l’utilisateur
fefenin
Messages : 196
Inscription : Mer Déc 05, 2007 7:32 pm
Localisation : Figeac,lot (46)

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar fefenin » Sam Nov 01, 2008 2:08 pm

bonjour,

la config de mon sub est la suivante:


deux moteur pour la plongée remontée placés au centre sur le coté droit et gauche
4 moteur placés en carré (deux devant placé a 90 degrés l'un de lautre et pareil derriere)

je suis entrain de voir pour uploader des images de mon sub mais j'ai des soucis informatique...


sinon pour le mixage simple j'avais fais ca (c'est la meme chose que toi mais en basic)

Code : Tout sélectionner

'Un mixage ailerons/profondeur revient à générer deux signaux de largeur d'impulsion:
'N + (A+P)/2 ms
'N + (A-P)/2 ms
'soit : neutral + ((psxjoystick_1-neutral) + (psxjoystick_2-neutral))/2

'mixage up/down left/right
voie1 = 128 + ((psxjoystick_2 - 128) + (psxjoystick_1 - 128))/2
voie2 = 128 + ((psxjoystick_2 - 128) - (psxjoystick_1 - 128))/2


reste pour le mixage d'un autre axes , j'ai fais un schema que je suis entrain d'uploader ca devras aider pour la comprehension

la commande play devra etre configurer de maniere a avoir l'accelerateur sur le pad analogic de droite (haut bas) le travelling ou crab sur le pad de droite (droite gauche)
la plongée sur le pad de gauche (haut bas) et pour tourner sur place pad de gauche (droite gauche)

maintenant faut arriver a mixer toutes ces entrées pour sortir un signal pour les quatres moteurs...



ImageImageImageImage
ImageImageImage
ImageImageImage
Image
fefenin (exploboticien!)

retrouvez les infos locales de figeac sur :
http://www.telefigeac.com

Avatar de l’utilisateur
Louky
Messages : 899
Inscription : Sam Oct 22, 2005 6:33 am
Localisation : Var

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar Louky » Lun Nov 03, 2008 11:04 am

Waow ! , jolie la pieuvre ! :wink:

impatient d'être à l'été prochain ...

Pour le mix, j'avias un peu réfléchi à la question pour mon futur ( gros ) engin ( aprés celui qui est en cours ! )

d'abord, les conventions :
manche 1 : X et Y => déplacement à cap constant dans la direction donnée par le joystick
manche 2 ( ou potar ) : RZ => rotation autour de l'axe Z ( positif vers la droite )

pour les moteurs :
M1 = avant droit , sens positif propulse vers avant gauche
M2 = avant gauche , sens positif propulse vers avant droit
M3 = arriere gauche , sens positif propulse vers arriere droit
M4 = arriere droit , sens positif propulse vers arriere gauche

( les moteurs avant tirent vers l'avant, les arrières, vers l'arrière ! )

pour les mixages je pensais à un truc trés simple du genre :
M1 = Y - X - Z
M2 = Y + X + Z
M3 = -Y + X - Z
M4 = -Y - X + Z

Ensuite il suffit de tronquer les valeurs comme pour l'exemple du p'tipi.

Pour améliorer, il est aussi possible de prévoir deux coeff de pondération :
un pour X et Y ( commun au 2 axes ) et un pour Z, pour affiner les réactions en fonction des tests

En tout cas, les 1er tests risquent d'être marrants ... :roll:

Avatar de l’utilisateur
YODA
Messages : 619
Inscription : Sam Oct 22, 2005 6:09 am
Localisation : Var

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar YODA » Lun Nov 03, 2008 6:14 pm

Superbe boulot !
Le Doc Octopus n'a qu'à bien se tenir... :lol: :lol: :lol:
Vivement l'été prochain pour voir la "bête" en pleine action !!!
je suis franchement épaté par le travail effectué; encore bravo.
A bientôt.

Avatar de l’utilisateur
fefenin
Messages : 196
Inscription : Mer Déc 05, 2007 7:32 pm
Localisation : Figeac,lot (46)

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar fefenin » Mer Nov 05, 2008 6:57 pm

merci pour vos encouragement!

je pense que sans le site je serais pas aller aussi loin aussi vite! merci a vous!

sinon pour le mix ca me parait pas mal se que tu m'a passe Louky.
j'essayerai plus tard avec la manette play , pour le moment je vais faire un teste de fonctionnement vite fais avec ma radio commande (faut d abord que je fasse le vide d air dans l huile et que je remplace les colliers plastique qui serrent pas assez les tuyaux (grosses fuites)

sinon doc octopus ca me va mais risque d' y avoir une surprise a la fin (c'est yellow turtle son petit nom)

j'avoue que pour le moment ca ressemble plus a une pieuvre!!

je vous tiens au jus

a bientot et bonne continuation
fefenin (exploboticien!)

retrouvez les infos locales de figeac sur :
http://www.telefigeac.com

Avatar de l’utilisateur
fefenin
Messages : 196
Inscription : Mer Déc 05, 2007 7:32 pm
Localisation : Figeac,lot (46)

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar fefenin » Dim Nov 16, 2008 2:44 pm

salut tout le monde!

le code fonctionne grace a vous (je pensais pas a un truc aussi simple en fait!)

je rajouterai plus tard comme le sugere louky un coef pour un pilotage plus fin...

voici donc:

Code : Tout sélectionner

louky_mix:
'  dive psxjoystick_4                throttle (Y) psxjoystick_2
'HHHHHHHHH                                         HHHHHHHHH
'H                H                                         H                H
'H       O       H  Rudder (X) psxjoystick_3  H       O       H  travelling (Z) psxjoystick_1
'H                H                                         H                H
'HHHHHHHHH                                         HHHHHHHHH


'pour les moteurs :
'M_f_r = avant droit , sens positif propulse vers avant gauche
'M_f_l = avant gauche , sens positif propulse vers avant droit
'M_r_l = arriere gauche , sens positif propulse vers avant droit
'M_r_r = arriere droit , sens positif propulse vers avant gauche

'( les moteurs avant tirent vers l'avant, les arrières, aussi )

'pour les mixages je pensais à un truc trés simple du genre :
'M_f_r = Y - X - Z
'M_f_l = Y + X + Z
'M_r_l = -Y + X - Z
'M_r_r = -Y - X + Z

'Ensuite il suffit de tronquer les valeurs comme pour l'exemple du p'tipi.

'Pour améliorer, il est aussi possible de prévoir deux coeff de pondération :
'un pour X et Y ( commun au 2 axes ) et un pour Z, pour affiner les réactions en fonction des tests

'servo_min = 800       '   might be too low
'servo_max = 2200      'might be too high

Dim M_f_r As DWord
Dim M_f_l As DWord
Dim M_r_l As DWord
Dim M_r_r As DWord

'all fine
'pas plus de deux operande par ligne avec proton ide
M_f_r = (psxjoystick_1-128)           
M_f_r = M_f_r + (psxjoystick_3-128)
M_f_r = M_f_r - (psxjoystick_2-128)

M_f_l = -(psxjoystick_1-128)         
M_f_l = M_f_l - (psxjoystick_3-128)
M_f_l = M_f_l - (psxjoystick_2-128)

M_r_r =-(psxjoystick_1-128)               
M_r_r = M_r_r + (psxjoystick_3-128)
M_r_r = M_r_r - (psxjoystick_2-128)

M_r_l = (psxjoystick_1-128)
M_r_l = M_r_l - (psxjoystick_3-128)
M_r_l = M_r_l - (psxjoystick_2-128)

'protection can't go under and behind the values
If M_f_r <= -128 Then M_f_r = -128
If M_f_r >= 128 Then M_f_r = 128
If M_f_l <= -128 Then M_f_l = -128
If M_f_l >= 128 Then M_f_l = 128
If M_r_l <= -128 Then M_r_l = -128
If M_r_l >= 128 Then M_r_l = 128
If M_r_r <= -128 Then M_r_r = -128
If M_r_r >= 128 Then M_r_r = 128

'display on lcd 16 *2 char
SerOut lcd,bds,[$A1,0,0,$A2,SDEC3 M_f_r,",",SDEC3 M_f_l," ",$00,$A1,0,1,$A2,SDEC3 M_r_r,",",SDEC3 M_r_l," ",$00] 


reste a convertir mes valeurs -128 a plus 128 en valeur servo ppm de 800 ms a 2200ms (servo min et max dans le code)

je pense tester tout ca sans camera, et sans cable pour le moment, assez vite

a bientot pour les news!
fefenin (exploboticien!)

retrouvez les infos locales de figeac sur :
http://www.telefigeac.com

Avatar de l’utilisateur
fefenin
Messages : 196
Inscription : Mer Déc 05, 2007 7:32 pm
Localisation : Figeac,lot (46)

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar fefenin » Mar Nov 18, 2008 2:55 pm

une idéee pour ne pas utiliser de signe est traiter les données de la manette directement (de 0 a 256 et non pas de -128 a +128)

cela simplifierais le travail de mon pic

ps: je suis plus qu'une bille en math


merci d'avance
fefenin (exploboticien!)

retrouvez les infos locales de figeac sur :
http://www.telefigeac.com

Avatar de l’utilisateur
Louky
Messages : 899
Inscription : Sam Oct 22, 2005 6:33 am
Localisation : Var

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar Louky » Mar Nov 18, 2008 4:56 pm

je ne suis pas sur de saisir ta question.

veux tu soulager le Pic juste parce que tu est membre du FLP ?
( = front de libération des Pics :lol: )

ou alors, est ce pour un problème de vitesse d'execution ou de taille de code ?

si c'est un problème de vitesse, je ne voit pas trop de solutions !
( à part virer le basic et passer au C :D )

si c'est un problème de taille mémoire => prendre un Pic plus gros ...
( brillant comme idée, non ? )

Par contre pour la conversion entre la valeur de consigne et la largeur d'impulsion, tu peux utiliser une table de constantes.
tu passe la consigne en indice et en sortie du tableau tu récupère ta largeur de pulse.
ça prend 256 octets en mémoire, mais l'avantage c'est que tu peux avoir une loi non linéaire.
tu peux aussi ajouter une "zone morte" ( dead band ) autour du neutre.
tu peux ensuite attaquer directement avec la valeur de pulse qui permet le démarrage du moteur.
( au lieu d'avoir toute une plage ou ton moteur "grogne" sans tourner )

bref, beaucoup de possibilités pour juste 256 octets perdus !

par contre je ne sais pas si ça répond à ta question ? :shock:

Avatar de l’utilisateur
fefenin
Messages : 196
Inscription : Mer Déc 05, 2007 7:32 pm
Localisation : Figeac,lot (46)

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar fefenin » Mar Nov 18, 2008 7:03 pm

en fait ta reponse est tres satisfaisante!

je pensais etre membre du FLP!! car sur le forum de proton ide quand l'on post un code non optimiser on se fait presque lincher!

sinon la vitesse pour l'instant est plus que tres correcte.
en clair tout fonctionne tres bien, je viens de terminer tout ca, j'ai branché mes varios,et tout fonctionne a merveille.

sinon pour le coup de passer au C je suis pas chaud... en plus il parait que proton est aussi efficace grace a son "optimiser" que les compiler C,

il contient aussi une librairie enorme de macro , comme pour communiquer avec les protocoles rc5 sony servomoteur i2c etc...,vraiment simple a utiliser pour les "Not very good" comme moi, exemple:

Code : Tout sélectionner

'pilotage de servomoteur :
'boucle
loop:
servo portb.0, 1500 'envoie d'une impulsion de 1500 ms au servo
delayms 10 'pause de 10ms
goto loop



je suis sur que le C est bien plus performant mais malheureusement le circuit scolaire peut creer des blocages...

en tout les cas je ne souhaite pas vous faire envie avec se soft car il est beaucoup trop cher a mon gout.

sinon, plus important, maintenant que le programme tourne :

la dead band, compte tenue des characteristique de retour a zero des potars de la manette playstation2 ,(trop pourries en fait) va etre completement necessaire!

par contre pour infos les vario brushless (en tout cas les miens) ne "grogne" pas les moteurs tournent ou pas
si l'on bloque les helices est que la consigne donné est "a fond" alors le vario essaye plusieur fois de demarrer le moteur (2 ou 3) et se met en securité tout cela s'effectue en moins de deux secondes ( la mise en secu

la table de constante me parait une idée genial pour ameliorer mon programme car, possibilitée de faire des courbe exponentiel pour ameliorer le pilotage etc...

par contre je n'ai jamais fait ca!?

un petit liens?? meme en C car je le lis .

merci encore et bonne continuation sur les projet en cours!!


ps: le coup du moteur brushless special sub me plait vachement :wink:
fefenin (exploboticien!)

retrouvez les infos locales de figeac sur :
http://www.telefigeac.com

Avatar de l’utilisateur
Louky
Messages : 899
Inscription : Sam Oct 22, 2005 6:33 am
Localisation : Var

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar Louky » Mer Nov 19, 2008 5:43 pm

sur le forum de proton ide quand l'on post un code non optimiser on se fait presque lincher!

optimiser et basic ? , c'est une blague ? :roll: :lol:
( désolé, je sais c'est mesquin, mais je n'ai pas pu m'en empécher... :wink: )

en plus il parait que proton est aussi efficace grace a son "optimiser" que les compiler C,

Je parie que c'est ce que disent les accros du basic ? :lol:
( re-désolé !!! :D , ... mais bon, ça vient d'un accro au C :wink: )

par contre pour infos les vario brushless (en tout cas les miens) ne "grogne" pas les moteurs tournent ou pas


effectivement, ce n'est pas vrai pour les brushless, j'en suis encore au moteur CC dans ma tête !

Pour le code, je n'ais pas d'exemple en C, j'ai utilisé ce truc pour un tout petit variateur à base de PIC 8 pattes, c'est en assembleur Microchip.
il récupère une pulse type R/C et en fonction de sa largeur pilote l'étage de puissance avec le rapport cyclique adapté.

voici l'extrait du code, 'pulse' contient la largeur de la pulse de commande

Code : Tout sélectionner

fcalcde:         ; fin calcul commande

   ;************************ on limite 'pulse' a $1F pour ne pas eclater le tableau
   movlw   0x20      ; on teste en fait >= 0x20
   subwf   pulse,0      ; pulse - w => w
   btfss   STATUS,C   ; si carry = 1 , resultat positif ou nul, pulse trop grande

   goto   lectab      ; pulse OK , on lit le tableau

   movlw   0x1F      ; pulse trop grande on limite a $1F
   movwf   pulse

lectab:   
   movf   pulse,0            
   call   tabcde      ; lecture de la table
   movwf   ton      ; stocke dans ton

     goto    fintstf


et voici la déclaration de la table

Code : Tout sélectionner

; TABLE CORRESPONDANCE TPS PULSE -> TPS CDE ------------------------
tabcde   addwf   PCL,F
p0   retlw   0
p1   retlw   0   
p2   retlw   0
p3   retlw   0
p4   retlw   0
p5   retlw   0
p6   retlw   0X05
p7   retlw   0X0F
p8   retlw   0X19
p9   retlw   0X23
p10   retlw   0X2D
p11   retlw   0X37
p12   retlw   0X41
p13   retlw   0X4B
p14   retlw   0X55
p15   retlw   0X5F
p16   retlw   0X69
p17   retlw   0X73   
p18   retlw   0X7D
p19   retlw   0X87
p20   retlw   0X91
p21   retlw   0X9B
p22   retlw   0XA5
p23   retlw   0XAF
p24   retlw   0XB9
p25   retlw   0XC3
p26   retlw   0XCD
p27   retlw   0XD7
p28   retlw   0XE1
p29   retlw   0XEB
p30   retlw   0XF5
p31   retlw   0XFF


les valeurs à 0 correspondent à la deadband. ( le sens est géré ailleurs )

En C ça donnerai un truc du genre

Code : Tout sélectionner

// declaration du tableau
unsigned const char TABCDE[]= { 0,0,0,5,15,25, ( ici le reste des valeurs ) ,255} ;

// ... autre code

// dans la fonction concernée, utilisation du tableau
ton=TABCDE[pulse] ;

Avatar de l’utilisateur
fefenin
Messages : 196
Inscription : Mer Déc 05, 2007 7:32 pm
Localisation : Figeac,lot (46)

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar fefenin » Mer Nov 19, 2008 7:09 pm

merci pour les exemples!

par contre l'assembler c'est meme pas la peine je pige que dalle !

le C c'est ok!

je sais qu'il y a toujours eux une petite gueguere C contre basic!

je suis quand meme content de pouvoir faire des calculs de trajectoires GPS de logger sur SD etc... en moins de 2 pages, en basic ... je pense qu'en C je m'en sortirai pas (c'est valable seulement pour moi)


par contre si vous connaissez des cours ca m'interresse!
car j'aimerais me mettre au filtre de kalman que le pic, en basic a du mal a gerer (meme la serie 18f, peut etre en DSP car ils sont gerer par proton aussi..)


du coup j'hesite a poster le soft...???
fefenin (exploboticien!)

retrouvez les infos locales de figeac sur :
http://www.telefigeac.com

Avatar de l’utilisateur
Louky
Messages : 899
Inscription : Sam Oct 22, 2005 6:33 am
Localisation : Var

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar Louky » Jeu Nov 20, 2008 4:48 pm

pour le soft pas d'hésitations à avoir, tu peux le poster sans crainte !
( je suis juste un peu taquin de temps en temps :wink: )

pour les cours, je n'ai rien en stock, mais il y a des masses de bouquins sur le C et surement encore plus de sites web !

je suis quand meme content de pouvoir faire des calculs de trajectoires GPS de logger sur SD etc... en moins de 2 pages, en basic ...


Oui, mais il ne faut pas confondre le language et les librairies. Ces facilités te sont permises grace aux librairies que proton fourni avec son language.
De la même façon, en C, il y a par exemple le compilateur CCS qui fournit tout un tas de librairies pour le PIC.

Pour les filtres de Kalman, je n'ai jamais utilisé,mais si c'est pour les asservissement du ROV, un bon PID marche aussi bien.
et c'est beaucoup moins compliqué !

Avatar de l’utilisateur
fefenin
Messages : 196
Inscription : Mer Déc 05, 2007 7:32 pm
Localisation : Figeac,lot (46)

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar fefenin » Sam Nov 22, 2008 10:12 am

hello!

voici donc mon caode

Code : Tout sélectionner

'****************************************************************
'*  Name    : UNTITLED.BAS                                      *
'*  Author  : [select VIEW...EDITOR OPTIONS]                    *
'*  Notice  : Copyright (c) 2008 [select VIEW...EDITOR OPTIONS] *
'*          : All Rights Reserved                               *
'*  Date    : 07/06/2008                                        *
'*  Version : 1.0                                               *
'*  Notes   :                                                   *
'*          :                                                   *
'****************************************************************
Device = 16F88
Config INTRC_IO, WDT_OFF , MCLR_OFF, PWRTE_ON , BODEN_OFF , LVP_OFF , CP_OFF , DEBUG_OFF

'Put the 232 in an interrupt
'Make a loop to scan keyboard, look to see if data has arrived in the buffer and print to the screen.
'No delays....
'Really its a simple one.

'------ GP Interrupt ROUTINE VARS -----------------------

'Symbol GIEL = INTCON.6 ' LOW PRIORITY PERIPHERAL INTERRUPT
'Symbol GIE = INTCON.7 ' GLOBAL INTERRUPT ENABLE
'Symbol GIEH = INTCON.7 ' HIGH PRIORITY INTERRUPT ENABLE
'Symbol TMR2IE = PIE1.1 ' TMR2 TO PR2 MATCH INTERRUPT ENABLE BIT
'Symbol TMR2IP = IPR1.1 ' TMR2 TO PR2 MATCH INTERRUPT PRIORITY BIT


'--------------- ALIAS'S FOR USART1 ---------------
'Symbol USARTRIF1 = PIR1.5 ' USART1 RECEIVE INTERRUPT FLAG BIT
'Symbol USARTRIEF1 = PIE1.5 ' USART1 RECEIVE INTERRUPT ENABLE BIT
'Symbol USARTRIP1 = IPR1.5 ' USART1 RECEIVE INTERRUPT PRIORITY BIT
ALL_DIGITAL true

OSCCON = %01100000 ' Internal x MHz osc  %01100000

TRISB = %01000000'%01000111
TRISA = %00000000
Symbol lcd = PORTA.1
Symbol bds = 32 ' 19200 188'4800 bds




init:

Symbol dat = PORTB.6 'need a pull up' Play Station game controller connections.
Symbol cmd = PORTB.7
Symbol att = PORTB.5
Symbol clk = PORTB.4
'ack is not plugged
 
'SerOut lcd,bds,[$A0]
Dim psxmode As Byte  ' Variable definitions.
Dim psxbutton_1 As Byte  ' Variable definitions.
Dim psxbutton_2 As Byte
Dim psxjoystick_1 As DWord'Byte
Dim psxjoystick_2 As DWord'Byte
Dim psxjoystick_3 As DWord'Byte
Dim psxjoystick_4 As DWord'Byte
    Dim voie1 As Byte
    Dim voie2 As Byte
    Dim voieA As Byte
    Dim voieB As Byte
    Dim voieC As Byte
    Dim voieD As Byte
Dim S_motor As Byte

    Dim up      As Bit
    Dim down    As Bit
    Dim right   As Bit
    Dim left    As Bit
    Dim selec   As Bit
    Dim start   As Bit
    Dim L_stick As Bit
    Dim R_stick As Bit
    Dim cross   As Bit
    Dim trian   As Bit
    Dim round   As Bit
    Dim squar   As Bit


DelayMS 1000
SerOut lcd,bds,[$A0]
DelayMS 100
SerOut lcd,bds,[$A3,$01]',$A5,$02]
SerOut lcd,bds,[$A1,1,0]'place cursor on x y
SerOut lcd,bds,[$A2,"PS2 Controller",$00]   'elcd
DelayMS 1000
SerOut lcd,bds,[$A3,$01]',$A5,$02]

'software starts here
main:
 
GoSub config_mode              'run config mode at startup
'gosub init_dualshock           'doesn't work!?
GoSub analog_lock              'set the pad to analog mode with analog button lock

'GoSub digital_lock
GoSub exit_config_mode         'all settings ok ,exit the config mode
DelayMS 200
GoSub config_mode              'run config mode at startup
'gosub init_dualshock           'doesn't work!?
GoSub vibration_en             'set analog vibration to enable (default is probably disable)

GoSub exit_config_mode         'all settings ok ,exit the config mode
DelayMS 200


loop:                          'main loop

GoSub get_buttons_sticks       'get all datas from the pad
'DelayMS 500                    'wait before vibrate both motors

S_motor = $01

If psxbutton_2 = $FE Then  GoSub vibration_run            'vibrate motors (see file ps2cmd01.txt to change the speed)
'display datas from get_buttons_sticks subroutine on a serial lcd (alcd from selectronic)
GoSub louky_mix
'SerOut lcd,bds,[$A1,0,0,$A2,BIN8 psxbutton_1,",",BIN8 psxbutton_2,$00,$A1,0,1,$A2,DEC3 psxjoystick_1,",",DEC3 psxjoystick_2,",",DEC3 psxjoystick_3,",",DEC3 psxjoystick_4,$00]     
'DelayMS 100                    'wait a bit
         
GoTo loop                      'again and again


louky_mix:
'  dive psxjoystick_4                throttle (Y) psxjoystick_2
'HHHHHHHHH                              HHHHHHHHH
'H       H                              H       H
'H   O   H  Rudder (X) psxjoystick_3    H   O   H  travelling (Z) psxjoystick_1
'H       H                              H       H
'HHHHHHHHH                              HHHHHHHHH


'pour les moteurs :
'M_f_r = avant droit , sens positif propulse vers avant gauche
'M_f_l = avant gauche , sens positif propulse vers avant droit
'M_r_l = arriere gauche , sens positif propulse vers avant droit
'M_r_r = arriere droit , sens positif propulse vers avant gauche

'( les moteurs avant tirent vers l'avant, les arrières, vers l'arrière ! )

'pour les mixages je pensais à un truc trés simple du genre :
'M_f_r = Y - X - Z
'M_f_l = Y + X + Z
'M_r_l = -Y + X - Z
'M_r_r = -Y - X + Z

'Ensuite il suffit de tronquer les valeurs comme pour l'exemple du p'tipi.

'Pour améliorer, il est aussi possible de prévoir deux coeff de pondération :
'un pour X et Y ( commun au 2 axes ) et un pour Z, pour affiner les réactions en fonction des tests
    Dim Steps As Float
Dim M_f_r As DWord
Dim M_f_l As DWord
Dim M_r_l As DWord
Dim dummy As Byte
Dim M_r_r As DWord
   
Dim Servo_min As Word
Dim Servo_cen As Word
Dim Servo_max As Word

Servo_min = 500       '   may be to low
Servo_cen = 1500
Servo_max = 2500     '

'mixing device all fine
M_f_r = (psxjoystick_1-128)           
M_f_r = M_f_r + (psxjoystick_3-128)
M_f_r = M_f_r - (psxjoystick_2-128)
    M_f_l = -(psxjoystick_1-128)         
    M_f_l = M_f_l - (psxjoystick_3-128)
    M_f_l = M_f_l - (psxjoystick_2-128)
M_r_r =-(psxjoystick_1-128)               
M_r_r = M_r_r + (psxjoystick_3-128)
M_r_r = M_r_r - (psxjoystick_2-128)
    M_r_l = (psxjoystick_1-128)
    M_r_l = M_r_l - (psxjoystick_3-128)
    M_r_l = M_r_l - (psxjoystick_2-128)

'can't go under and behind the values
If M_f_r <= -128 Then M_f_r = -128
If M_f_r >= 128 Then M_f_r = 128
    If M_f_l <= -128 Then M_f_l = -128
    If M_f_l >= 128 Then M_f_l = 128
If M_r_l <= -128 Then M_r_l = -128
If M_r_l >= 128 Then M_r_l = 128
    If M_r_r <= -128 Then M_r_r = -128
    If M_r_r >= 128 Then M_r_r = 128

'debuging on lcd
'SerOut lcd,bds,[$A1,0,0,$A2,SDEC3 M_f_r,",",SDEC3 M_f_l," ",$00,$A1,0,1,$A2,SDEC3 M_r_r,",",SDEC3 M_r_l," ",$00] 

'convert to useable servo pulse
Steps = Servo_max - Servo_min
Steps = Steps / 256     'dec  7.812
'if servo_min = 500 Servo_cen = 1500 and  Servo_max = 2000 then :
'2500-500=2000; 2000/256values = 7.8125; 1500 + (-128 * 7.8125) = 500;
'1500 + (0 * 7.8125) = 1500;1500 + (+128 * 7.8125) = 2500 ;
M_f_r = Servo_cen + (M_f_r * Steps)
    M_f_l = Servo_cen + (M_f_l * Steps)
M_r_l = Servo_cen + (M_r_l * Steps)
    M_r_r = Servo_cen + (M_r_r * Steps)

'debuging on lcd
'SerOut lcd,bds,[$A1,0,0,$A2,Dec M_f_r,",",Dec M_f_l," ",$00,$A1,0,1,$A2,Dec M_r_r,",",Dec M_r_l," ",$00]

'protection against over range
If M_f_r <= Servo_min Then M_f_r = Servo_min
If M_f_r >= Servo_max Then M_f_r = Servo_max
    If M_f_l <= Servo_min Then M_f_r = Servo_min
    If M_f_l >= Servo_max Then M_f_r = Servo_max
If M_r_l <= Servo_min Then M_f_r = Servo_min
If M_r_l >= Servo_max Then M_f_r = Servo_max
    If M_r_r <= Servo_min Then M_f_r = Servo_min
    If M_r_r >= Servo_max Then M_f_r = Servo_max

'debuging on lcd
SerOut lcd,bds,[$A1,0,0,$A2,SDEC M_f_r,",",SDEC M_f_l," ",$00,$A1,0,1,$A2,SDEC M_r_r,",",SDEC M_r_l," ",$00]

Servo PORTB.0,M_f_r
Servo PORTB.1,M_f_l
Servo PORTB.2,M_r_r
Servo PORTB.3,M_r_l


DelayMS 10

Return

reverse:
'Le principe du montage consiste à générer un signal de durée égale à 3 millisecondes moins la durée de l'impulsion de commande issue du récepteur.
'au neutre : durée = 3 -1,5 = 1,5 ms ==> le servo reste au neutre
'à droite : durée = 3 - 2 = 1 ms ==> le servo part à gauche
'à gauche : durée = 3 - 1 = 2 ms ==> le servo part à droite.
Return

get_buttons_sticks:
' The first shiftin gets the mode. (73) (my pic is not fast enaught to get the first byte send by the pad it gets next byte 5A instead)
' The next 2 shiftins gets the pushbutton data.
' The next 4 shiftins gets the analog joystick data.
    Low att
    SHOut cmd,clk,lsbfirst_h,[$01,$42] ' Initiate request for data from PSX controller.
        SHIn dat,clk,lsbpost_h,[psxmode,psxbutton_1,psxbutton_2,psxjoystick_1,psxjoystick_2,psxjoystick_3,psxjoystick_4]
    High att
Return
        'SerOut lcd,bds,[$A1,0,1,hex psxdata[3],",",hex psxdata[5],",",hex psxdata[1]]      'see what is changing...
        'SerOut lcd,bds,[$A1,0,2,hex psxdata[4],",",hex psxdata[6],",",hex psxdata[2]]
         
config_mode:
Low att
SHOut cmd,clk,lsbfirst_h,[$01,$43,$00,$01,$00]      'simply send config_mode data on command line
SHIn dat,clk,lsbpost_h,[psxmode,psxbutton_1,psxbutton_2,psxjoystick_1,psxjoystick_2,psxjoystick_3,psxjoystick_4]
High att
Return

init_dualshock:
Low att
SHOut cmd,clk,lsbfirst_h,[$01,$4D,$00,$00,$01]      'simply send init_dualshock data on command line
SHIn dat,clk,lsbpost_h,[psxmode,psxbutton_1,psxbutton_2,psxjoystick_1,psxjoystick_2,psxjoystick_3,psxjoystick_4]
High att
Return

vibration_en:
Low att
SHOut cmd,clk,lsbfirst_h,[$01,$4D,$00,$00,$01]
'SHIn dat,clk,lsbpost_h,[psxmode,psxbutton_1,psxbutton_2,psxjoystick_1,psxjoystick_2,psxjoystick_3,psxjoystick_4]
High att
Return

vibration_run:
Low att
SHOut cmd,clk,lsbfirst_h,[$01,$42,$00,S_motor,psxjoystick_1]
'look's like we don't need to get data in the config mode so the line underneath is not necessary!
'SHIn dat,clk,lsbpost_h,[psxmode,psxbutton_1,psxbutton_2,psxjoystick_1,psxjoystick_2,psxjoystick_3,psxjoystick_4]
High att
Return

analog_lock:
Low att
SHOut cmd,clk,lsbfirst_h,[$01,$44,$00,$01,$03]  '$03 lock the mode $01 (analog)
SHIn dat,clk,lsbpost_h,[psxmode,psxbutton_1,psxbutton_2,psxjoystick_1,psxjoystick_2,psxjoystick_3,psxjoystick_4]
High att
Return

digital_lock:
Low att
SHOut cmd,clk,lsbfirst_h,[$01,$44,$00,$00,$03]
'SHIn dat,clk,lsbpost_h,[,psxbutton_1,psxbutton_2,psxjoystick_1,psxjoystick_2,psxjoystick_3,psxjoystick_4]
High att
Return


exit_config_mode:
Low att
SHOut cmd,clk,lsbfirst_h,[$01,$43,$00,$00,$5A]
SHIn dat,clk,lsbpost_h,[psxmode,psxbutton_1,psxbutton_2,psxjoystick_1,psxjoystick_2,psxjoystick_3,psxjoystick_4]
High att
Return
End


il gere la manette playstation mais d'une maniere un peu bizarre, dans tous les cas ca fonctionne et je peux meme faire vibrer la manette (pas si evident d'apres internet)

tu a raisons Louky pour les librairies j'ai une tendance a faire comme si c'etait le compilateur lui meme

je serait interressé par des compilateur C ou autre avant pas mal de librairies ... (ca evite de refaire se qui est fait) je vais chercher ca sur le net!


pour les filtres de Kalman je ne pense pas en avoir besoin sur le ROV dans un premier temps...

mais j'ai un projet d'autopilote Heli qui necessite un filtre de kalman pour fusion les 3 gyros ,accelerometres, et 3 magnetometres

j'en suis pas la de toutes facons, et c'est assez vrai que le PID fonctionne bien, pour illustrer le principe je me suis fait un robot autobalancé (balancing bot) et ca marché du tonnerre (trop fendard)


sinon pour les news j'ai depioter mes varios et je vais les souder sur une carte au dessus de la carte de puissance mais je sais pas trop si je risque de parasiter ma carte de gestion (a pic bien sur), la place va me manquer sur mon engin si j'enpile pas les cartes... et j'ai quelques mesconnaissances dans la conception bien faite d'une carte.

je vais faire ca sur veroboard dans un premier temps puis en verra!

a bientot pour la suite
fefenin (exploboticien!)

retrouvez les infos locales de figeac sur :
http://www.telefigeac.com

Avatar de l’utilisateur
Louky
Messages : 899
Inscription : Sam Oct 22, 2005 6:33 am
Localisation : Var

Re: probleme de mixage 3 axes vers 4 moteurs?!

Messagepar Louky » Dim Nov 23, 2008 5:13 pm

Je vous que je ne suis pas le seul à m'ennuyer !!! :wink:

Pour ton héli, ça m'a fait penser à un lien qui pourrait t'intéresser :
http://serge.laforest.free.fr/drone/drone.htm


Revenir vers « Electronique et capteurs »

Qui est en ligne ?

Utilisateurs parcourant ce forum : Aucun utilisateur inscrit et 2 invités