Merge pull request #6 from timmmoore/board-detect

board detect before usage Tim, good find thank you!
ironsheep · Sep 10, 2023 · c82dfd5 · c82dfd5
2 parents 5b6604e + ed4562c
commit c82dfd5
Showing 1 changed file with 21 additions and 40 deletions.
diff --git a/src/isp_bldc_motor.spin2 b/src/isp_bldc_motor.spin2
@@ -104,27 +104,14 @@ PUB start(eMotorBasePin, eMotorVoltage, eDetectionMode) : ok | legalBase
 '' Specify motor control board connect location for this motor and start the driver
         ' setup runtime constants
     init(eMotorBasePin, eMotorVoltage, eDetectionMode)
+    params_ptr := @offset_fwd   ' point to driver variables
+    setTargetAccel(0)           ' do NOT move at startup
 
-    ' ensure we only use legal values
-    legalBase := eMotorBasePin
-    CASE eMotorBasePin
-        PINS_P0_P15 :
-        PINS_P16_P31 :
-        PINS_P32_P47 :
-        OTHER :
-            legalBase := INVALID_PIN_BASE
-
-    ' if legal base-pin then start cog
-    if legalBase <> INVALID_PIN_BASE
-        pinbase := legalBase        ' record base pin number
-        params_ptr := @offset_fwd   ' point to driver variables
-        setTargetAccel(0)           ' do NOT move at startup
-
-        ok := motorCog := coginit(NEWCOG, @driver, @pinbase) + 1
-        if motorCog == 0    ' did fail?
-            debug("!! ERROR filed to start Motor Control task")
-        else
-            debug("* Motor COG #", sdec_(motorCog- 1))
+    ok := motorCog := coginit(NEWCOG, @driver, @pinbase) + 1
+    if motorCog == 0    ' did fail?
+        debug("!! ERROR filed to start Motor Control task")
+    else
+        debug("* Motor COG #", sdec_(motorCog- 1))
 
 PUB stop()
 '' Stop cogs and release pins assigned to this motor
@@ -145,18 +132,12 @@ PUB stop()
 PUB testSetup(eMotorBasePin, eMotorVoltage, eDetectionMode) | legalBase
 '' Specify motor control board connect location for this motor BUT DON'T start the driver
     init(eMotorBasePin, eMotorVoltage, eDetectionMode)
-    ' ensure we only use legal values
-    legalBase := eMotorBasePin
-    CASE eMotorBasePin
-        PINS_P0_P15 :
-        PINS_P16_P31 :
-        PINS_P32_P47 :
-        OTHER :
-            legalBase := INVALID_PIN_BASE
 
-    ' if legal base-pin then start cog
-    if legalBase <> INVALID_PIN_BASE
-        pinbase := legalBase        ' record base pin number
+PRI validatePinBase(eMotorBasePin) : legalBase
+
+  legalBase := VALUE_NOT_SET
+  if lookdown(eMotorBasePin: PINS_P0_P15, PINS_P16_P31, PINS_P32_P47) <> 0
+    legalBase := eMotorBasePin
 
 PRI init(eMotorBasePin, eMotorVoltage, eDetectionMode) | fwdDegrees, revDegrees
 '' Specify motor control board connect location for this motor BUT DON'T start the driver
@@ -169,7 +150,12 @@ PRI init(eMotorBasePin, eMotorVoltage, eDetectionMode) | fwdDegrees, revDegrees
     ' set up debug variables
     useDebug := FALSE
     showHDMIDebug := FALSE
-    pinbase := VALUE_NOT_SET        ' record base pin number
+    pinbase := validatePinBase(eMotorBasePin)           ' record base pin number
+
+    eUserDetectionMode, bUserForced := getInternalDetectMode(eDetectionMode)
+
+    ' set the mode now used by the driver to condition things (current-sense scaling, configpowerLimits, etc.)
+    eDetectedBoard := getBoardType()
 
     bResetSenseData := FALSE
 
@@ -276,11 +262,6 @@ PRI init(eMotorBasePin, eMotorVoltage, eDetectionMode) | fwdDegrees, revDegrees
 
     confgurePowerLimits(user.DRIVE_VOLTAGE)
 
-    eUserDetectionMode, bUserForced := getInternalDetectMode(eDetectionMode)
-
-    ' set the mode now used by the driver to condition things (current-sense scaling, etc.)
-    eDetectedBoard := getBoardType()
-
     rSenseForBoard := VALUE_NOT_SET
     if eDetectedBoard == REV_A
         rSenseForBoard := F_REV_A_RSENSE
@@ -903,8 +884,8 @@ PRI offsetsForMotor(eMotorType) : fwdDegrees, revDegrees | ofsDegr
         }
 
         ' Chips values (orig driver)
-        ' CHIP ORIG: offset_fwd	long	280 frac 360			'280 frac 360 (Doug's motor) 20
-        ' CHIP ORIG: offset_rev	long	230 frac 360			'230 frac 360 (Doug's motor) 340
+        ' CHIP ORIG: offset_fwd long    280 frac 360                    '280 frac 360 (Doug's motor) 20
+        ' CHIP ORIG: offset_rev long    230 frac 360                    '230 frac 360 (Doug's motor) 340
 
         ' my values from characterization at 18v5:
         ofsDegr := 43
@@ -2501,4 +2482,4 @@ CON { license }
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
   SOFTWARE.
  =================================================================================================
-}}
+}}