Diy servo driver (microcontroller)

 

 

DIYdriverEagleFiles.zip <- click to download Eagle files

Files is in free Cadsoft Eagle

List of parts:

 - Attiny44 or Attiny84

 - 7805 Voltage regulator

 - 2 x 100nF capacitor

 -  4 x 33pF capacitors (not needed)

 - 10K resistor

 - few cennectors

and... VNH2SP30 dual shield for 2 motors controll

Eagle files (microcontroler shield) are made to connect it to VNH2SP30 board

Here is the BAS file for free BASCOM program (need to modify it to I2C controll)

Just made PCB, solder few elements, program and use...


'-----------------------------------------------------------------------------------------
'name                     : servo DIY Driver
'copyright                : Bartosz Scencelek Bartcam
'micro                    : attiny44
'commercial use           : no
'-----------------------------------------------------------------------------------------

$regfile = "attiny44.dat"                                   ' specify the used micro
$crystal = 8000000                                          ' used crystal frequency


Dim Pot1 As Word
Dim Pot2 As Word
Dim Pot11 As Word
Dim Pot22 As Word
Dim X As Byte
Dim Flaga1 As Bit
Dim Flaga2 As Bit
Dim Fp1 As Word
Dim Fp2 As Word
Dim Stop1 As Word
Dim Stop2 As Word

Config Portb.0 = Output                                     'inb1
Config Portb.1 = Output                                     'ina1
Config Portb.2 = Output                                     'ina2
Config Porta.0 = Output                                     'inb2
Config Porta.7 = Output                                     'oc0b-pwm1
Config Porta.6 = Output                                     'oc1a-pwm2

Config Adc = Single , Prescaler = 2
Config Timer0 = Pwm , Compare_b_pwm = Clear_up , Prescale = 8
Config Timer1 = Pwm , Compare_a_pwm = Clear_up , Prescale = 8


Enable Interrupts
Start Timer0
Start Timer1

Ocr0b = 0                                                   'silnik1
Ocr1a = 0                                                   'silnik2

Reset Portb.0                                               'silnik 1
Reset Portb.1                                               'silnik 1
Reset Portb.2                                               'silnik 2
Reset Porta.0                                               'silnik 2
Flaga1 = 0
Flaga2 = 0
Fp1 = 0
Fp2 = 0
Stop1 = 0
Stop2 = 0

Waitms 200

Do

'******************pomiar adc potencjometrów (get pot values)*****
Pot1 = 0
Pot2 = 0
For X = 0 To 10
Pot11 = Getadc(1)
Pot22 = Getadc(2)
'***************usrednienie wartosci (filtering)*************
Pot1 = Pot1 + Pot11
Pot2 = Pot2 + Pot22
Next X
Pot1 = Pot1 / 10
Pot2 = Pot2 / 10
Pot11 = 0
Pot22 = 0

'*************waiting for any change of ADC to start control******
If Flaga1 = 0 Then

If Fp1 = 0 Then Fp1 = Pot1
If Fp1 > 0 Then
   Pot11 = Pot1 + 5
   If Fp1 > Pot11 Then Flaga1 = 1
   Pot11 = Pot1 - 10
   If Fp1 < Pot11 Then Flaga1 = 1
End If
End If
'End If

If Flaga2 = 0 Then

If Fp2 = 0 Then Fp2 = Pot2
If Fp2 > 0 Then
   Pot22 = Pot2 + 5
   If Fp2 > Pot22 Then Flaga2 = 1
   Pot22 = Pot2 - 10
   If Fp2 < Pot22 Then Flaga2 = 1
End If
End If
'End If
'***************sterowanie silnikami (motor controll)********

If Flaga2 = 1 Then
If Pot2 > 513 Then
  Ocr0b = 0
  If Pinb.0 = 0 Then
'  If Ocr0b > 50 Then
'  Decr Ocr0b
'  Else
  Set Portb.0
'  End If
  End If

  If Pinb.1 = 1 Then Reset Portb.1

  Stop2 = 0
  Stop2 = Pot2 - 512
  Stop2 = Stop2 / 2
  If Stop2 < 20 Then Stop2 = 20
  If Stop2 > 200 Then Stop2 = 200

  Ocr0b = Stop2

'  If Pot2 > 513 And Pot2 < 541 Then Ocr0b = 30
'  If Pot2 > 540 And Pot2 < 561 Then Ocr0b = 35
'  If Pot2 > 560 And Pot2 < 581 Then Ocr0b = 40
'  If Pot2 > 580 And Pot2 < 601 Then Ocr0b = 50
'  If Pot2 > 600 And Ocr0b < 250 Then Ocr0b = Ocr0b + 25


Elseif Pot2 > 510 And Pot2 < 514 Then

Flaga2 = 0
Ocr1b = 0
'Flaga2 = 0
'  Do
'  If Ocr0b > 0 And Ocr0b < 50 Then Ocr0b = Ocr0b - 25
'  If Ocr0b > 49 Then Ocr0b = Ocr0b - 50
    If Pinb.0 = 0 Then Set Portb.0
    If Pinb.1 = 0 Then Set Portb.1
    Ocr0b = 50
'  Loop Until Ocr0b = 0

Elseif Pot2 < 511 Then
  Ocr0b = 0
  If Pinb.1 = 0 Then
'  If Ocr0b > 50 Then
'  Decr Ocr0b
'  Else
  Set Portb.1
'  End If
  End If

  If Pinb.0 = 1 Then Reset Portb.0

  Stop2 = 0
  Stop2 = 510 - Pot2
  Stop2 = Stop2 / 2
  If Stop2 < 20 Then Stop2 = 20
  If Stop2 > 200 Then Stop2 = 200
  Ocr0b = Stop2

'  If Pot2 < 511 And Pot2 > 483 Then Ocr0b = 30
'  If Pot2 < 484 And Pot2 > 463 Then Ocr0b = 35
'  If Pot2 < 464 And Pot2 > 443 Then Ocr0b = 40
'  If Pot2 < 444 And Pot2 > 423 Then Ocr0b = 50
'  If Pot2 < 424 And Ocr0b < 250 Then Ocr0b = Ocr0b + 25

End If
End If

If Flaga1 = 1 Then
If Pot1 > 513 Then
  Ocr1a = 0
  If Pina.0 = 0 Then
'  If Ocr0b > 50 Then
'  Decr Ocr0b
'  Else
  Set Porta.0
'  End If
  End If

  If Pinb.2 = 1 Then Reset Portb.2

  Stop1 = 0
  Stop1 = Pot1 - 512
  Stop1 = Stop1 / 2
  If Stop1 < 20 Then Stop1 = 20
  If Stop1 > 200 Then Stop1 = 200
  Ocr1a = Stop1

'  If Pot1 > 513 And Pot1 < 541 Then Ocr1a = 30
'  If Pot1 > 540 And Pot1 < 561 Then Ocr1a = 35
'  If Pot1 > 560 And Pot1 < 581 Then Ocr1a = 40
'  If Pot1 > 580 And Pot1 < 601 Then Ocr1a = 50
'  If Pot1 > 600 And Ocr1a < 250 Then Ocr1a = Ocr1a + 25

Elseif Pot1 > 510 And Pot1 < 514 Then

'Flaga1 = 0

'  Do
  Ocr1a = 0
  Flaga1 = 0
    If Pina.0 = 0 Then Set Porta.0
    If Pinb.2 = 0 Then Set Portb.2
  Ocr1a = 50
'  Loop Until Ocr1a = 0

Elseif Pot1 < 511 Then
  Ocr1a = 0
  If Pinb.2 = 0 Then
'  If Ocr0b > 50 Then
'  Decr Ocr0b
'  Else
  Set Portb.2
'  End If
  End If

  If Pina.0 = 1 Then Reset Porta.0

  Stop1 = 0
  Stop1 = 510 - Pot1
  Stop1 = Stop1 / 2
  If Stop1 < 20 Then Stop1 = 20
  If Stop1 > 200 Then Stop1 = 200
  Ocr1a = Stop1

'  If Pot1 < 511 And Pot1 > 483 Then Ocr1a = 30
'  If Pot1 < 484 And Pot1 > 463 Then Ocr1a = 35
'  If Pot1 < 464 And Pot1 > 443 Then Ocr1a = 40
'  If Pot1 < 444 And Pot1 > 423 Then Ocr1a = 50
'  If Pot1 < 424 And Ocr1a < 250 Then Ocr1a = Ocr1a + 25

End If
End If
Loop

 

bartcam's picture

solutions of sterring

first option

second option

 

and this is what i have now, only for testing manualy all mechanism (soft is for this and need to write it to other solutions) , most importand is the dynamics of motor stering. Manually I check all solutions of motors dynamics. Motors must be fast but with correct acceleration and braking (manually may check all of the solutions)

Ray.Edgley's picture

Bartcam as Discussed

This is the start of the proposed pin usage
 
'Port      Pin     Function Used
 'PB0       2       Motor 1 Fwd
 'PB1       3       Motor 1 Rev
 'PB2       5       OC0A    PWM Motor 1
 'PB3       4       Motor 2 Fwd
 'PA0       13     Motor 2 Rev
 'PA1       12      ADC1    Motor 1 Position
 'PA2       11      ACD2    Motor 2 Position
 'PA3       10
 'PA4       9       SCL     I2C Communications
 'PA5       8
 'PA6       7       SDA     I2C Communications
 'PA7       6       OC0B    PWM Motor 2
 
Ray.Edgley's picture

Try this out Bartcam

Hello Bart,

Try out the following code.

it uses over 80% of the Flash memory but does have PID control with a choice of Analog control or PWM input like an RC servo.  Output is dual cahnnel PWM.

'-----------------------------------------------------------------------------------------
'name                     : servo DIY Driver
'copyright                : Bartosz Scencelek Bartcam
'micro                    : attiny44
'commercial use           : no
'-----------------------------------------------------------------------------------------
 
$regfile = "attiny44.dat"                                   ' specify the used micro
$crystal = 8000000                                          ' used crystal frequency
 
'The following is just pinout information
'Port      Pin     Function Used
'PB0       2       Output     Motor 1 Fwd
'PB1       3       Output     Motor 1 Rev
'PB2       5       OC0A       PWM Motor 1
'PB3       4       Reset
'PA0       13      Output     Motor 2 Rev
'PA1       12      ADC1       Motor 1 Position
'PA2       11      ACD2       Motor 2 Position
'PA3       10      Output     Motor 2 Fwd
'PA4       9       SCL/Ref1   I2C Communications
'PA5       8       Input      Control Mode
'PA6       7       SDA/Ref2   I2C Communications
'PA7       6       OC0B       PWM Motor 2
'############################################
' Just a note on the Control Mode
'
' When set to VCC we will use the PA4 and PA6
' as I2C communications
'
' When set to Gnd    We will use the PA4 and PA6
' as PWM inputs for Ref1 and Ref2
'
' When set to 2.5V or ADC 512 we will use PA4 and PA6
' as Analog inputs for Ref1 and Ref2
'############################################
Dim Pot1 As Single                                          'Current Average position value Motor 1
Dim Pot2 As Single                                          'Current Average position value Motor 2
Dim Ref1 As Single                                          'Motor 1 Target Position
Dim Ref2 As Single                                          'Motor 2 Target Position
Dim Refcount1 As Word
Dim Refcount2 As Word
Dim Err1 As Single
Dim Err2 As Single
Dim Integral1 As Single
Dim Integral2 As Single
Dim Derivative1 As Single
Dim Derivative2 As Single
Dim Preverr1 As Single
Dim Preverr2 As Single
Dim Output1 As Single
Dim Output2 As Single
Dim Kp1 As Single
Dim Ki1 As Single
Dim Kd1 As Single
Dim Kp2 As Single
Dim Ki2 As Single
Dim Kd2 As Single
Dim Deadband1 As Single                                     'Motor 1 DeadBand
Dim Deadband2 As Single                                     'Motor 2 DeadBand
Dim Filter As Single                                        'Filtering Value
Dim Filter2 As Single
'Dim Temp As String * 10
Dim Temp2 As Single
Dim Temp3 As Word
Dim Dt As Single
Dim Controltype As Bit
Dim Pwm_control As Byte
 
On Pcint0 Chint Save
On Oc1a Pwmint Save
 
Config Portb.0 = Output                                     'Motor1 Fwd
Config Portb.1 = Output                                     'Motor1 Rev
Config Portb.2 = Output                                     'Motor1 PWM
Config Porta.0 = Output                                     'Motor2 Rev
Config Porta.3 = Output                                     'Motor2 Fwd
Config Porta.7 = Output                                     'Motor2 PWM
Config Porta.5 = Input                                      'input Mode "Cotrol"
 
Config Adc = Single , Prescaler = 2
Config Timer0 = Pwm , Compare_a_pwm = Clear_up , Compare_b_pwm = Clear_up , Prescale = 8
Config Timer1 = Timer , Compare_a = Disconnect , Compare_b = Disconnect , Prescale = 8
 
Enable Interrupts
 
Ocr0b = 0                                                   'Motor1 Power
Ocr0a = 0                                                   'Motor2 Power
' the OCR1A (Output Compare Register 1 A) is used to generate an interruput every
' 20 mS which is the pulse period of most PWM controlled servos.
' Since this controller will be working as a servo driver, we need to use
' the same timings.
' 20mS periods occure 50 times per second
' With a system clock of 8000000 cycles persecond, 160000 cycles will elapse each period.
' Timer/Counter 1 can coun up to 65535 before it over runs, so we need to use a prescaler
' of at least 2.441 times. This of course does not exist, so the next lowest available
' is 8 time. Soo if we take 8000000/8 gives us 1000000. we divide that by our 50 times
' per second, we get 20000, which is below the max of 65535, so we set the OCR1A to 20000
' if a PWM pulse exceeds the 20 mS, then we will set the ref to the current pos.
Ocr1a = 20000                                               'PWM input Timer
 
Reset Portb.0                                               'Motor1 Fwd
Reset Portb.1                                               'Motor1 Rev
Reset Porta.0                                               'Motor2 Rev
Reset Porta.3                                               'Motor2 Fwd
Controltype = Porta.5
 
 
Start Timer0                                                'PWM Output Timer
 
If Controltype = 1 Then                                     'This section for PWM Input
   Set Pcie0
   Enable Oc1a
   Start Timer1
   Tcnt1 = 0
   Pwm_control = 0
   Set Pcint4
End If
 
Ref1 = 512
Ref2 = 512
Integral1 = 0
Integral2 = 0
Kp1 = 0.1
Ki1 = 0.1
Kd1 = 0
Deadband1 = 5
Deadband2 = 5
Filter = 5                                                  ' Set the filter to 5 Cycles
Filter2 = Filter + 1
Dt = 0.00173
Waitms 200
 
 
 
Do
 
'*************** Motor Feed Back with Filtering *************
' By taking the existing pot value and multiplying it by Filter
' then adding the raw pot value and deviding the total by Filter + 1
' We end up with a sudo average taken over filter cycles through the program
' The best part of this method of averaging is it is fast and doen't
' used much memory, the down side is it can take a long time for the average
' to match the raw when the raw is not moving.
    Temp3 = Getadc(1)
    Temp2 = Temp3
    Pot1 = Pot1 * Filter
    Pot1 = Pot1 + Temp2
    Pot1 = Pot1 / Filter2
 
    Temp3 = Getadc(2)
    Temp2 = Temp3
    Pot2 = Pot2 * Filter
    Pot2 = Pot2 + Temp2
    Pot2 = Pot2 / Filter2
 
'############################################################
' Control Method 2 Analog input control.
'############################################################
    If Controltype = 0 Then                                 ' Analog Input Control
         Temp3 = Getadc(4)
         Temp2 = Temp3
         Ref1 = Ref1 * Filter
         Ref1 = Ref1 + Temp2
         Ref1 = Ref1 / Filter2
 
         Temp3 = Getadc(6)
         Temp2 = Temp3
         Ref2 = Ref2 * Filter
         Ref2 = Ref2 + Temp2
         Ref2 = Ref2 / Filter2
    Else                                                    ' PWM input Control
         If Refcount1 < 1000 Or Refcount1 > 2000 Then       ' if the Refcount is not in range
            Ref1 = Pot1                                     'set the Ref to be equal to the current position
         Else
            Temp3 = Refcount1 - 988
            Ref1 = Temp3
         End If
         If Refcount2 < 1000 Or Refcount2 > 2000 Then
            Ref2 = Pot2
         Else
            Temp3 = Refcount2 - 988
            Ref2 = Temp3
         End If
    End If
 
'############################################################
' PID Loop to calculate the output value from the Input Error
'############################################################
 
    Err1 = Ref1 - Pot1
    Temp2 = Err1 * Dt
    Integral1 = Integral1 + Temp2
    Temp2 = Err1 - Preverr1
    Derivative1 = Temp2 / Dt
    Derivative1 = Derivative1 * Kd1
    Temp2 = Integral1 * Ki1
    Output1 = Kp1 * Err1
    Output1 = Output1 + Temp2
    Output1 = Output1 + Derivative1
    Preverr1 = Err1
    If Output1 > 255 Then
         Output1 = 255
    Elseif Output1 < -255 Then
         Output1 = -255
    End If
 
    Err2 = Ref2 - Pot2
    Temp2 = Err2 * Dt
    Integral2 = Integral2 + Temp2
    Temp2 = Err2 - Preverr2
    Derivative2 = Temp2 / Dt
    Derivative2 = Derivative2 * Kd2
    Temp2 = Integral2 * Ki2
    Output2 = Kp2 * Err2
    Output2 = Output2 + Temp2
    Output2 = Output2 + Derivative2
    Preverr2 = Err2
 
'############################################################
' Drive the Motor Output and PWM signal.
'############################################################
    If Output1 > Deadband1 Then                             ' Is the motor to far along
         Set Portb.0                                        ' Turn on the Fwd output
         Reset Portb.1                                      ' Turn off the Rev output
         Ocr0a = Output1                                    ' Set the PWM to Output
    Else
      Output1 = Output1 * -1
      If Output1 > Deadband1 Then                           ' Is the motor not far enough along
         Reset Portb.0                                      ' Turn off the Fwd output
         Set Portb.1                                        ' Turn on the Rev output
         Ocr0a = Output1                                    ' Set the PWM to Output
      Else
         Reset Portb.0                                      ' Turn off the Fwd output
         Reset Portb.1                                      ' Turn off the Rev output
         Ocr0a = 0                                          ' Turn Off the PWM
      End If
    End If
 
    If Output2 > Deadband2 Then                             ' Is the motor to far along
         Set Porta.3                                        ' Turn on the Fwd output
         Reset Porta.0                                      ' Turn off the Rev output
         Ocr0b = Output2                                    ' Set the PWM to Output
    Else
      Output2 = Output2 * -1
      If Output2 > Deadband2 Then                           ' Is the motor not far enough along
         Reset Porta.3                                      ' Turn off the Fwd output
         Set Porta.0                                        ' Turn on the Rev output
         Ocr0b = Output2                                    ' Set the PWM to Output
      Else
         Reset Porta.3                                      ' Turn off the Fwd output
         Reset Porta.0                                      ' Turn off the Rev output
         Ocr0b = 0                                          ' Turn Off the PWM
      End If
    End If
 
 
Loop
 
Chint:
   If Pwm_control = 0 Then
      If Porta.4 = 0 Then
         Tcnt1 = 0
         Pwm_control = 1
      End If
   Elseif Pwm_control = 1 Then
      If Porta.4 = 1 Then
         Refcount1 = Tcnt1
         Tcnt1 = 0
         Reset Pcmsk0.4
         Set Pcint6
         Pwm_control = 2
      End If
   Elseif Pwm_control = 2 Then
      If Porta.6 = 0 Then
         Tcnt1 = 0
         Pwm_control = 3
      End If
   Elseif Pwm_control = 3 Then
      If Porta.4 = 1 Then
         Refcount2 = Tcnt1
         Tcnt1 = 0
         Reset Pcmsk0.6
         Set Pcint4
         Pwm_control = 0
      End If
   End If
Return
 
 
Pwmint:
   If Pwm_control = 0 Or Pwm_control = 1 Then
      Reset Pcmsk0.4
      Set Pcint6
      Pwm_control = 2
      Refcount1 = 0
      Tcnt1 = 0
   Elseif Pwm_control = 2 Or Pwm_control = 3 Then
      Reset Pcmsk0.6
      Set Pcint4
      Pwm_control = 0
      Refcount2 = 0
      Tcnt1 = 0
   End If
Return
------------------------------------------------------------------------------------------------------------------------------------
Please let me know how it goes.
 
It seams the I2C library is only available in the comercial paid version of Bascom and I don't think it's worth it.
 
Ray
         
bartcam's picture

OK, i modify PCB and check

OK, i modify PCB and check this out. I read all the code and it seems to be Very OK. I am impressed Ray!

Ray.Edgley's picture

Do let me know how it

Do let me know how it goes.

There are ome serious limitations with this chip, and the programming software.

But it will be interesting to know if it works.

 

Ray

bartcam's picture

Chip may be changed to other

Chip may be changed to other like atmega or xmega..

GroG's picture

Wow Bartcam ! Such a great

Wow Bartcam !
Such a great post with your excellent mobile platform solutions.
Quality work - thanks for posting.

bartcam's picture

PB3 - is the reset pin of

Thank You Grog, and thank You Ray...

PB3 - is the reset pin of microcontroller (if I swich off this i can not program it)

ok i try to project new PCB for those, I must think about how to comunicate by I2C without any breaks in ADC, on int, we must do some sub funcion where we get signals from I2C.

We have one pin free, this may be used for some endstop switch, better will be to use two pins , one for each motor..

bartcam's picture

problem with filtering

There is problem with filtering in program, if I use

 


output1=ref1-pot1


 

without this filtering:


    Err1 = Ref1 - Pot1
    Temp2 = Err1 * Dt
    Integral1 = Integral1 + Temp2
    Temp2 = Err1 - Preverr1
    Derivative1 = Temp2 / Dt
    Derivative1 = Derivative1 * Kd1
    Temp2 = Integral1 * Ki1
    Output1 = Kp1 * Err1
    Output1 = Output1 + Temp2
    Output1 = Output1 + Derivative1
    Preverr1 = Err1

with this filtering all works in only one direction, I belive there is some small mistake in calculations. I try to find this ;/

 

all it's working in analog mode, I don't checked PWM and I2C mode yet.

Sorry no I2C mode ... yet!

Ok, i find one problem:


Derivative1 = Derivative1 * Kd1

 


Derivative1 will be always =0 because Kd1 is =0

 

hmmm filter is working in excel maybe is something with variables in bascom

 

bartcam's picture

errors in simulation

that I am think about in  simulations in bascom of filtering is many errors.
I cutted the code and do a small of it to simulation only:
 

$regfile = "attiny44.dat"                                   ' specify the used micro
$crystal = 8000000                                          ' used crystal frequency
 
'The following is just pinout information
'Port      Pin     Function Used
'PB0       2       Output     Motor 1 Fwd
'PB1       3       Output     Motor 1 Rev
'PB2       5       OC0A       PWM Motor 1
'PB3       4       Reset
'PA0       13      Output     Motor 2 Rev
'PA1       12      ADC1       Motor 1 Position
'PA2       11      ACD2       Motor 2 Position
'PA3       10      Output     Motor 2 Fwd
'PA4       9       SCL/Ref1   I2C Communications
'PA5       8       Input      Control Mode
'PA6       7       SDA/Ref2   I2C Communications
'PA7       6       OC0B       PWM Motor 2
'############################################
' Just a note on the Control Mode
'
' When set to VCC we will use the PA4 and PA6
' as I2C communications
'
' When set to Gnd    We will use the PA4 and PA6
' as PWM inputs for Ref1 and Ref2
'
' When set to 2.5V or ADC 512 we will use PA4 and PA6
' as Analog inputs for Ref1 and Ref2
'############################################
Dim Pot1 As Single                                          'Current Average position value Motor 1
Dim Pot2 As Single                                          'Current Average position value Motor 2
Dim Ref1 As Single                                          'Motor 1 Target Position
Dim Ref2 As Single                                          'Motor 2 Target Position
Dim Refcount1 As Word
Dim Refcount2 As Word
Dim Err1 As Single
Dim Err2 As Single
Dim Integral1 As Single
Dim Integral2 As Single
Dim Derivative1 As Single
Dim Derivative2 As Single
Dim Preverr1 As Single
Dim Preverr2 As Single
Dim Output1 As Single
Dim Output2 As Single
Dim Kp1 As Single
Dim Ki1 As Single
Dim Kd1 As Single
Dim Kp2 As Single
Dim Ki2 As Single
Dim Kd2 As Single
Dim Deadband1 As Single                                     'Motor 1 DeadBand
Dim Deadband2 As Single                                     'Motor 2 DeadBand
Dim Filter As Single                                        'Filtering Value
Dim Filter2 As Single
'Dim Temp As String * 10
Dim Temp2 As Single
Dim Temp3 As Word
Dim Dt As Single
Dim X As Single
 
 
Ref1 = 512
Ref2 = 512
Integral1 = 0
Integral2 = 0
Kp1 = 0.1
Ki1 = 0.1
Kd1 = 0.1
Deadband1 = 5
Deadband2 = 5
Filter = 5                                                  ' Set the filter to 5 Cycles
Filter2 = 6
Dt = 0.00173
Pot1 = 510
Ref1 = 512
 
Do
 
    Err1 = Ref1 - Pot1
    Temp2 = Err1 * Dt
    Integral1 = Integral1 + Temp2
    Temp2 = Err1 - Preverr1
    Derivative1 = Temp2 / Dt
    Derivative1 = Derivative1 * Kd1
    Temp2 = Integral1 * Ki1
    Output1 = Kp1 * Err1
    Output1 = Output1 + Temp2
    Output1 = Output1 + Derivative1
    Preverr1 = Err1
    X = Output1
 
    '
 
 
Loop
 
 
 
 
 
 
The X is calculation of what is end value to output. I don't know where is erroe in calculations, must check each by each
Ray.Edgley's picture

Is it the interrupts?

Is it possible the error is comming from an external source such as the two timer interrupts?

There are quite a number of interrupts on the tiny84, i used one for the two pwm outputs, and one is usec for the pwm input timing!

 

bartcam's picture

At last work on analog mode

At last work on analog mode with PID :)

but, no live with PWM mode, I connected normal servo to check signal from the arduino and normal servo its working , an these same pin DIY SERV DRV not work. I thin there is something with INT