I'm having trouble trying to understand how to use HPWM to control both motors of my 2-wheeled robot.
I'm using a dual full-bridge motor driver. I want to be able to drive both motors at different speeds and even different directions. First of all - I see no reference in any of the PICAXE documentation to using the "enable" inputs of the motor driver. Documentation of the L298 motor driver chip seems to require use of the "enable" inputs. Secondly, I can't figure out how to have a different DUTY cycle for each motor. This seems to be a basic requirement to me.
Here is my code. I would appreciate any suggestions or criticism:
'Last modified - 02/03/2012 11:45
#PICAXE 28X2
#terminal 9600
#simspeed 200
Symbol servopin = 0 ' Defines output pin to which the servo turning the head is connected
Symbol speakerpin = 3 ' Defines output pin to which a speaker is connected
symbol trig = 4 ' Defines output pin for Trigger pulse of SRF05 - it can in fact be same as a motor is on, freeing up one pin
symbol echo = 0 ' Defines input pin for Echo pulse of SRF05
'=================DEFINITIONS======================
symbol EnableLeft = b.3 'Enable(Left/Right) pins to ENA and ENB on
symbol EnableRight = b.4 ' dual full-bridge driver board -High=enabled
symbol DriverA = c.1 'Driver(A/B/C/D) - Pins to IN1, IN2. IN3, IN4
symbol DriverB = b.1 ' on dual full-bridge driver board -
symbol DriverC = c.2 ' High=enabled
symbol DriverD = b.2
symbol DutyLeft = b0 ' Duty(Left/Right) - Duty period of PWM
symbol DutyRight = b1 ' signal incremented in 8 "Gaits" from
' zero (Stop) to maximum (FullOn)
' Stop (0), Walk(60), Amble(120),
' Trot(180), Canter(240), Gallup(300),
' Sprint(360), FullOn(400)
symbol RampUp = b2 ' If "on", DutyLeft and DutyRight will be
' incremented by 1/4 "Gait" each Loop ' through Main program loop
symbol RampDn = b3 ' If "on", DutyLeft and DutyRight will be
' decremented by 1/4 "Gait" each Loop
' through Main program loop
symbol GoMode = b4
symbol loopcnt = b5
symbol head = b6
symbol Remember = w4 ' Temporary variable just used in routines
symbol MayTurn = w5 ' An undocumented variable
'=======================================================
symbol servohead = 1 'Defines output pin for servo to turn SRF05
symbol range = w1 ' 16 bit word variable for range
symbol oldrange = w2 ' 16 bit word variable for "Where were we last time?"
symbol calcrange = w3 ' 16 bit word variable for "The difference"
symbol differencebit = 50 ' sets how little difference we can tolerate to call this "an object *not used* "
symbol kopbit = b25 ' this is overkill *not used*
'================ Symbols and variables set up to optimize performance
symbol ForwardLeft = 100 ' These set where "green and red" starts / ends
symbol ForwardRight = 200 ' -||- They should be trimmed so obstacles are
' just avoided, and be on each side of "150"
' which is "servo-middle"
symbol Danger = 200 ' Distance from which we say "Oops - too close"
symbol Safetydistance = 250 ' Distance from which we say "OK,reversed enough"
symbol ClearPathAhead = 300 ' Distance before starting again after a stop
symbol interesting = 500 ' How deep a space before we can call it
' "interesting" (such as a doorway in a wall)
symbol MaxNumberTurns = 5 'how many times should we turn before we just
' take an "interesting route"
'===============================================================
SET_UP:
' HPWM mode, polarity, setting, period, duty
HPWM 3,0,0,99,0
PWMDUTY C.1,DutyLeft
PWMDUTY C.2,DutyRight
Low EnableLeft
Low EnableRight
Let GoMode = 0
'===============================================================
PHASEA1:
'PHASEA1 is Main routine.
'Drive ahead!
'Pan head from left to right and back
'Constantly return sonar range, and jump out into the rest of the code
for Remember = 75 to 225 step 20
head = Remember
servo Servopin,head
servo 1,head
pause 15
gosub TESTER
next Remember
for Remember = 225 to 75 step -20
head = Remember
servo Servopin,head
servo 1,head
pause 15
gosub TESTER
next Remember
Goto PHASEA1
'===============================================================
PHASEA2:
If head => ForwardLeft and head < 150 and range < Danger then
goto FORWARDLEFT_MIDDLE
endif
If head => 150 and head <= ForwardRight and range < Danger then
goto FORWARDRIGHT_MIDDLE
endif
If head => ForwardRight and head <= 225 and range > ClearPathAhead then
goto SHARP_R
endif
If head => 75 and head < ForwardLeft and range > ClearPathAhead then
goto SHARP_L
endif
goto ERROR ' one of the above should be true, if not, something is wrong, this code will self-terminate ! :)
'===============================================================
FORWARDLEFT_MIDDLE: '''''''''Danger'''''''''''''''''''''
Gosub SPIN_RIGHT 'Gosub RIGHTSPIN
if head > ForwardLeft and head < 150 then
head = head - 15
pause 50
servo Servopin,head
servo 1,head
endif
gosub PULS
if range < Danger then
goto FORWARDLEFT_MIDDLE : gosub AFTERTURN
endif '*****added by DED ******
If MayTurn < MaxNumberTurns then
MayTurn = MayTurn + 1
endif
return
'===============================================================
FORWARDRIGHT_MIDDLE:
Gosub SPIN_LEFT 'Gosub LEFTSPIN
if head > 150 and head < ForwardRight then
head = head + 15
pause 50
servo Servopin,head
servo 1,head
endif
Gosub PULS
if range < Danger then
goto FORWARDRIGHT_MIDDLE
gosub AFTERTURN
endif
If MayTurn < MaxNumberTurns then
MayTurn = MayTurn + 1
endif
return
'===============================================================
SHARP_R:
If MayTurn > 0 then
Gosub SPIN_RIGHT 'Gosub RIGHTSPIN
endif
If head < ForwardLeft and head => 75 then
head = head + 30
pause 50
servo Servopin,head
servo 1,head
endif
gosub PULS
if range > interesting then
goto SHARP_R:
gosub AFTERTURN
MayTurn = MayTurn - 1
endif
return
'===============================================================
SHARP_L:
If MayTurn > 0 then
Gosub SPIN_LEFT
endif 'Gosub LEFTSPIN
If head => ForwardRight and head < 225 then
head = head - 30
pause 50
servo Servopin,head
servo 1,head
endif
gosub PULS
if range > interesting then
goto SHARP_L:
gosub AFTERTURN
MayTurn = MayTurn - 1
endif
return
'===============================================================
AFTERTURN:
Gosub MOVE_FORWARD 'Gosub DRIVEAHEAD
return
'===============================================================
TESTER: 'Things that should be looked for all the time
' If a bumper is activated etc..
If RampUp = 1 then '
DutyLeft = DutyLeft + 15 Max 400
DutyRight = DutyRight + 15 Max 400
endif
If RampDn = 1 then
DutyLeft = DutyLeft - 15 Min 60
DutyRight = DutyRight - 15 Min 60
endif
PWMDUTY C.1,DutyLeft
PWMDUTY C.2,DutyRight
gosub PULS
if range < Danger and head > ForwardLeft and head < ForwardRight then
gosub AFTERTURN
gosub PHASEA2
end if
if range > ClearPathAhead and head > ForwardRight and head <= 225 then
gosub AFTERTURN
gosub PHASEA2
end if
if range > ClearPathAhead and head < ForwardLeft and head >= 75 then
gosub AFTERTURN
gosub PHASEA2
end if
'===============================================================
PULS:
pulsout trig,1 ' produce 20uS trigger pulse
pulsin echo,1,range ' measures the range in 10uS steps
pause 10
return
'===============================================================
ERROR:
gosub TOTALHALT
sound speakerpin,(120,10): low speakerpin
wait 1
goto ERROR
'===============================================================
' Set these up to match your hardware
TOTALHALT:
Gosub HALTX 'low 4 : low 5 : low 6 : low 7
return
RIGHTSPIN:
select case range
Case 0 to 50
Gosub MOVE_BACKWARD 'high 5 : low 4 : high 6 :low 7 ' Reverse
Case 51 to 150
Gosub SPIN_RIGHT 'low 4 : high 5 : high 7 : low 6 Spin right.
Case > 150
Gosub VEER_RIGHT 'low 4 : low 5 : high 7 : low 6 Slide right.
endselect
return
LEFTSPIN:
select case range
Case 0 to 50
Gosub MOVE_BACKWARD 'high 5 : low 4 : high 6 :low 7 Reverse
Case 51 to 150
Gosub SPIN_LEFT 'low 5 : high 4 : high 6 : low 7 Spin left
Case > 150
Gosub VEER_LEFT 'low 5 : high 4 : low 6 : low 7 Slide left
endselect
return
DRIVEAHEAD:
Gosub MOVE_FORWARD 'high 4 : low 5 : high 7 :low 6
return
goto ERROR 'Make sure we don't fall through
MOVE_FORWARD: 'rotate both wheels forward, gradually increasing speed
Low EnableLeft : Low EnableRight
Let DutyLeft = 60 : Let DutyRight = 60 ' Start at Walk gait
High DriverA : Low DriverB : High DriverC : Low DriverD
High EnableLeft : High EnableRight
Let RampUp = 1
SerOut 0, T2400, ( "Forward " ) 'For testing
Let GoMode = 1
Return
MOVE_BACKWARD: ' rotate both wheels backward, maintain Walk gait
Low EnableLeft : Low EnableRight
Let DutyLeft = 60 : Let DutyRight = 60
Low DriverA : High DriverB : Low DriverC : High DriverD
High EnableLeft : High EnableRight
Let RampUp = 0
SerOut 0, T2400, ( "Backward " ) 'For testing
Let GoMode = 2
Return
VEER_LEFT: 'If DutyLeft > Walk, reduce DutyLeft 1 Gait
' otherwise increase DutyRight 1 Gait
If DutyLeft > 60 Then
DutyLeft = DutyLeft - 60
Else
DutyRight = DutyRight + 60
Endif
SerOut 0, T2400, ( "Veer Left ") 'For testing
Let GoMode = 3
Return
VEER_RIGHT: ' If DutyRight > Walk, reduce DutyRight 1 Gait,
' otherwise increase DutyLeft 1 Gait
If DutyRight > 60 Then
DutyRight = DutyRight - 60
Else
DutyLeft = DutyLeft + 60 Max 400
Endif
SerOut 0, T2400, ( "Veer Right " ) 'For testing
Let GoMode = 4
Return
CIRCLE_LEFT: 'Set DutyLeft to Walk - Set DutyRight to Amble
' - hold this mode for 1/4 (90 deg.) turn
Let DutyLeft = 60
Let DutyRight = 120 : Pause 1000 'Length of time to be determined
SerOut 0, T2400, ( "Circle Left ") 'For testing
Let GoMode = 5
Return
CIRCLE_RIGHT: 'Set DutyRight to Walk - Set DutyLeft to Amble
'- hold this mode for 1/4 (90 deg.) turn
Let DutyLeft = 120
Let DutyRight = 60 : Pause 1000 'Length of time to be determined
SerOut 0, T2400, ( "Circle Right ") 'For testing
Let GoMode = 6
Return
ROTATE_LEFT: 'Set DutyLeft to Stop - Set DutyRight to Walk
Let DutyLeft = 0 ' - hold this mode for 1/4 (90 deg.) turn
Let DutyRight = 60 : Pause 1000 'Length of time to be determined
SerOut 0, T2400, ( "Rotate Left ") 'For testing
Let GoMode = 7
Return
ROTATE_RIGHT: 'Set DutyRight to Stop - Set DutyLeft to Walk
Let DutyLeft = 60 ' - hold this mode for 1/4 (90 deg.) turn
Let DutyRight = 0 : Pause 1000 'Length of time to be determined
SerOut 0, T2400, ( "Rotate Right ") 'For testing
Let GoMode = 8
Return
SPIN_LEFT: 'rotate right wheel forward and left wheel backward
' (slow) - hold this mode for 1/4 (90 deg.) turn
Gosub HALTX ' first HALT
Low EnableLeft : Low EnableRight
Let DutyLeft = 60
Let DutyRight = 60
Low DriverA : High DriverB : High DriverC : Low DriverD
High EnableLeft : High EnableRight
Pause 1000 'Length of time to be determined
Let RampUp = 0
SerOut 0, T2400, ( "Spin Left ") 'For testing
Let GoMode = 9
Return
SPIN_RIGHT: 'rotate left wheel forward and right wheel
'backward (slow) - hold this mode for 1/4 (90 deg.) turn
Gosub HALTX 'first HALT -
Low EnableLeft : Low EnableRight
Let DutyLeft = 60
Let DutyRight = 60
High DriverA : Low DriverB : Low DriverC : High DriverD
High EnableLeft : High EnableRight
Pause 1000 'Length of time to be determined
Let RampUp = 0
SerOut 0, T2400, ( "Spin Right ") 'For testing
Let GoMode = 10
Return
HALTX: 'Set DutyLeft to Stop - Set DutyRight to Stop
Let DutyLeft = 0
Let DutyRight = 0
SerOut 0, T2400, ( "Halt ") 'For testing
Let GoMode = 11
Return
BRAKE: '(Collision has occurred or is eminent)
Low EnableLeft ' set all DriverPins Low Hold this mode for 5 seconds
Low EnableRight
Let DutyLeft = 60
Let DutyRight = 60
Low DriverA : Low DriverB : Low DriverC : Low DriverD
High EnableLeft : High EnableRight
SerOut 0, T2400, ( "Brake ") 'For testing
Let GoMode = 12
Pause 5000
Return
END