Code for control of DC motor using back emf using 18m2.
Works very well.
This uses 18m2. Use NOT gate for motor direction (I used a couple of transistor but you can use 7404).
See for circuit see here.
This is part of a larger project I am working on but I thought I would share.
Anobium
Works very well.
Controls two DC motors via L293D
Bi-directional feedback
Each motor is controller independently
Feedback used to ensure motor is at correct speed and to adjustment to ensure each wheel is at the same speed
Bi-directional feedback
Each motor is controller independently
Feedback used to ensure motor is at correct speed and to adjustment to ensure each wheel is at the same speed
This uses 18m2. Use NOT gate for motor direction (I used a couple of transistor but you can use 7404).
See for circuit see here.
This is part of a larger project I am working on but I thought I would share.
Anobium
Code:
' < ================================ >
' BackEMF
' See http://frontrangerobotics.org/PIDbackEMF/DavesBEMFmotorArticle.htm
#rem
ÚÄÄÄÄÄÄÄÄ¿
10k POT to adjust delay c.2 > ³1 18³ c.1 < Switch input. Up btn
c.3 ³2 17³ c.0 < Switch input. Down btn
(not used) Serial In c.4 ³3 16³ c.7 < Direction change button
c.5 ³4 15³ c.6 > Motor 2 Information LED
-------- 0v ³5 14³ +5V --------
Motor 1 Dir to NOT gate&L293D b.0 < ³6 13³ b.7 < Motor 2 EMF #1
Motor 1 EMF #1 b.1 > ³7 12³ b.6 > Motor 2 Speed Out via PWM
Motor 1 EMF #2 b.2 > ³8 11³ b.5 < Motor 2 EMF #2
Motor 1 Speed Out via PWM b.3 < ³9 10³ b.4 > Motor 2 Dir to NOT gate&L293D
ÀÄÄÄÄÄÄÄÄÙ
#endrem
#picaxe 18m2
#define serial_debugx
setfreq m32
#no_data
#terminal 38400
' Assign Input/Output pins
symbol modeUp_btn = pinc.1 'INPUT from physical PIN 4;
symbol modeDn_btn = pinc.0 'INPUT from physical PIN 3;
symbol dir_btn = pinc.7 'INPUT from physical PIN 3;
symbol motor1_dirport = b.0: output b.0
symbol motor1_EMFport1 = b.1
symbol motor1_EMFport2 = b.2
symbol motor1_pwmport = b.3
symbol motor2_dirport = b.4
symbol motor2_EMFport1 = b.7
symbol motor2_EMFport2 = b.5
symbol motor2_pwmport = b.6
symbol delay_pot = c.2
symbol motor1_tempDuty = W1
symbol motor2_tempDuty = W2
symbol motor_stall1 = b6
symbol motor_stall2 = b7
symbol motor1_Direction = b8
symbol motor2_Direction = b9
symbol motor1_pulseCount = b10
symbol motor2_pulseCount = b11
symbol motor_stall = b12
symbol motor1_Duty = W11
symbol motor2_Duty = W12
symbol stallvalue = 10
symbol minDuty = 450
symbol mtr_off = 0
symbol mtr_fwd = 1
symbol mtr_rev = 2
symbol motor1_mtr_off = mtr_off
symbol motor1_mtr_fwd = mtr_fwd
symbol motor1_mtr_rev = mtr_rev
symbol motor2_mtr_off = mtr_off
symbol motor2_mtr_fwd = mtr_fwd
symbol motor2_mtr_rev = mtr_rev
motor1_Duty = minDuty 'Start with medium duty
motor1_Duty = minDuty
motor2_Duty = minDuty
motor1_Direction = motor1_mtr_rev
motor2_Direction = motor2_mtr_rev
gosub change_direction_init
' main loop
do
If modeDn_btn = 1 and modeUp_btn = 0 Then
gosub Less ' if Less depressed
if motor1_Direction = motor1_mtr_off then
motor1_Direction = motor1_mtr_fwd
end if
end if
If modeDn_btn = 0 and modeUp_btn = 1 Then
gosub More ' if More depressed
if motor1_Direction = motor1_mtr_off then
motor1_Direction = motor1_mtr_fwd
end if
end if
If modeUp_btn = 1 and modeDn_btn = 1 Then
gosub Dark ' if both pushbuttons depressed
end if
' check motor is moving
gosub motor1_state
gosub motor2_state
gosub change_direction
readadc10 delay_pot, w13
w13 = w13 * 10
pause w13
loop
motor1_state:
gosub motor1_readadcval
motor_stall1 = motor_stall
if motor_stall < 2 then
if motor1_tempDuty = 0 then
motor1_tempDuty = motor1_Duty
end if
if motor1_pulseCount = 0 then
motor1_tempDuty = motor1_tempDuty + 15
end if
inc motor1_pulseCount
if motor1_pulseCount = 3 then
motor1_pulseCount = 0
end if
if motor1_tempDuty > 984 then
motor1_tempDuty = 984
end if
#ifdef serial_debug then
sertxd ("+",#motor1_tempDuty)
#endif
PWMOut motor1_pwmport, 249, motor1_tempDuty
else
'set speed to programmed speed
if motor1_tempDuty > motor1_Duty then
motor1_tempDuty = motor1_tempDuty - 15
#ifdef serial_debug then
sertxd ("-",#motor1_tempDuty)
#endif
PWMOut motor1_pwmport, 249, motor1_tempDuty
else
PWMOut motor1_pwmport, 249, motor1_Duty
motor1_tempDuty = 0
end if
end if
#ifdef serial_debug then
sertxd (13,10)
#endif
return
motor2_state:
gosub motor2_readadcval
motor_stall2 = motor_stall
if motor_stall < 2 then
if motor2_tempDuty = 0 then
motor2_tempDuty = motor2_Duty
end if
if motor2_pulseCount = 0 then
motor2_tempDuty = motor2_tempDuty + 15
end if
inc motor2_pulseCount
if motor2_pulseCount = 3 then
motor2_pulseCount = 0
end if
if motor2_tempDuty > 984 then
motor2_tempDuty = 984
end if
#ifdef serial_debug then
sertxd ("+",#motor2_tempDuty)
#endif
PWMOut motor2_pwmport, 249, motor2_tempDuty
else
'set speed to programmed speed
if motor2_tempDuty > motor2_Duty then
motor2_tempDuty = motor2_tempDuty - 15
#ifdef serial_debug then
sertxd ("-",#motor2_tempDuty)
#endif
PWMOut motor2_pwmport, 249, motor2_tempDuty
else
' adjust motors if they are rotating at different speeds
if motor_stall1 > 2 then
' sertxd ("adjust",#motor_stall1, " - ", #motor_stall2)
if motor_stall2 < motor_stall1 then
motor_stall = motor_stall1 - motor_stall2
motor2_Duty = motor2_Duty + motor_stall
' sertxd (" +:",#motor_stall)
endif
if motor_stall2 > motor_stall1 then
motor_stall = motor_stall2 - motor_stall1
motor2_Duty = motor2_Duty - motor_stall
' sertxd (" -:",#motor_stall)
endif
end if
PWMOut motor2_pwmport, 249, motor2_Duty
motor2_tempDuty = 0
end if
end if
#ifdef serial_debug then
sertxd (13,10)
#endif
return
motor1_readadcval:
PWMOut motor1_pwmport, 0, 0
select motor1_Direction
case motor1_mtr_off
' motor is off
motor_stall = 255
case motor1_mtr_fwd
readadc motor1_EMFport1, motor_stall
#ifdef serial_debug then
sertxd ("M1P1:")
#endif
case motor1_mtr_rev
readadc motor1_EMFport2 , motor_stall
#ifdef serial_debug then
sertxd ("M1P2:")
#endif
end select
#ifdef serial_debug then
sertxd ("ms=",#motor_stall)
#endif
return
motor2_readadcval:
PWMOut motor2_pwmport, 0, 0
select motor2_Direction
case motor2_mtr_off
' motor is off
motor_stall = 255
case motor2_mtr_fwd
readadc motor2_EMFport1, motor_stall
#ifdef serial_debug then
sertxd ("M2P1:")
#endif
case motor2_mtr_rev
readadc motor2_EMFport2 , motor_stall
#ifdef serial_debug then
sertxd ("M2P2:")
#endif
end select
#ifdef serial_debug then
sertxd ("ms=",#motor_stall)
#endif
return
change_direction:
If dir_btn = 1 Then
change_direction_init:
if motor1_Direction = motor1_mtr_rev then
high motor1_dirport
motor1_Direction = motor1_mtr_fwd
' same direction as motor 1
high motor2_dirport
motor2_Direction = motor1_mtr_fwd
elseif motor1_Direction = motor1_mtr_fwd then
low motor1_dirport
motor1_Direction = motor1_mtr_rev
' same direction as motor 1
low motor2_dirport
motor2_Direction = motor1_mtr_rev
end if
PWMOut motor1_pwmport, 249, motor1_Duty
PWMOut motor2_pwmport, 249, motor2_Duty
motor1_tempDuty = 0
motor2_tempDuty = 0
motor1_pulseCount = 0
motor2_pulseCount = 0
end if
return
Less:
if motor1_Direction <> motor1_mtr_off then
motor1_Duty = motor1_Duty - 30 ' otherwise decrease duty
end if
if motor2_Direction <> motor2_mtr_off then
motor2_Duty = motor2_Duty - 30 ' otherwise decrease duty
end if
If motor1_Duty <= minDuty Then
motor1_Duty = minDuty
end if
If motor2_Duty <= minDuty Then
motor2_Duty = minDuty
end if
#ifdef serial_debug then
sertxd ("L: ",#motor1_Duty, 13,10)
#endif
motor1_tempDuty = 0
motor2_tempDuty = 0
return
More:
If motor1_Duty > 984 Then
return ' maximum Duty ceiling
end if
If motor2_Duty > 984 Then
return ' maximum Duty ceiling
end if
if motor1_Direction <> motor1_mtr_off then
motor1_Duty = motor1_Duty + 30 ' otherwise increase duty
end if
if motor2_Direction <> motor1_mtr_off then
motor2_Duty = motor2_Duty + 30 ' otherwise increase duty
end if
#ifdef serial_debug then
sertxd ("M: ",#motor1_Duty, 13,10)
#endif
motor1_tempDuty = 0
motor2_tempDuty = 0
return
Dark:
motor1_Direction = motor1_mtr_off
PWMOut motor1_pwmport, 0, 0 ' zero the period (FULL STOP/DARK)
motor2_Direction = motor2_mtr_off
PWMOut motor2_pwmport, 0, 0 ' zero the period (FULL STOP/DARK)
return