' PROJET PyRobot 2022-2023 . version 05-06-2022
' Programmé par HOAREAU BERTRAND bertrand.hoareau@gmail.com 
' https://host974.com/pyrobot/
' La carte communique avec le carte RASPBERRY PI 4
' Affichage sortie sur ecran du pc
' utilisez logiciel putty en configurant sur 19200 bauds ...

#chip 18f45k22, 16                 'Sélection du type de microcontrôleur
#define USART_BAUD_RATE 9600
#define USART_BLOCKING

#define SerInPort PORTc.7 'usart1 in
#define SerOutPort PORTc.6 'usart2 out


#Define Match_Val PR2
Match_Val = 50


Dir SerOutPort Out
Dir SerInPort In

Wait 100 Ms   ' delai d'attente pour initialiser probablement
Dim  compteur,compteur1,delta_t as Word
Dim capteur_g,capteur_d as word   ' capteur oeil gauche et droit


Dim t1,t2,t3,t4,pas1,pas2,pas3,pas4 as Byte
Dim compteur_t1 , compteur_t2, compteur_t3, compteur_t4, compteur_g,i1 as Byte


Dim valeur,lsb1 as Word   ' pour envoyer data


Dim c1,c2, rappd, rappg,flag,vitessed,vitesseg as Byte   ' reglage vitesse : rappd et rappg
Dim a0,a1,a2,a3,a4,a5,a6,a7 as Bit     ' bits pour la communication raspberry
Dim commande as Byte ' commande à exécuter

' Mesure de vitesse
Dim start_chrono, flag_commande as Byte
Dim val_chrono as Word

flag_commande = 0

INTCON.INTEDG0=1  ' int B0 sur front montant ?
INTCON.INTEDG1=1  '  int B0 sur front montant

Dir PORTA in    '
Dir PORTB in   '  B0 (GPIO 21)  et B1 (GPIO 20)  entrée d'interruptions (reception data)
Dir PORTD out  ' D0 (GPIO 16) et D1 (GPIO 12)  envoyer data   D6 : Bleu led RGB Gauche

 ' Pilotage des moteurs
Dir PORTC.0 out    ' Sens M1    (Moteur droit)
Dir PORTC.1 out    ' Vitesse M1
Dir PORTC.2 out    ' Sens M2     (Moteur Gauche)
Dir PORTC.3 out    ' Vitesse M2

Dir PORTC.4 out    'Rouge Led RGB gauche
Dir PORTC.5 out    'Vert  Led RGB gauche
                   'Bleu  PDRTD.6  led RGB Gauche

' Utiliser le timer 0 pour génerer des impulsions.
' Si on utilise le timer 1 , la lisaison série utilsée par HserPrint ... plante
' Avec le timer 2 , pas d'interruption sur overflow
InitTimer1 Osc, PS1_1/2    ' config pour interruption toutes les 16ms

'InitTimer3 Osc, PS1_1/1    ' pas de division , donc plus rapide normalement
'StartTimer 3

' Configuration pour mesurer valeur Timer
' super cool . equivalent TOC du 68hc11
Inittimer2 PS2_64, 15 'Prescale 1:64 /Postscale 1:16 (15)



CounterValue = 0
flag = 0

' Attention, les variables utilisées dans subroutines ne doivent
' pas etre initialisée ici sinon ca plante
'valeur = 24  ' valeur vers raspberry
'lsb1 = 0    ' associee pour envoi raspberry

wait 100 ms   ' ne pas enlever ...

HSerPrint " Projet PyRobot "   ' message
HSerPrintCRLF          ' passage à la ligne


' Sourcils entre 100 et 200
t1 = 150       ' 100 Valeur minimale
t2 = 150

' Tourelle camera entre 70 et 250
t3 = 150   ' gauche droite
t4 = 190   ' bas  haut


delta_t = 0

' a corriger dans les routines , melange sourcils
pas1 = 5         ' pas de deplacement servo sourcil
pas2 = 5         ' attention pas trop grand sinon débordement
pas3 = 5         ' pas deplacement servo tourelle
pas4 = 5         ' attention pas trop grand sinon débordement

c1 = 0
c2 = 0

vitessed = 2  ' vitesse si moteur droit on
vitesseg = 2  ' vitesse si moteur gauche on
rappd = 0    ' vitesse en cours moteur droit
rappg = 0    ' vitesse en cours moteur gauche
compteur = 0
compteur1 = 0

'PORTD.0 = 1   ' pour envoyer data vers raspberry
'PORTD.1 = 1   ' horloge
a0=0
a1=0
a2=0
a3=0
a4=0
commande = 0
autorise = 1

PORTC.0 = 0   ' sens Moteur droit
PORTC.1 = 0   ' pwm

PORTC.2 = 0    ' sens Moteur Gauche
PORTC.3 = 0    ' pwm

' Led RGB Gauche
PORTC.4 = 0    ' rouge
PORTC.5 = 0    ' vert
PORTD.6 = 0    ' Bleu

On Interrupt Timer1Overflow Call montimer   ' config routine interruption (toutes les 16ms)
StartTimer 1
'On Interrupt Timer3Overflow Call montimer3
' Super , je peux directement générer int sur une valeur du Timer
' Equivalent TOC du 68HC11
On interrupt Timer2Match call montimer2
Starttimer 2

'On Interrupt ExtInt0 Call IncCounter  ' interruption sur B0
' B0 est utilisée pour Data in, ne pas mettre interruption
' B1 synchro horloge , active interruption
On Interrupt ExtInt1 Call IncCounter1  ' interruption sur B1




Do


  if compteur1 >=8 and autorise=1 then

    ' HSerPrint compteur1

    ' HSerPrint " Decimale : "
    ' HSerPrint commande
    ' HSerPrint "  :  "

     if commande = 1 Then
       pas1 = 1
       tourelle_rotation_gauche
     end if
     if commande = 2 Then
       pas2 = 1
       tourelle_rotation_droite
     end if
     if commande = 3 Then
       pas3 = 1
       tourelle_rotation_haut
     end if
     if commande = 4 Then
       pas4 = 1
       tourelle_rotation_bas
     end if


     if commande = 5 Then
       pas1 = 5
       tourelle_rotation_gauche
     end if
     if commande = 6 Then
       pas2 = 5
       tourelle_rotation_droite
     end if
     if commande = 7 Then
       pas3 = 5
       tourelle_rotation_haut
     end if
     if commande = 8 Then
       pas4 = 5
       tourelle_rotation_bas
     end if

     ' Commande des moteurs de propulsion

     if commande  = 30 Then
        avance
     end if

     if commande = 31 Then
        recule
     end if
     if commande = 32 Then
        gauche
     end if
     if commande = 33 Then
        droite
     end if

     if commande = 34 Then
        moteur_stop
     end if



   ' Commandes de tests et debug
   ' ************************

     if commande = 10 Then
        envoyer(25)
     end if

     if commande = 11 Then
        capteur_g = readAD10(AN0)
        capteur_d = readAD10(AN1)
        HSerPrint " capteur oeil g et d : "
        HSerPrint capteur_g
        HSerPrint "  :  "
        HSerPrint capteur_d

        envoyer(capteur_g)
        wait 4 ms
        envoyer(capteur_d)
     end if

     if commande = 20 then
        start_chrono = 1   ' pour mesurer vitesse
        PORTC.5 = 1    ' vert
     end if

     if commande = 21 Then
        start_chrono = 0
       ' HSerPrint " Chrono : "
       ' HSerPrint val_chrono
        PORTC.5 = 0    ' vert
        val_chrono = 0
     end if

     if commande = 22 then
        if flag_commande = 0 Then
            start_chrono = 1   ' pour mesurer vitesse
            PORTC.5 = 1    ' vert
            flag_commande = 1
        Else
            start_chrono = 0
           ' HSerPrint " Chrono : "
           ' HSerPrint val_chrono
            PORTC.5 = 0    ' vert
            val_chrono = 0
            flag_commande = 0


        end if

     end if
   ' *************************
   ' fin zone de test et debug

     compteur1=0
     commande = 0
    ' HSerPrintCRLF
    ' Wait 10 ms

  end if


  if start_chrono = 1 Then
     val_chrono = val_chrono + 1
  end if


Loop



sub avance
    PORTC.0 = 1    '1 : sens avant moteur droit
    PORTC.2 = 0    '0 : sens avant moteur gauche
    moteur_go
end Sub

sub recule
    PORTC.0 = 0    '0 : sens arriere moteur droit
    PORTC.2 = 1    '1 : sens arriere moteur gauche
    moteur_go
end Sub

sub droite
    PORTC.0 = 0    '0 : sens arriere moteur droit
    PORTC.2 = 0    '0 : sens avant moteur gauche
    moteur_go
end Sub

sub gauche
    PORTC.0 = 1    '1 : sens avant moteur droit
    PORTC.2 = 1    '1 : sens arriere moteur gauche
    moteur_go
end Sub

sub moteur_stop
   rappd = 0
   rappg = 0
End Sub

sub moteur_go
    rappd = vitessed
    rappg = vitesseg
End Sub

'
sub envoyer(valeur)

  wait 4 ms ' laisser temps raspberry pour s'initialiser
  'valeur = 25
  PORTD.1 = 0

  for i=1 to 9

    lsb1 = valeur & 1
    valeur = FnLSR(valeur,1)
    PORTD.0 = lsb1

    PORTD.1 = 1
    wait 15 ms
    PORTD.1 = 0
    wait 10 ms
  next

end sub



sub tourelle_rotation_gauche

   t3 = t3+pas1
   if t3>240 Then
      t3=240
   end if
End sub

sub tourelle_rotation_droite
   t3 = t3-pas2
   if t3<70 Then
      t3=70
   end if
End sub

sub tourelle_rotation_haut
   t4 = t4-pas3
   if t4< 70 Then
      t4=70
   end if
End sub

sub tourelle_rotation_bas
   t4 = t4+pas4
   if t4>240 Then
      t4=240
   end if
End sub



Sub IncCounter
  compteur = compteur +1

 ' PORTB.7 = 1
 ' Wait t2 us
 ' PORTB.7 = 0


 ' Test de reconnaissance de variables par cowbasic
 INTCON.INT0IF = 0  ' clear the interrupt flag
 ' INTCON.INT1IF = 0  '
 'INTCON.INTEDG0=1   ' Régler interruption B0 sur front montant
  'INTCON.INTEDG1=1  ' Régler interruption B0 sur front montant
End Sub

Sub IncCounter1
  compteur1 = compteur1 +1

  if compteur1 < 9 then
    a0=PORTB.0
    if a0=0 then    ' on a recu un 1, inversion transistor

      commande = FnLSR(commande,1)   ' décalage à droite
      commande = commande | 128
    '  commande = 22
    Else
      commande = FnLSR(commande,1)   ' décalage à droite
    '   commande = 22
    end if

  end if




  INTCON.INT1IF = 0  ' clear the interrupt flag
End Sub

sub montimer   ' Impulsion pour les servos

  compteur_t1 = t1   ' 100: sourcil droit vertical
  compteur_t2 = t2   ' 100  à  200

  compteur_t3 = t3    ' 70 à 250 pour servo tourelle
  compteur_t4 = t4


  PORTD.2 = 1      ' Sourcils
  PORTD.3 = 1

  PORTD.4 = 1      ' impulsion servo tourelle rotation
  PORTD.5 = 1      ' servo tourelle haut - bas


  for compteur_g = 0 to 255

     compteur_t1 = compteur_t1 - 1
     if compteur_t1 = 0 Then
        PORTD.2 = 0
     end if

     compteur_t2 = compteur_t2 - 1
     if compteur_t2 = 0 Then
        PORTD.3 = 0
     end if


     compteur_t3 = compteur_t3 - 1
     if compteur_t3 = 0 Then
        PORTD.4 = 0

     end if


      compteur_t4 = compteur_t4 - 1
      if compteur_t4 = 0 Then
         PORTD.5 = 0

      end if

     wait 12 us  ' 12us pour 4 servo , à ajuster en fonction du nb de servo

  Next

  PORTD.2 = 0
  PORTD.3 = 0
  PORTD.4 = 0
  PORTD.5 = 0
end sub

sub montimer2

  c1 = c1 + 1 ' comptage toutes les 16ms

  if c1 > 10 Then
     c1 = 0
  end if

  if c1 < rappd Then
     PORTC.1 = 1     ' moteur droit pwm
  Else
     PORTC.1 = 0
  end if


  c2 = c2 + 1

  if c2 > 10 Then
     c2 = 0
  end if

  if c2 < rappg Then
     PORTC.3 = 1     ' moteur gauche pwm
  Else
     PORTC.3 = 0
  end if


End Sub