Let's Make Robots!

PICAXE HWPM - 2 motors

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


Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

Thanx Jax.  I've got one on order.  I think that will solve that problem but I have a long way to go.

I just turned 72 and I want to get this baby finished before I'm another year older!

OK!  I just read voodoobot's entry of 3/19/2011.

Guess I need to trade in my 20X2 for a 28 pin model.

lol I just came to tell you that. The new 28X2s are much more capable in terms of PWM usage. The 20X2 is... ok (I have my misgivings about that chip) but it has some nuances that can get in the way.