'' ================================================================================================= '' '' File....... jm_dual_motor_ez.spin '' Purpose.... '' Author..... Jon "JonnyMac" McPhalen '' Copyright (c) 2011-2020 Jon McPhalen '' -- see below for terms of use '' E-mail..... jon@jonmcphalen.com '' Started.... '' Updated.... 20 JUN 2020 '' '' ================================================================================================= con { fixed io pins } RX1 = 31 ' serial / programming TX1 = 30 SDA1 = 29 ' i2c / eeprom SCL1 = 28 con #0, FORWARD, REVERSE #0, COAST, RUN var long cog ' id of pwm cog long stack[32] ' stack for Spin cog long m0enable ' M0 h-bridge enable (optional) long m0pwm ' M0 pwm (speed) long m0dir ' M0 direction long m1enable ' M1 h-bridge enable (optional) long m1pwm ' M1 pwm (speed) long m1dir ' M1 direction long pwmticks ' sets pwm period long m0ticks ' M0 "on" ticks long m1ticks ' M1 "on" ticks pub null '' This is not a top-level object pub start(e0, p0, d0, e1, p1, d1, freq) '' Start dual motor driver '' -- ex are h-bridge enable pins (-1 if not used) '' -- px pins are for pwm output (-1 if not used) '' -- dx pins are direction control (-1 if not used) '' -- freq is pwm frequency (<= ~35kHz) stop ' stop cog if already running if ((p0 < 0) or (d0 < 0)) ' disable if no speed or direction pins longfill(@e0, -1, 3) if ((p1 < 0) or (d1 < 0)) longfill(@e1, -1, 3) longmove(@m0enable, @e0, 6) ' copy pins pwmticks := clkfreq / freq ' set ticks in pwm cycle cog := cognew(dual_pwm, @stack) + 1 ' start the pwm cog return cog pub stop | idx, pin '' Stop pwm cog '' -- unloads pwm cog '' -- clears previously assigned pins if (cog) ' if running cogstop(cog - 1) ' stop the pwm cog cog := 0 ' mark stopped repeat idx from 0 to 5 pin := long[@m0enable][idx] ' cycle through pins if (pin => 0) ' if defined input(pin) ' clear pin driver longfill(@m0enable, -1, 6) ' disable all pins longfill(@pwmticks, 0, 3) ' clear pwm values pub set_enable(motor, state) '' Set enable pin for h-bridge '' -- ignored if enable pin for motor is not defined if ((motor == 0) and (m0enable => 0)) ' motor 1 & enable pin defined? if (state) high(m0enable) ' enable h-bridge outputs else low(m0enable) ' disable h-bridge outputs if ((motor == 1) and (m1enable => 0)) if (state) high(m1enable) else low(m1enable) pub set_brake(motor, state) '' Apply "braking" to motor setting both motor outputs high '' -- warning: motor should be slowed to stop before using this method if (motor == 0) set_speed(0, 0) ' ensure motor is stopped waitcnt(cnt + pwmticks) ' let current cycle finish if (state) ' if braking high(m0pwm) ' override pwm signal high(m0dir) ' apply brake else input(m0pwm) ' release to pwm signal low(m0dir) ' release brake elseif (motor == 1) set_speed(1, 0) waitcnt(cnt + pwmticks) if (state) high(m1pwm) high(m1dir) else input(m1pwm) low(m1dir) pub set_speed(motor, speed) '' Set motor speed in 0.1% increments (-100_0 to 100_0) speed := -100_0 #> speed <# 100_0 ' limit range -100.0% to +100.0% if (speed == 0) ' stop if (motor == 0) m0ticks := 0 ' set pwm cycle to 0 (always low) low(m0dir) elseif (motor == 1) m1ticks := 0 low(m1dir) elseif (speed > 0) ' forward speed := speed * pwmticks / 100_0 ' convert speed to pwm ticks if (motor == 0) m0ticks := -speed ' set speed for pwm cog low(m0dir) ' set direction to forward elseif (motor == 1) m1ticks := -speed low(m1dir) else ' reverse speed := (100_0 + speed) * pwmticks / 100_0 if (motor == 0) m0ticks := -speed ' set speed for pwm cog high(m0dir) ' set direction to reverse elseif (motor == 1) m1ticks := -speed high(m1dir) pri dual_pwm | t ' launch with cognew() '' Run dual-motor pwm '' -- pins must be defined before calling if (m0pwm => 0) ' m0 in use? ctra := %00100 << 26 | m0pwm ' set for pwm/nco mode frqa := 1 ' highest resolution phsa := 0 ' stop motor dira[m0pwm] := 1 ' make pwm pin output in this cog if (m1pwm => 0) ctrb := %00100 << 26 | m1pwm frqb := 1 phsb := 0 dira[m1pwm] := 1 ' pwm loop t := cnt ' sync pwm timing repeat phsa := m0ticks ' load pwm ticks phsb := m1ticks waitcnt(t += pwmticks) ' let pwm period expire pri high(pin) outa[pin] := 1 dira[pin] := 1 pri low(pin) outa[pin] := 0 dira[pin] := 1 pri input(pin) outa[pin] := 0 dira[pin] := 0 con { license } {{ Terms of Use: MIT License Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. }}