Shop OBEX P1 Docs P2 Docs Learn Events
Cannot make the Wheel_Controller.spin to work with FullDuplexSerial — Parallax Forums

Cannot make the Wheel_Controller.spin to work with FullDuplexSerial

VovikVovik Posts: 4
edited 2014-11-30 08:56 in Robotics
I am trying to use the Motor Mount and Wheel Kit (#27971) with Position Controllers (#29319).
Trying to use the object I have got from here http://obex.parallax.com/object/663

The problem is when I am just trying to add a serial terminal to the WC_Test_Harness.spin to be able to query position of a wheel and see in terminal the wheel does not rotate in spite I have just only add FullDuplexSerial to the OBJ block and add a line to start it. Without those 2 lines wheel rotates as programmed.

The test harness stops working as soon as I try to add this two lines to the example WC_Test_Harness.spin (the_____ pst: "FullDuplexSerial" _____ to OBJ block and ____pst.Start(31, 30, 0, 115200)_____ to the PUB MAIN)

...
...
OBJ

WCtrl: "Wheel_Controller"
pst: "FullDuplexSerial"
...
...
PUB MAIN | success
..
...
pst.Start(31, 30, 0, 115200)
...
...


Basically once those 2 lines with pst.... added to the test harness it does not rotate wheels at WCtrl.spin_turn(WCtrl#LEFT_TURN, 360) or any other WCtrl commands.

Does anybody knows what could be a problem?

By the way the similar situation happens when I am trying to add a Ping.spin object ( the one from the "Ping Demo")


Does anybody know

Comments

  • ElectrodudeElectrodude Posts: 1,646
    edited 2014-11-29 10:47
    How many other cogs are you using? I suspect you have run out of cogs.

    If not, can you please post all of your code?
  • VovikVovik Posts: 4
    edited 2014-11-30 08:56
    Here it is the code. Basically it is just the code from http://obex.parallax.com/object/663 with only 2 lines added.

    When I have add the line "pst: "FullDuplexSerial" to the object block all still works ok...
    But when in addition I have add "pst.Start(31, 30, 0, 115200)" to the PUB MAIN it behaves like this:

    1. upon Run / Compile it compiles successfuly
    2. then it runs and light the RED LED (as well as expected)
    3. Them when I push the button it blionks the RED LED (while the expected result it should light the GREEN LED and move wheels).

    The WC_Test_Harness.spin code is here (as well I attach the both files of the project for your convenience).
    '''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' WC_TestHarness.spin
''
'' This test harness is designed to exercise the Wheel Controller object.
''
'' The cart uses a Propeller Education Kit board with:
''
''      1. Push button on pin zero
''      2. Red LED on pin 2
''      3. Green LED on pin 3
''      2. Position Controllers on pin 27
''
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

CON
   
  _clkmode = xtal1 + pll16x      ' Crystal feedback & PLL multiplier
  _xinfreq = 5_000_000           ' 5 MHz crystal

  START_BUTTON = 8
  RED_LED = 9
  GREEN_LED = 10

VAR

  ' Global Variables:

  long WC_Stat                  ' Wheel Controller Status
  word RobotSpeed
  long loop_cnt
  long FreezeCount
  long LastStatus 
  long Delta
  long Val
  
OBJ
                        
  WCtrl: "Wheel_Controller"
  pst: "FullDuplexSerial"
  
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' Main Entry

PUB MAIN | success

  dira~
  outa~
  dira[RED_LED..GREEN_LED]~~    ' Set LED pins to output
                           
  FreezeCount := 0
  LastStatus := 0
                 
  waitcnt(clkfreq + cnt)
           
  success := WCtrl.open(0)     ' Initialize Wheel Controller
  if success == FALSE 
    repeat
      outa[RED_LED] := 1
      waitcnt(clkfreq/2 + cnt)
      outa[RED_LED] := 0
      waitcnt(clkfreq/2 + cnt)
                                                      
  waitcnt(clkfreq + cnt)

  pst.Start(31, 30, 0, 115200)                        'Start serial communication, and wait 2 s for Parallax Serial Terminal connection.   

  ' Loop forever.
    
  repeat

    ' Turn on the Red LED to indicate that we are stopped.
    
    outa[RED_LED] := 1
    outa[GREEN_LED] := 0 

    ' Wait for the Start Button to be pressed.
    
    repeat
      waitcnt(clkfreq/4 + cnt)
      if ina[START_BUTTON]
        if WC_Stat == 0

          ' This is the first time the Start Button has been pressed
          ' so the Wheel Controller is started.
          
          WCtrl.start    
          waitcnt(clkfreq + cnt)
          WC_Stat := WCtrl.get_Status
        QUIT
      Check_Status  

    ' Turn on Green LED to indicate we are moving.
                 
    outa[RED_LED] := 0                          
    outa[GREEN_LED] := 1
     
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''                                                           
'' The following test will command each wheel to go 200 encode units.
{   
    WCtrl.travel_units(200, WCtrl#RIGHT_WHEEL)
    WaitForArrival
     
    WCtrl.travel_units(200, WCtrl#LEFT_WHEEL)
    WaitForArrival
}    
                             
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''                                                           
'' The following test will travel in a five foot square with a 360 degree
'' right turn at the start and a 270 degree left turn at the end:
                                             
    WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 176) ' 2 mile/hour
            
    WCtrl.spin_turn(WCtrl#LEFT_TURN, 360)
    WaitForArrival
                                            
    WCtrl.go_Distance(60, WCtrl#FORWARD)
    WaitForArrival
                                                   
    WCtrl.spin_turn(WCtrl#RIGHT_TURN, 90)
    WaitForArrival
                                              
    WCtrl.go_Distance(60, WCtrl#FORWARD)
    WaitForArrival
                                                   
    WCtrl.spin_turn(WCtrl#RIGHT_TURN, 90)
    WaitForArrival
                                            
    WCtrl.go_Distance(60, WCtrl#FORWARD)
    WaitForArrival
                                                   
    WCtrl.spin_turn(WCtrl#RIGHT_TURN, 90)
    WaitForArrival
                                              
    WCtrl.go_Distance(60, WCtrl#FORWARD)
    WaitForArrival
                                                     
    WCtrl.spin_turn(WCtrl#LEFT_TURN, 270)
    WaitForArrival
  
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' The following test will:
''
''    1. Set speed to 100 ft/min
''    2. Go forward
''    3. Turn to the right until the right wheel stops
''    4. Turn back to the left until the left wheel stops
''    5. Stop all wheels
''    6. Wait for all wheels to come to a complete stop
{      
    WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 100)  'Set speed to 100 ft/min
    Delta := 10 
    Val := 0      
    WCtrl.go_Distance(2000, WCtrl#FORWARD)  'Go forward for long distance
    
    repeat
      waitcnt(clkfreq/2 + cnt)
      Val += Delta 
      WCtrl.turn(Val)
      if Val => 100
        Delta := -10
      elseif Val =< -100
        quit
                              
    WCtrl.stop 
    WaitForArrival
}                                
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' The following test will:
''    1. travel forward for 4 seconds
''    2. travel backward for 4 seconds
''    3. spin turn 360 degrees to the left
                                                         
    loop_cnt := 0       
    WCtrl.go_Distance(2000, WCtrl#FORWARD)
    repeat
      waitcnt(clkfreq + cnt)
      loop_cnt++
      if loop_cnt == 4
        quit
    WCtrl.stop 
    WaitForArrival
                                                           
    loop_cnt := 0       
    WCtrl.go_Distance(2000, WCtrl#REVERSE)
    repeat
      waitcnt(clkfreq + cnt)
      loop_cnt++
      if loop_cnt == 4
        quit
    WCtrl.stop 
    WaitForArrival
                                                         
    WCtrl.spin_turn(WCtrl#LEFT_TURN, 360) 
    WaitForArrival
   
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' The following test will:
''     1. set speed to one mile per hour
''     2. start forward
''     3. at 2 seconds increase speed to two miles per hour
''     4. at 4 seconds increase speed to three miles per hour
''     5. at 6 seconds decrease speed to two miles per hour
''     6. at 8 seconds decrease speed to one mile per hour
''     3. at 10 seconds stop
{                                                             
    WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 88)
    
    loop_cnt := 0       
    WCtrl.go_Distance(2000, WCtrl#FORWARD)
    repeat
      waitcnt(clkfreq + cnt)
      loop_cnt++
      if loop_cnt == 2                                        
        WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 176)  
      elseif loop_cnt == 4                                    
        WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 264) 
      elseif loop_cnt == 6                                    
        WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 176) 
      elseif loop_cnt == 8                                    
        WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 88)
      elseif loop_cnt == 10
        quit
    WCtrl.stop 
    WaitForArrival                      
}                                                                                 

'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' The following test will move the cart in a arc to the right and then to
'' the left.
                                                                           
    WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 0)    
    WCtrl.turn(0)                                                                          
    WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 100) 
    WCtrl.turn(20)                               
    WCtrl.go_Distance(60, WCtrl#FORWARD)
    WaitForArrival
                                                  
    WCtrl.turn(-20)                              
    WCtrl.go_Distance(60, WCtrl#FORWARD)
    WaitForArrival
    WCtrl.turn(0)
                                     
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' The following test will test the throttle during a straight run:
''
''    1. Set speed to zero
''    2. Go forward
''    3. Throttle up to 120 percent 
''    4. Stop all wheels
''    5. Wait for all wheels to come to a complete stop
              
 {
    WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 0)
    Delta := 5 
    Val := 0      
    WCtrl.go_Distance(2000, WCtrl#FORWARD)  'Go forward for long distance
    
    repeat
      waitcnt(clkfreq + cnt)
      Val += delta
      WCtrl.throttle(val)
      if Val => 120
        quit
                              
    WCtrl.stop 
    WaitForArrival 
}

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' The following test will test the throttle during a turn:
''
''    1. Set speed to zero
''    2. Set turn bias to 25 percent
''    2. Go forward
''    3. Throttle up to 120 percent 
''    4. Stop all wheels
''    5. Wait for all wheels to come to a complete stop
{              
    WCtrl.set_WheelSpeed(WCtrl#ALL_WHEELS, 0)
    Delta := 5 
    Val := 0 
    WCtrl.turn(25)      
    WCtrl.go_Distance(2000, WCtrl#FORWARD)  'Go forward for long distance
    
    repeat
      waitcnt(clkfreq + cnt)
      Val += Delta
      WCtrl.throttle(Val)
      if Val => 120
        quit
                              
    WCtrl.stop 
    WaitForArrival
}        
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' This method will wait for the arrival flag to get set.  It will periodically
'' ckeck the Wheel Controller Cog to determine if it has failed.

PRI WaitForArrival

  repeat

    ' Wait or do other processing.   
         
    waitcnt(clkfreq/4 + cnt)
    
    if WCtrl.arrival_check <> 0
      quit
      
    Check_Status

'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'' Check Status Method -- this method will determine if the Wheel Controller Cog
'' has stopped.  If it has stopped then the Red LED is flashed and any further
'' processing suspended.  This will most likely be caused by a low battery
'' condition.  A user may not want to stop here but instead go on to some sort of
'' recovery processing.
''
'' Note that calling the Wheel Controller 'start' method will reset it.

PRI Check_Status | r1

  r1 := WCtrl.get_Status
  if r1 == 0
    return                      ' Wheel Controller not started yet
  
  if r1 == LastStatus
    FreezeCount++
    if FreezeCount == 6  

    ' Wheel Controller Failure
      
      outa[RED_LED] := 1
      outa[GREEN_LED] := 0
      repeat
        outa[RED_LED] := 1
        waitcnt(clkfreq/2 + cnt)
        outa[RED_LED] := 0 
        waitcnt(clkfreq/2 + cnt)
  else                     
    FreezeCount := 0
             
  LastStatus := r1
    spin
    spin
    Wheel_Controller.spin
    61K
    spin
    spin
    WC_Test_Harness.spin
    11K
Sign In or Register to comment.