'===============================================================
'PWM + KOMUTASI2FASA MOTOR KANAN VERSI 2 TIMER2  CCW
'===============================================================

$regfile "m16adef.dat"
$crystal = 16000000
$baud = 115200

'-- Timers init ----------------------------------------------------------------
'TMR2 use FAST PWM mode
 Tccr2.wgm20 = 1
 Tccr2.wgm21 = 1

 Tccr2.com20 = 0
 Tccr2.com21 = 1

Config Adc = Single , Prescaler = Auto , Reference = Avcc



Config Porta = Output
Config Portb = Output
Config Portc = Output
Config Pind.7 = Output
Config Portd.2 = Input
Portd.2 = 1

Hijau Alias Pina.5
Biru Alias Pina.6
Kuning Alias Pina.7

2fasa_step6_ccw Alias &B00100100
2fasa_step5_ccw Alias &B00100001
2fasa_step4_ccw Alias &B00001001
2fasa_step3_ccw Alias &B00011000
2fasa_step2_ccw Alias &B00010010
2fasa_step1_ccw Alias &B00000110


Dim Pot As Word
Dim Ta As Single
Dim Count As Integer
Dim Rotation As Single
Dim Rpm As Single
Dim Num_of_pole As Integer


Ta = 0.04
Num_of_pole = 20

'-- LCD config ------------------------------------------------------------------
Config Lcdbus = 4
Config Lcd = 16 * 2
Config Lcdpin = Pin , Db4 = Porta.0 , Db5 = Porta.1 , Db6 = Porta.2 , Db7 = Porta.3 , E = Porta.5 , Rs = Porta.4
Cls

Start Adc
Enable Interrupts

Do
   Config Int0 = Falling
   Enable Interrupts
   Enable Int0
   On Int0 Enkoder_hall


   Config Timer1 = Timer , Prescale = 256
   On Timer1 Regler
   Timer1 = 63035
   Enable Timer1
   Start Timer1

   Config Timer2 = Pwm , Pwm = On , Prescale = 64 , Compare Pwm = Clear Down
   Pot = Getadc(1)
   Ocr2 = Pot
   Gosub Komutasi2fasa_ccw
   Gosub Ceksensor_ccw
   Enable Timer2
   Start Timer2
   $sim

   Wait 4
   Print Rpm
Loop
End

Enkoder_hall:
   Incr Count
Return

Regler:
   Timer1 = 63035
   Rotation = Count / Ta
   Rpm = Rotation / Num_of_pole
   Count = 0
Return

Komutasi2fasa_ccw:
   If Hijau = 0 And Biru = 0 And Kuning = 1 Then Portb = 2fasa_step5_ccw
   If Hijau = 0 And Biru = 1 And Kuning = 1 Then Portb = 2fasa_step4_ccw
   If Hijau = 0 And Biru = 1 And Kuning = 0 Then Portb = 2fasa_step3_ccw
   If Hijau = 1 And Biru = 1 And Kuning = 0 Then Portb = 2fasa_step2_ccw
   If Hijau = 1 And Biru = 0 And Kuning = 0 Then Portb = 2fasa_step1_ccw
   If Hijau = 1 And Biru = 0 And Kuning = 1 Then Portb = 2fasa_step6_ccw
Return

Ceksensor_ccw:
   If Hijau = 0 And Biru = 0 And Kuning = 1 Then
      Portc.0 = 0
      Portc.2 = 0
      Portc.4 = 1
   End If
   If Hijau = 0 And Biru = 1 And Kuning = 1 Then
      Portc.0 = 0
      Portc.2 = 1
      Portc.4 = 1
   End If
   If Hijau = 0 And Biru = 1 And Kuning = 0 Then
      Portc.0 = 0
      Portc.2 = 1
      Portc.4 = 0
   End If
   If Hijau = 1 And Biru = 1 And Kuning = 0 Then
      Portc.0 = 1
      Portc.2 = 1
      Portc.4 = 0
   End If
   If Hijau = 1 And Biru = 0 And Kuning = 0 Then
      Portc.0 = 1
      Portc.2 = 0
      Portc.4 = 0
   End If
   If Hijau = 1 And Biru = 0 And Kuning = 1 Then
      Portc.0 = 1
      Portc.2 = 0
      Portc.4 = 1
   End If
Return