Twin DC Bidirectional Motor Controller with back EMF using an 18m2

Anobium

Senior Member
Code for control of DC motor using back emf using 18m2.

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

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
 
Top