PID Motor Control by G.De Luca
( delucagiovanni at interfree dot it )
Description
of the board:
The board
has been from me realized for moving contemporarily and
independent
way two motors brushed with relative encoder.
The
profiles with which it is possible to let move the motors are: control of
position, control position with trapezoidal profile and continuous control of
speed.
To move
the motor (one) in control of position we for example use the command:
GO 1
+100000
To move
the motor (one) in trapezoidal control with a maximum speed of 125 we use
the command:
POS 1
+100000 125
To move
the two motors contemporarily in two different positions:
MOVE
+50000 -50000
To change
the values of the PID of the motor one: [kp] [ki] [kd]
SPID 1
500 10 500
To let
move the motors contemporarily with two different speeds:
VELO
100 10
To change
the acceleration of the motor two with inclination of 30 degrees:
SACC 2
30.
To read
the position from the encoder one:
ENC
1
It is
important that when the circuit is in power on, the motor, it doesn't
move.
If this
happens, it is necessary to reverse the connections of the motor.
Schematic of the board:
Source program for BASCOM: '///////////////////////////////////////////////////////////////////////////////
'///////////////////////////////////////////////////////////////////////////////
'
' PROGRAMMA PER IL
CONTROLLO PID DI due MOTORI CON hp ENCODER HEDS-5540 100 IMPULSI GIRO
' data 28-05-2007
versione BASCOM 1.8.11.3
' la seriale in
questa versione non interferisce con il timer 0-1
' poichč la
trasmissione della posizione viene eseguita alla fine del
' ciclo PID
all'interno del ciclo DO-LOOP MAIN
'
' GIOVANNI DE LUCA
' HTTP://delucagiovanni.interfree.it robot applications
' HTTP://WWW.LNS.INFN.IT nuclear phisics
applications
' HTTP://www.youtube.com/tubincolo my video mobilerobots
' deluca@lns.infn.it e-mail
'
'///////////////////////////////////////////////////////////////////////////////
'///////////////////////////////////////////////////////////////////////////////
'
' Input Format Command:
' Command [par1] [par2] [par3] Cr
'
' Out Format Command:
' [mot] [command], [par1]
'
' Input Commands From Windows Terminal [ 115200 , 8 , N ,
1 ]
' No Case Sentitive
'.................................................................................................................
' Rstf RESET CPU
WITH WATCHDOG START
' Made WHO
WROTE PROGRAMM GIOVANNI DE LUCA
' Ver VERSION OF PROGRAMM
' Skp [mot] [kp
Value] SET PROPORZIONAL PID 0-1000 MAX
' Ski [mot] [ki
Value] SET
INTEGRAL PID 0-100 MAX
' Skd [mot] [kd
Value] SET
DERIVATIVE PID 0-1000 MAX
' Vmax [mot] [vmax Value] SET MAX VELOCITY IN
PROPORZIONAL VELOCITY PROFILE 0-255
' Svm [mot] [svm
Value] SET MAX
VELOCITY IN TRAPEZOIDAL MODE 0-255
' Spid [mot] [kp] [ki] [kd] SET PID MOTOR KP KI KD
' Stime[stime Value] SET PID EXECUTION
TIME 0-10
' Smode[mot] [mode] SET SINGLE MODE FOR
ONE MOTOR
' Svel [mot] [svel Value] SET MOTOR_ON WITH INTEGRAL
VELOCITY PROFILE AND MOVE 0-255
' Velo [vel 1] [vel 2] SET TWO INDIPENDENT
SPEED FOR MOTORS 0-255 0-255
' Spwm [mot] [spwm Value] SET MAX PWM OUT FOR TORQUE
LIMIT 0-255
' Sacc [mot] [acc Value] SET ACCELERATION DEGREE
1-89 DEGREES
' Sang [mot] [angular Factor] SET ANGULAR FACTOR FOR
VELOCITY PROFILE 1-100
' Go [mot]
[position Value] GO TO
POSITION IN POSITION MODE CONTROL +- 23 BITS
' Pos [mot]
[position Value] [speed] SET
POSITION IN TRAPEZOIDAL PROFILE 0-255
' Move [position 1] [position 2] MOVE MOTORS TO POS 1 POS 2 +-23 BITS TRAPEZOIDAL MODE
' Velp [position 1] [position 2] MOVE CONTINUOS POSITION WITH
VARIBLE PROPORZIONAL VELOCITY
' Start START
MOVE IF START_MOVE = FALSE
' Stop STOP
MOVE AND MOTORS POWER DOWN
' Gmax [mot] GET MAX
VELOCITY IN VELOCITY PROFILE MODE
' Gvm [mot] GET MAX
VELOCITY IN TRAPEZOIDAL MODE
' Gve [mot] GET ABSOLUTE
SPEED FROM ENCODER
' Enc [mot] READ
ABSOLUTE ENCODER POSITION
' Gpid [mot] GET PID MOTOR
VALUES
' Gtime GET PID EXECUTION TIME
' Szp [mot] SET ZERO
POSITION
' Idle [mot] POWER OFF
MOTOR
' Gsm [mot] GET MOTOR
STATE 0=STOP 1=MOVE
' Mode [mode] SET MODE
PROFILE FOR 2 MOTOR 0,1,2,3,4
' Gmode GET
PROFILE MODE SETTING
' Gpwm [mot] GET MAX PWM
OUT
' Rpwm [mot] READ REAL PWM OUT TO CONTROL MAX
CURRENT
' Gacc [mot] GET
ACCELERATION PROFILE IN DEGREE 1-89
' Gang [mot] GET ANGULAR
FACTOR
' Rst [mot] RESET ENCODER
'.............................................................................................................
$prog &HFF , &H6F , &HD7 , &H00 'FUSE BITS
$regfile = "m32def.dat"
$crystal = 18432000 'over
clocked CPU
$baud = 115200
$hwstack = 64
$swstack = 64
$framesize = 64
'-------------------------------------------------------------------------------
Const Si = 1
Const No = 0
Const Vb6 = No 'YES PER USARE LA SERIALE CON Visual Basic 6
' 'NO PER TERMINAL WINDOWS
'-------------------------------------------------------------------------------
'------------------- SUBROUTINE
------------------------------------------------
'-------------------------------------------------------------------------------
Declare Sub Hctl_2032(byval Mot As Byte)
Declare Sub Exe_pid(byval Mot As Byte , Byval Pid_setpoint As Long , Byval Pid_actual As Long)
Declare Sub Configura_pid(byval M As Byte , Byval Mot_kp As Long , Byval Mot_ki As Long , Byval Mot_kd As Long)
Declare Sub Init_parameter(byval M As Byte)
Declare Sub Calcola_trapezio(byval M As Byte)
Declare Sub Make_print(byval Mt As Byte , Byval Stringa As String , Byval Valore As Long)
Declare Sub Set_mode(byval Mode_control As Byte)
'-------------------------------------------------------------------------------
'------------------- CONFIGURAZIONI
--------------------------------------------
'-------------------------------------------------------------------------------
'configuriamo il timer1 per pwm a 8 bit a freq = 565 hz
con xtal=18mhz e prescale=1
'abbiamo (18432000 / 256 = 72 Khz) (72 K / presc = 72
K) (72 K/2 pwm = 36 Khz)
'-------------------------------------------------------------------------------
Config Watchdog = 16
Stop Watchdog
Config Int0 = Falling
Config Int1 = Falling
Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 1 '36.2 Khz pwm
Config Timer0 = Timer , Prescale = 64 '
On Timer0 Timer_0 '
On Int1 Int_1 '
On Int0 Int_0 '
On Urxc Rs232 '
Enable Timer0 'handler seriale
Enable Urxc 'abilita seriale
Enable Int0 'fine corsa motore 1
Enable Int1 'fine corsa motore 2
Enable Interrupts 'abilita tutti interrupts
'------------------- ALIAS
-----------------------------------------------------
Motor_pwm_1 Alias Pwm1a : Motor_pwm_1 = 0 'pwm out motore x
Motor_pwm_2 Alias Pwm1b : Motor_pwm_2 = 0 'pwm out motore y
Ddrd.2 = 0 : Portd.2 = 1 'pullup x int 0
Ddrd.3 = 0 : Portd.3 = 1 'pullup x int 1
Fc2_1 Alias Pinb.0 : Ddrb.0 = 0 'limit 1
Fc2_2 Alias Pinb.1 : Ddrb.1 = 0 'limit 2
Aux_0 Alias Pinb.2 : Ddrb.2 = 0 : Portb.2 = 1 'pullup aux per indirizzo scheda
Aux_1 Alias Pinb.3 : Ddrb.3 = 0 : Portb.3 = 1 'pullup aux per indirizzo scheda
Smt_1 Alias Portb.4 : Ddrb.4 = 1 'stato motore 1
Smt_2 Alias Portc.6 : Ddrc.6 = 1 'stato motore 2
Motor_dir_1 Alias Portd.6 : Ddrd.6 = 1 'direzione motore x
Motor_dir_2 Alias Portd.7 : Ddrd.7 = 1 'direzione motore y
Motor_led Alias Portc.7 : Ddrc.7 = 1 'led test
Hctl_xy Alias Portc.0 : Ddrc.0 = 1 'selezione xy hctl2032
Hctl_sel1 Alias Portc.1 : Ddrc.1 = 1 'selezione byte bit 0
Hctl_sel2 Alias Portc.2 : Ddrc.2 = 1 'selezione byte bit 1
Hctl_oe Alias Portc.3 : Ddrc.3 = 1 'selezione oe hctl data
Hctl_rst_1 Alias Portc.4 : Ddrc.4 = 1 'reset encoder x
Hctl_rst_2 Alias Portc.5 : Ddrc.5 = 1 'reset encoder y
Hctl_data Alias Pina : Ddra = &H00 'porta
input data encoder
'-------------------------------------------------------------------------------
Hctl_xy = 0 'seleziona encoder x
Hctl_sel1 = 0 'selezione primo byte msb
Hctl_sel2 = 1 'seleziona primo byte msb
Hctl_oe = 1 'no output enable
Hctl_rst_1 = 1 'no reset
Hctl_rst_2 = 1 'no reset
'---------------------------- COSTANTI
-----------------------------------------
Const Mode_pos = 0
Const Mode_vel = 1
Const Mode_trp = 2
Const Mode_velp = 3
Const Mode_idle = 4
Const True = 1
Const False = 0
'---------------------- VARIABILI
----------------------------------------------
Dim Pid_kp(2) As Long
Dim Pid_ki(2) As Long
Dim Pid_kd(2) As Long
Dim Pid_scale(2) As Long
Dim Pid_prev_error(2) As Long
Dim Pid_integral_error(2) As Long
Dim Pid_error(2) As Long
Dim Ptemp(2) As Long
Dim Motore_setpoint(2) As Long
Dim Pid_out(2) As Long 'valore da assegnare a pwm
Dim Data_1 As Byte 'hctl used
Dim Data_2 As Byte 'hctl used
Dim Data_3 As Byte 'hctl used
Dim Data_4 As Byte 'hctl used
Dim Timer_pid As Byte 'sample timer pid
Dim Pos_encoder(2) As Long 'encoder position
Dim Old_encoder(2) As Long 'prewius encoder position
Dim New_speed(2) As Long 'velocita in ingresso pid
Dim Act_speed(2) As Long 'velocita attuale
Dim Acc_speed(2) As Long 'velocita accelerazione
Dim Pos_finale(2) As Long 'posizione trapezoidale finale
Dim Temp_enc As Long 'temp encoder position
Dim Comando As String * 30
Dim Comando_old As String * 30
Dim Char(20) As Byte
Dim Idt As Byte : Idt = 1
Dim N_cmd As Byte
Dim Cmd_ar(5) As String * 10
Dim Mode_ctrl As Byte 'mode control
Dim Mot_kp As Long
Dim Mot_ki As Long
Dim Mot_kd As Long
Dim Vel_pos(2) As Long
Dim Vel_neg(2) As Long
Dim Vel_last(2) As Long
Dim Rpwm(2) As Byte
Dim Fattore_acc(2) As Long
Dim Punto_p1(2) As Long
Dim Dir_diff(2) As Byte
Dim Diff_posizione(2) As Long
Dim Trap_1(2) As Long
Dim Trap_2(2) As Long
Dim New_encoder(2) As Long
Dim Deg(2) As Single
Dim Rad(2) As Single
Dim Vel_max(2) As Single
Dim Diff_2(2) As Long
Dim Max_pwm(2) As Byte
Dim Mot As Byte
Dim M As Byte
Dim Start_move As Bit
Dim Tx_enable As Bit
Dim Str_tx_1 As String * 20
Dim Flag_velocita As Bit
Dim Somma As Long
Dim Vmax_pos(2) As Long
Dim Vmax_neg(2) As Long
Dim Fattore_ang(2) As Long 'fattore angolare 0-99
Dim Differenza(2) As Long 'differenza
Dim Acc_speed_p(2) As Long 'accelerazione positiva x controllo velocita
Dim Acc_speed_n(2) As Long 'accelerazione negativa x controllo velocita
Dim Pid_time As Byte 'tempo max pid overflow
Dim Num_smt(2) As Byte 'numero di volte lettura motore fermo
'-------------------------------------------------------------------------------
Call Init_parameter(1)
Call Init_parameter(2)
Call Configura_pid(1 , 500 , 100 , 500) 'configura pid iniziale
Call Configura_pid(2 , 500 , 100 , 500) 'configura pid iniziale
'...................P....I......D
Print "(0 Motor Control by G.De Luca V1.0, 0)"
'-------------------------------------------------------------------------------
'---------------------- MAIN PROGRAM
-------------------------------------------
'-------------------------------------------------------------------------------
Main:
Do
If Tx_enable = 1 Then 'permetti la trasmissione
Tx_enable = 0
Print Str_tx_1 'trasmetti la stringa
End If
'.....qui
controlliamo i fine corsa 2 dei motori
Loop
'-------------------------------------------------------------------------------
End
'-------------------------------------------------------------------------------
Int_0: 'fine corsa 1 motore 1
Call Configura_pid(1 , 0 , 0 , 0)
Print "(1 FCM, 1)"
Return
'-------------------------------------------------------------------------------
Int_1: 'fine corsa 1 motore 2
Call Configura_pid(2 , 0 , 0 , 0)
Print "(2 FCM, 1)"
Return
'-------------------------------------------------------------------------------
'---------------------- GESTIONE SERIALE
---------------------------------------
'-------------------------------------------------------------------------------
Rs232:
#if Vb6
Input Comando Noecho 'se usato con vb6
#else
Char(1) = Inkey() 'se usato con terminal
Char(1) = Ucase(char(1))
If Char(1) = Chr(13) Then
Print
#endif
'...........................................................................
If Comando = "" Then Comando = Comando_old
N_cmd = Split(comando , Cmd_ar(1) , " ")
M = Val(cmd_ar(2))
'......................................................................
Select Case Cmd_ar(1)
Case "RSTF"
Print "(0 RST
Program, 0)"
Start Watchdog
'..........................................................
Case "MADE"
Print "(0 De Luca
Giovanni, 0)"
'..........................................................
Case "VER"
Print "(0 Ver 2.0
DEL 22-05-07, 0)"
'..........................................................
Case "SKP"
Pid_kp(m) = Val(cmd_ar(3))
Print "(" ; M ; " SKP, " ; Pid_kp(m) ; ")"
'..........................................................
Case "SKI"
Pid_ki(m) = Val(cmd_ar(3))
Print "(" ; M ; " SKI, " ; Pid_ki(m) ; ")"
'..........................................................
Case "SKD"
Pid_kd(m) = Val(cmd_ar(3))
Print "(" ; M ; " SKD, " ; Pid_kd(m) ; ")"
'..........................................................
Case "GO" 'vai in controllo posizione
If Mode_ctrl <> Mode_pos Then 'se mode <>
da pos
Call Set_mode(mode_pos) 'setta a
pos
End If
M = Val(cmd_ar(2))
Pos_finale(m) = Val(cmd_ar(3))
Print "(" ; M ; " GO, " ; Pos_finale(m) ; ")"
'..........................................................
Case "VMAX"
Vmax_pos(m) = Val(cmd_ar(3))
Vmax_neg(m) = -vmax_pos(m)
Print "(" ; M ; " VMAX, " ; Vmax_pos(m) ; ")"
'..........................................................
Case "GMAX"
Print "(" ; M ; " GMAX, " ; Vmax_pos(m) ; ")"
'..........................................................
Case "SVM" 'set max velocita in trp
Vel_pos(m) = Val(cmd_ar(3))
Vel_last(m) = Vel_pos(m)
Vel_neg(m) = -vel_pos(m)
Print "(" ; M ; " SVM, " ; Vel_pos(m) ; ")"
'..........................................................
Case "GVM"
Print "(" ; M ; " GVM, " ; Vel_pos(m) ; ")" 'get max velocita impostata
'..........................................................
Case "GVE"
Tx_enable = 1
Call Make_print(m , Cmd_ar(1) , Act_speed(m))
'..........................................................
Case "ENC"
Tx_enable = 1
Call Make_print(m , Cmd_ar(1) , Pos_encoder(m))
'..........................................................
Case "SPID" 'set pid parameter
Pid_kp(m) = Val(cmd_ar(3))
Pid_ki(m) = Val(cmd_ar(4))
Pid_kd(m) = Val(cmd_ar(5))
Print "(" ; M ; " SPID, " ; Pid_kp(m) ; " " ; Pid_ki(m) ; " " ; Pid_kd(m) ; ")"
'..........................................................
Case "GPID" 'get pid parameter
Print "(" ; M ; " GPID, " ; Pid_kp(m) ; " " ; Pid_ki(m) ; " " ; Pid_kd(m) ; ")" 'get pid parameter
'..........................................................
Case "STIME"
Pid_time = Val(cmd_ar(2))
Print "(" ; 0 ; " STIME, " ; Pid_time ; ")"
'..........................................................
Case "GTIME"
Print "(" ; 0 ; " GTIME, " ; Pid_time ; ")"
'..........................................................
Case "SZP" 'set zero position
Select Case M
Case 1
Hctl_rst_1 = 0
Waitms 1
Hctl_rst_1 = 1
Case 2
Hctl_rst_2 = 0
Waitms 1
Hctl_rst_2 = 1
End Select
Pos_encoder(m) = 0
Old_encoder(m) = 0
Motore_setpoint(m) = 0
Pos_finale(m) = 0
New_speed(m) = 0
Call Hctl_2032(m)
Print "(" ; M ; " SZP, " ; Pos_encoder(m) ; ")"
'..........................................................
Case "IDLE" 'metti in idle e disalimenta
Call Configura_pid(m , 0 , 0 , 0)
Print "(" ; M ; " IDLE, )"
'..........................................................
Case "GSM" 'get stato motore
If Act_speed(m) > 3 Or Act_speed(m) < -3 Then
Tx_enable = 1
Call Make_print(m , Cmd_ar(m) , 1) 'qui si muove
Else
Tx_enable = 1
Call Make_print(m , Cmd_ar(m) , 0) 'qui č fermo
End If
'..........................................................
Case "SMODE" 'set mode control
Mode_ctrl = Val(cmd_ar(3))
Select Case Mode_ctrl
Case Mode_pos
Call Configura_pid(m , 200 , 0 , 200) 'POSIZIONE 0
Motore_setpoint(m) = Pos_encoder(m)
Case Mode_vel
Call Configura_pid(m , 700 , 100 , 700) 'VELOCITA integrale 1
Case Mode_trp
Call Configura_pid(m , 500 , 100 , 500) 'TRAPEZOIDALE 2
Pos_finale(m) = Pos_encoder(m)
Case Mode_velp
Call Configura_pid(m , 100 , 10 , 100) 'VELOCITA proporzionale 3
Case Mode_idle
Call Configura_pid(m , 0 , 0 , 0) 'IDLE 4
End Select
Print "(0 MODE,
" ; Mode_ctrl ; ")"
'..........................................................
Case "MODE"
Mode_ctrl = Val(cmd_ar(2)) 'tipo di
controllo
Call Set_mode(mode_ctrl)
Print "(0 MODE,
" ; Mode_ctrl ; ")"
'..........................................................
Case "GMODE" 'get mode control
Print "(0 GMODE,
" ; Mode_ctrl ; ")"
'..........................................................
Case "SVEL" 'set velocita in vel mode
If Mode_ctrl <> Mode_vel Then 'se mode <>
da trapezoidale
Call Set_mode(mode_vel)
End If
Motore_setpoint(m) = Val(cmd_ar(3)) 'valori -127 +127
Print "(" ; M ; " SVEL, " ; Motore_setpoint(m) ; ")"
'..........................................................
Case "VELO"
If Mode_ctrl <> Mode_vel Then 'se mode <>
da trapezoidale
Call Set_mode(mode_vel)
End If
Motore_setpoint(1) = Val(cmd_ar(2)) 'valori -127 +127
Motore_setpoint(2) = Val(cmd_ar(3))
'..........................................................
Case "SPWM" 'max pwm per regolare la coppia massima
Max_pwm(m) = Val(cmd_ar(3))
Print "(" ; M ; " SPWM, " ; Max_pwm(m) ; ")"
'..........................................................
Case "GPWM" 'get pwm massimo
Print "(" ; M ; " GPWM, " ; Max_pwm(m) ; ")"
'..........................................................
Case "RPWM" 'get valore pwm uscita
Print "(" ; M ; " RPWM, " ; Rpwm(m) ; ")"
'..........................................................
Case "SACC"
Deg(m) = Val(cmd_ar(3))
Rad(m) = Deg2rad(deg(m))
Rad(m) = Tan(rad(m))
Vel_max(m) = Vel_pos(m) * 10
Punto_p1(m) = Vel_max(m) / Rad(m)
Punto_p1(m) = Punto_p1(m) * 10
Print "(" ; M ; " SACC, " ; Deg(m) ; ")"
'... Print "P1: "
; Punto_p1(m)
'..........................................................
Case "GACC" 'calcola pendenza accelerazione
Print "(" ; M ; " GACC, " ; Deg(m) ; ")"
'..........................................................
Case "SANG" 'imposta fattore angolare
Fattore_acc(m) = Val(cmd_ar(3))
Print "(" ; M ; " SANG, " ; Fattore_acc(m) ; ")"
'..........................................................
Case "GANG" 'leggi fattore angolare
Print "(" ; M ; " GANG, " ; Fattore_acc(m) ; ")"
'..........................................................
Case "POS"
If Mode_ctrl <> Mode_trp Then 'se mode <>
da trapezoidale
Call Set_mode(mode_trp)
End If
Pos_finale(m) = Val(cmd_ar(3))
Vel_pos(m) = Val(cmd_ar(4))
Call Calcola_trapezio(m)
Start_move = True
'..........................................................
Case "MOVE"
If Mode_ctrl <> Mode_trp Then 'se mode <>
da trapezoidale
Call Set_mode(mode_trp)
End If
Pos_finale(1) = Val(cmd_ar(2))
Pos_finale(2) = Val(cmd_ar(3))
Vel_pos(1) = Val(cmd_ar(4))
Vel_pos(2) = Val(cmd_ar(4))
Call Calcola_trapezio(1)
Call Calcola_trapezio(2)
Print "(0 MOVE,
" ; Pos_finale(1) ; " " ; Pos_finale(2) ; ")"
'Start_move = False 'se si vuole avviare con
start
'..........................................................
Case "VELP"
If Mode_ctrl <> Mode_velp Then 'se mode <>
da velocita
Call Set_mode(mode_velp)
End If
Acc_speed_p(1) = Vmax_pos(1)
Acc_speed_p(2) = Vmax_pos(2)
Acc_speed_n(1) = Vmax_neg(1)
Acc_speed_n(2) = Vmax_neg(2)
Fattore_ang(1) = Fattore_acc(1)
Fattore_ang(2) = Fattore_acc(2)
Pos_finale(1) = Val(cmd_ar(2))
Pos_finale(2) = Val(cmd_ar(3))
'..........................................................
Case "SUM"
Somma = Val(cmd_ar(2))
'..........................................................
Case "START"
Start_move = True
Print "(0 START,
0)"
'..........................................................
Case "STOP"
Mode_ctrl = Mode_idle
Print "(0 STOP,
0)"
'..........................................................
Case "RST"
If M = 1 Then
Hctl_rst_1 = 0
Waitms 1
Hctl_rst_1 = 1
End If
If M = 2 Then
Hctl_rst_2 = 0
Waitms 1
Hctl_rst_2 = 1
End If
Call Hctl_2032(m)
Print "(" ; M ; " RST, " ; Pos_encoder(m) ; ")"
'..........................................................
Case Else
If Mode_ctrl <> Mode_velp Then 'se mode <>
da trapezoidale
Call Set_mode(mode_velp)
End If
Pos_finale(1) = Pos_finale(1) + Somma
Pos_finale(2) = Pos_finale(2) + Somma
'..........................................................
End Select
Comando_old = Comando
Comando = ""
'......................................................................
#if Vb6
#else
Else
Comando = Comando + Chr(char(1))
Print Comando ; Chr(13);
End If
#endif
Return
'-------------------------------------------------------------------------------
'----------------------- esecuzione PID
----------------------------------------
'-------------------------------------------------------------------------------
'la routine seguente viene eseguita per i due motori in
500 uSec ad una freq di 280 hz
'il periodo di intervento PID č di 3.5 msec con timer_pid
= 4
Timer_0:
If Timer_pid = Pid_time Then
Timer_pid = 0
For Mot = 1 To 2
Motor_led = True
Call Hctl_2032(mot) 'LEGGI IL VALORE DALL'ENCODER
Act_speed(mot) = Pos_encoder(mot) - Old_encoder(mot) 'con 12 Volt ed encoder HEDS-5540
Old_encoder(mot) = Pos_encoder(mot) 'se Pos_encoder(1)=10000 abbiamo trap_1(1)=30000
New_speed(mot) = Pos_finale(mot) - Pos_encoder(mot) 'differenza per
ogni campionamento
Differenza(mot) = Pos_finale(mot) - Pos_encoder(mot) 'questo calcolo serve x controllo continuo velocita
'------------------------------------------------------------------------
'///// se si cambia di segno prima togli velocita e
poi accelera \
'------------------------------------------------------------------------
If Flag_velocita = True Then
'.....................................................................
If Pos_finale(mot) => Pos_encoder(mot) Then Dir_diff(mot) = True '--->
If Pos_finale(mot) < Pos_encoder(mot) Then Dir_diff(mot) = False '<---
'.....................................................................
If Dir_diff(mot) = True Then ' 99 / 100 = 0 'qui si ferma
prima ma abbiamo la decellerazione
If Differenza(mot) <= 10000 Then Acc_speed_p(mot) = New_speed(mot) / Fattore_acc(mot) 'decellera
If Acc_speed_p(mot) <= 1 Then Acc_speed_p(mot) = 1
If New_speed(mot) => Vmax_pos(mot) Then New_speed(mot) = Vmax_pos(mot)
If New_speed(mot) => Acc_speed_p(mot) Then New_speed(mot) = Acc_speed_p(mot)
End If
'.....................................................................
If Dir_diff(mot) = False Then
If Differenza(mot) => -10000 Then
Acc_speed_n(mot) = New_speed(mot) / Fattore_acc(mot) 'decellera
If Acc_speed_n(mot) => -1 Then
Acc_speed_n(mot) = -1
If New_speed(mot) <= Vmax_neg(mot) Then New_speed(mot) = Vmax_neg(mot)
If New_speed(mot) <= Acc_speed_n(mot) Then New_speed(mot) = Acc_speed_n(mot)
End If
'.....................................................................
End If
'------------------------------------------------------------------------
If Flag_velocita = False Then
If Dir_diff(mot) = True Then
New_encoder(mot) = Trap_1(mot) - Pos_encoder(mot) '30000 - 10000 = 20000
New_encoder(mot) = Punto_p1(mot) - New_encoder(mot) '20000 - 20000 =
0
If Pos_encoder(mot) < Trap_1(mot) Then Acc_speed(mot) = New_encoder(mot) / Fattore_acc(mot) 'accelerazione
prima di trap_1 -->
If Pos_encoder(mot) => Trap_2(mot) Then Acc_speed(mot) = New_speed(mot) / Fattore_acc(mot) 'decelerazione dopo trap_2 -->
If Acc_speed(mot) <= 1 Then Acc_speed(mot) = 1
If New_speed(mot) => Vel_pos(mot) Then New_speed(mot) = Vel_pos(mot) 'ferma su velocita massima
If New_speed(mot) => Acc_speed(mot) Then New_speed(mot) = Acc_speed(mot) 'riduci la velocita
End If 'mentre viene ridotta l'accelerazione
'........................................................................
If Dir_diff(mot) = False Then
New_encoder(mot) = Trap_1(mot) - Pos_encoder(mot) '80000 - 99000 = -20000
New_encoder(mot) = New_encoder(mot) + Punto_p1(mot) '21000 - 20000) = -1000
New_encoder(mot) = -new_encoder(mot) 'inverti di
segno. qui new_encoder č negativo
If Pos_encoder(mot) => Trap_1(mot) Then Acc_speed(mot) = New_encoder(mot) / Fattore_acc(mot) 'accelerazione
prima di trap_1 <--
If Pos_encoder(mot) < Trap_2(mot) Then Acc_speed(mot) = New_speed(mot) / Fattore_acc(mot) 'decelerazione
dopo di trap_2 <--
If Acc_speed(mot) => -1 Then Acc_speed(mot) = -1
If New_speed(mot) <= Vel_neg(mot) Then New_speed(mot) = Vel_neg(mot) 'ferma su velocita negativa
If New_speed(mot) <= Acc_speed(mot) Then New_speed(mot) = Acc_speed(mot) 'riduci la velocita
End If
End If
'------------------------------------------------------------------------
Select Case Mode_ctrl
Case Mode_pos
Call Exe_pid(mot , Pos_finale(mot) , Pos_encoder(mot))
'...........................................................
Case Mode_vel
Call Exe_pid(mot , Motore_setpoint(mot) , Act_speed(mot))
'...........................................................
Case Mode_trp
Motore_setpoint(mot) = New_speed(mot)
If Start_move = 0 Then Motore_setpoint(mot) = 0
Call Exe_pid(mot , Motore_setpoint(mot) , Act_speed(mot))
'...........................................................
Case Mode_velp
Motore_setpoint(mot) = New_speed(mot)
Call Exe_pid(mot , Motore_setpoint(mot) , Act_speed(mot))
'...........................................................
Case Mode_idle
Call Configura_pid(mot , 0 , 0 , 0)
'..Pos_encoder(mot) = 0
'..Old_encoder(mot) = 0
'..Motore_setpoint(mot) = 0
'..Pos_finale(mot) = 0
'..New_speed(mot) = 0
If Mot = 1 Then
Motor_pwm_1 = 0
If Mot = 2 Then
Motor_pwm_2 = 0
End Select
'........................................................................
If Act_speed(1) > 2 Or Act_speed(1) < -2 Then 'se quasi fermo
Num_smt(1) = 0 'controlla se in movimento
Else
Incr Num_smt(1) 'se fermo, vedi x quanto tempo
End If
If Act_speed(2) > 2 Or Act_speed(2) < -2 Then 'se quasi fermo
Num_smt(2) = 0
Else
Incr Num_smt(2)
End If
'..........................................................................
If Num_smt(1) > 200 Then Num_smt(1) = 200 'non azzerra a 255
If Num_smt(2) > 200 Then Num_smt(2) = 200 'non azzerra a 255
If Num_smt(1) > 150 Then Smt_1 = 0 Else Smt_1 = 1
If Num_smt(2) > 150 Then Smt_2 = 0 Else Smt_2 = 1
'..........................................................................
Next Mot
End If
Incr Timer_pid
Motor_led = False
Return
'-------------------------------------------------------------------------------
'----------------------- CALCOLO DEL PID
---------------------------------------
'----------------------- tempo PER PID = 90 uSecondi
---------------------------
'-------------------------------------------------------------------------------
Sub Exe_pid(mot , Pid_setpoint , Pid_actual) '90
microsecondi
'.. Motor_led = True
Pid_error(mot) = Pid_setpoint - Pid_actual
Pid_out(mot) = Pid_error(mot) * Pid_kp(mot)
Ptemp(mot) = Pid_error(mot) - Pid_prev_error(mot)
Pid_prev_error(mot) = Pid_error(mot)
Ptemp(mot) = Ptemp(mot) * Pid_kd(mot)
Pid_out(mot) = Pid_out(mot) + Ptemp(mot)
Ptemp(mot) = Pid_integral_error(mot) * Pid_ki(mot)
Pid_out(mot) = Pid_out(mot) + Ptemp(mot)
Pid_out(mot) = Pid_out(mot) / Pid_scale(mot)
If Pid_out(mot) > 255 Then 'limita il valore a 8 bit
Pid_out(mot) = 255
Elseif Pid_out(mot) < -255 Then
Pid_out(mot) = -255
Else
Pid_error(mot) = Pid_error(mot) +
Pid_integral_error(mot)
If Pid_error(mot) > 255 Then
Pid_error(mot) = 255
Elseif Pid_error(mot) < -255 Then
Pid_error(mot) = -255
End If
Pid_integral_error(mot) = Pid_error(mot)
End If
If Mot = 1 Then
If Pid_out(mot) => 0 Then Motor_dir_1 = 0 'setta la direzione del motore
If Pid_out(mot) < 0 Then Motor_dir_1 = 1
Pid_out(mot) = Abs(pid_out(mot)) 'calcola il valore assoluto
If Pid_out(mot) => Max_pwm(mot) Then Pid_out(mot) = Max_pwm(mot)
Motor_pwm_1 = Pid_out(mot) 'imposta il pwm
Rpwm(mot) = Pid_out(mot) 'segnale uscita test
End If
If Mot = 2 Then
If Pid_out(mot) => 0 Then Motor_dir_2 = 0 'setta la direzione del motore
If Pid_out(mot) < 0 Then Motor_dir_2 = 1
Pid_out(mot) = Abs(pid_out(mot)) 'calcola il valore assoluto
If Pid_out(mot) => Max_pwm(mot) Then Pid_out(mot) = Max_pwm(mot)
Motor_pwm_2 = Pid_out(mot) 'imposta il pwm
Rpwm(mot) = Pid_out(mot) 'segnale uscita test
End If
'.. Motor_led = False
End Sub
'-------------------------------------------------------------------------------
'---------------------- Configurazione pid
-------------------------------------
'-------------------------------------------------------------------------------
Sub Configura_pid(m , Mot_kp , Mot_ki , Mot_kd)
Pid_kp(m) = Mot_kp
Pid_ki(m) = Mot_ki
Pid_kd(m) = Mot_kd
Pid_scale(m) = 100
End Sub
'-------------------------------------------------------------------------------
'----------------------- LETTURA HCTL 2032
-------------------------------------
'-------------------------------------------------------------------------------
Sub Hctl_2032(mot)
If Mot = 1 Then Hctl_xy = 0
If Mot = 2 Then Hctl_xy = 1
Hctl_oe = 0 'oe
Hctl_sel1 = 0 'selezione primo byte msb
Hctl_sel2 = 1 'seleziona primo byte msb
Data_4 = Hctl_data 'msb byte
Hctl_sel1 = 1
Hctl_sel2 = 1
Data_3 = Hctl_data '2nd byte
Hctl_sel1 = 0
Hctl_sel2 = 0
Data_2 = Hctl_data '3rd byte
Hctl_sel1 = 1
Hctl_sel2 = 0
Data_1 = Hctl_data 'lsb byte
Hctl_oe = 1
Pos_encoder(mot) = Data_1
Temp_enc = Data_2 * 256
Pos_encoder(mot) = Pos_encoder(mot) + Temp_enc
Temp_enc = Data_3 * 65536
Pos_encoder(mot) = Pos_encoder(mot) + Temp_enc
Temp_enc = Data_4 * 16777216
Pos_encoder(mot) = Pos_encoder(mot) + Temp_enc
End Sub
'-------------------------------------------------------------------------------
'------------------------- inizializza parametri
-------------------------------
'-------------------------------------------------------------------------------
Sub Init_parameter(m)
Pid_time = 4 'tempo di pid
Somma = 1000
Vmax_pos(m) = 126
Vmax_neg(m) = -vmax_pos(m)
Vel_pos(m) = 126 'velocita iniziale
Vel_last(m) = Vel_pos(m) 'salva il valore last
Vel_neg(m) = -vel_pos(m) 'velocita negativa iniziale
Flag_velocita = False
Start_move = True 'start move attivo
Max_pwm(m) = 250 'massimo valore pwm in uscita
Deg(m) = 45 'pendenza accelerazione in grad(1)i
Fattore_acc(m) = 100
Punto_p1(m) = 20000 'punto 1
Motore_setpoint(m) = 0 'posizione iniziale = 0
If M = 1 Then Motor_pwm_1 = 0 'pwm x uscita = 0
If M = 2 Then Motor_pwm_2 = 0 'pwm y uscita = 0
Mode_ctrl = Mode_trp 'controllo trapezoidale
End Sub
'-------------------------------------------------------------------------------
'------------------------- CALCOLO DEL PROFILO
TRAPEZIOIDALE -------------------
'-------------------------------------------------------------------------------
Sub Calcola_trapezio(m)
If Vel_pos(m) = 0 Then Vel_pos(m) = Vel_last(m)
Rad(m) = Deg2rad(deg(m)) 'trasforma
in radianti i gradi pendenza
Rad(m) = Tan(rad(m)) 'calcola la tangente dell'angolo
Vel_max(m) = Vel_pos(m) * 10 'per calcolare la
distanza del punto p1
Punto_p1(m) = Vel_max(m) / Rad(m)
Punto_p1(m) = Punto_p1(m) * 10
'.....Print "P1: " ; Punto_p1(m)
Vel_neg(m) = -vel_pos(m)
Vel_last(m) = Vel_pos(m)
Call Hctl_2032(m)
Diff_posizione(m) = Pos_finale(m) - Pos_encoder(m)
Diff_posizione(m) = Abs(diff_posizione(m))
Fattore_acc(m) = Punto_p1(m) / Vel_pos(m) 'messo qui ma
prima era giu prima di print
'..........................................................aggiunto
con ACC command
Diff_2(m) = Diff_posizione(m) / 2
If Punto_p1(m) > Diff_2(m) Then
Punto_p1(m) = Diff_2(m)
Punto_p1(m) = Punto_p1(m) / 2
End If
'..........................................................
If Pos_finale(m) => Pos_encoder(m) Then Dir_diff(m) = True '--->
If Pos_finale(m) < Pos_encoder(m) Then Dir_diff(m) = False '<---
'..........................................................
If Dir_diff(m) = True Then
Trap_1(m) = Pos_encoder(m) + Punto_p1(m)
Trap_2(m) = Pos_finale(m) - Punto_p1(m)
End If
'..........................................................
If Dir_diff(m) = False Then
Trap_1(m) = Pos_encoder(m) - Punto_p1(m)
Trap_2(m) = Pos_finale(m) + Punto_p1(m)
End If
'..........................................................
Print "(" ; M ; " POS, " ; Pos_finale(m) ; ")"
'...Print "Trapezio: " ; Trap_1(m) ; ", " ;
Trap_2(m)
'...Print "Fatt ang: " ; Fattore_acc(m)
End Sub
'-------------------------------------------------------------------------------
Sub Make_print(mt , Stringa , Valore)
Str_tx_1 = "(" + Str(mt) + " " + Stringa + ", " + Str(valore) + ")"
End Sub
'-------------------------------------------------------------------------------
Sub Set_mode(mode_control)
Select Case Mode_control
Case Mode_pos 'POSIZIONE 0
Flag_velocita = False
Call Configura_pid(1 , 200 , 0 , 200)
Call Configura_pid(2 , 200 , 0 , 200)
Pos_finale(1) = Pos_encoder(1)
Pos_finale(2) = Pos_encoder(2)
Case Mode_vel 'VELOCITA 1
Flag_velocita = False
Call Configura_pid(1 , 700 , 100 , 700)
Call Configura_pid(2 , 700 , 100 , 700)
Motore_setpoint(1) = 0
Motore_setpoint(2) = 0
Case Mode_trp 'TRAPEZOIDALE 2
Flag_velocita = False
Call Configura_pid(1 , 500 , 100 , 500)
Call Configura_pid(2 , 500 , 100 , 500)
Pos_finale(1) = Pos_encoder(1)
Pos_finale(2) = Pos_encoder(2)
Case Mode_velp 'VELOCITA
PID 3
Flag_velocita = True
Call Configura_pid(1 , 400 , 100 , 400)
Call Configura_pid(2 , 400 , 100 , 400)
Pos_finale(1) = Pos_encoder(1)
Pos_finale(2) = Pos_encoder(2)
Case Mode_idle 'IDLE 4
Flag_velocita
= False
Call Configura_pid(1 , 0 , 0 , 0)
Call Configura_pid(2 , 0 , 0 , 0)
End Select
Mode_ctrl = Mode_control
End Sub
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------
|
