mirror of
https://github.com/eclipse/upm.git
synced 2025-07-02 01:41:12 +03:00
l298: Initial implementation
This was tested on the RobotBase Dual H-Bridge motor control board. There are two examples: l298-example: This example demonstrates using the class to contol one of the H-Brdges to control a DC motor. l298-stepper-example: This example demonstrates using the class to control a 4-wire dual-phase stepper motor. Signed-off-by: Jon Trulson <jtrulson@ics.com> Signed-off-by: Zion Orent <zorent@ics.com> Signed-off-by: John Van Drasek <john.r.van.drasek@intel.com>
This commit is contained in:
Jon Trulson
committed by
John Van Drasek
John Van Drasek
parent
925260a234
commit
518d80cdcd
5
src/l298/CMakeLists.txt
Normal file
5
src/l298/CMakeLists.txt
Normal file
@ -0,0 +1,5 @@
|
||||
set (libname "l298")
|
||||
set (libdescription "upm l298 dual h-bridge")
|
||||
set (module_src ${libname}.cxx)
|
||||
set (module_h ${libname}.h)
|
||||
upm_module_init()
|
||||
8
src/l298/jsupm_l298.i
Normal file
8
src/l298/jsupm_l298.i
Normal file
@ -0,0 +1,8 @@
|
||||
%module jsupm_l298
|
||||
%include "../upm.i"
|
||||
|
||||
%{
|
||||
#include "l298.h"
|
||||
%}
|
||||
|
||||
%include "l298.h"
|
||||
335
src/l298/l298.cxx
Normal file
335
src/l298/l298.cxx
Normal file
@ -0,0 +1,335 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Copyright (c) 2015 Intel Corporation.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "l298.h"
|
||||
|
||||
using namespace upm;
|
||||
using namespace std;
|
||||
|
||||
// constructor for the DC motor(s) mode
|
||||
L298::L298(int pwmA, int dir1, int dir2)
|
||||
{
|
||||
// No stepper in this mode
|
||||
m_stepper = false;
|
||||
|
||||
// disable until complete
|
||||
m_motor = false;
|
||||
|
||||
if ( !(m_pwm = mraa_pwm_init(pwmA)) )
|
||||
{
|
||||
cerr << __FUNCTION__ << ": mraa_pwm_init(pwm) failed" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if ( !(m_dir1 = mraa_gpio_init(dir1)) )
|
||||
{
|
||||
cerr << __FUNCTION__ << ": mraa_gpio_init(dir1) failed" << endl;
|
||||
mraa_pwm_close(m_pwm);
|
||||
return;
|
||||
}
|
||||
mraa_gpio_dir(m_dir1, MRAA_GPIO_OUT);
|
||||
|
||||
if ( !(m_dir2 = mraa_gpio_init(dir2)) )
|
||||
{
|
||||
cerr << __FUNCTION__ << ": mraa_gpio_init(dir2) failed" << endl;
|
||||
mraa_pwm_close(m_pwm);
|
||||
mraa_gpio_close(m_dir1);
|
||||
return;
|
||||
}
|
||||
mraa_gpio_dir(m_dir2, MRAA_GPIO_OUT);
|
||||
|
||||
setPeriodMS(L298_DEFAULT_PWM_PERIOD);
|
||||
setDirection(DIR_NONE);
|
||||
setSpeed(0);
|
||||
m_motor = true;
|
||||
}
|
||||
|
||||
// constructor for the stepper mode
|
||||
L298::L298(int stepsPerRev, int en, int i1, int i2, int i3, int i4)
|
||||
{
|
||||
// no DC motors in this mode
|
||||
m_motor = false;
|
||||
|
||||
// disable until complete
|
||||
m_stepper = false;
|
||||
|
||||
m_stepsPerRev = stepsPerRev;
|
||||
m_currentStep = 0;
|
||||
m_stepDelay = 0;
|
||||
m_stepDirection = 1; // default is forward
|
||||
|
||||
// init the gpio's we will need
|
||||
if ( !(m_stepEnable = mraa_gpio_init(en)) )
|
||||
{
|
||||
cerr << __FUNCTION__ << ": mraa_gpio_init(en) failed" << endl;
|
||||
return;
|
||||
}
|
||||
mraa_gpio_dir(m_stepEnable, MRAA_GPIO_OUT);
|
||||
|
||||
if ( !(m_stepI1 = mraa_gpio_init(i1)) )
|
||||
{
|
||||
cerr << __FUNCTION__ << ": mraa_gpio_init(i1) failed" << endl;
|
||||
return;
|
||||
}
|
||||
mraa_gpio_dir(m_stepI1, MRAA_GPIO_OUT);
|
||||
|
||||
if ( !(m_stepI2 = mraa_gpio_init(i2)) )
|
||||
{
|
||||
cerr << __FUNCTION__ << ": mraa_gpio_init(i2) failed" << endl;
|
||||
mraa_gpio_close(m_stepI1);
|
||||
return;
|
||||
}
|
||||
mraa_gpio_dir(m_stepI2, MRAA_GPIO_OUT);
|
||||
|
||||
if ( !(m_stepI3 = mraa_gpio_init(i3)) )
|
||||
{
|
||||
cerr << __FUNCTION__ << ": mraa_gpio_init(i3) failed" << endl;
|
||||
mraa_gpio_close(m_stepI1);
|
||||
mraa_gpio_close(m_stepI2);
|
||||
return;
|
||||
}
|
||||
mraa_gpio_dir(m_stepI3, MRAA_GPIO_OUT);
|
||||
|
||||
if ( !(m_stepI4 = mraa_gpio_init(i4)) )
|
||||
{
|
||||
cerr << __FUNCTION__ << ": mraa_gpio_init(i4) failed" << endl;
|
||||
mraa_gpio_close(m_stepI1);
|
||||
mraa_gpio_close(m_stepI2);
|
||||
mraa_gpio_close(m_stepI3);
|
||||
|
||||
return;
|
||||
}
|
||||
mraa_gpio_dir(m_stepI4, MRAA_GPIO_OUT);
|
||||
|
||||
m_stepper = true;
|
||||
}
|
||||
|
||||
|
||||
void L298::initClock()
|
||||
{
|
||||
gettimeofday(&m_startTime, NULL);
|
||||
}
|
||||
|
||||
uint32_t L298::getMillis()
|
||||
{
|
||||
struct timeval elapsed, now;
|
||||
uint32_t elapse;
|
||||
|
||||
// get current time
|
||||
gettimeofday(&now, NULL);
|
||||
|
||||
// compute the delta since m_startTime
|
||||
if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 )
|
||||
{
|
||||
elapsed.tv_usec += 1000000;
|
||||
elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec;
|
||||
}
|
||||
|
||||
elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000));
|
||||
|
||||
// never return 0
|
||||
if (elapse == 0)
|
||||
elapse = 1;
|
||||
|
||||
return elapse;
|
||||
}
|
||||
|
||||
|
||||
L298::~L298()
|
||||
{
|
||||
if (m_stepper)
|
||||
{
|
||||
enable(false);
|
||||
mraa_gpio_close(m_stepEnable);
|
||||
mraa_gpio_close(m_stepI1);
|
||||
mraa_gpio_close(m_stepI2);
|
||||
mraa_gpio_close(m_stepI3);
|
||||
mraa_gpio_close(m_stepI4);
|
||||
}
|
||||
|
||||
if (m_motor)
|
||||
{
|
||||
setDirection(DIR_NONE);
|
||||
setSpeed(0);
|
||||
enable(false);
|
||||
mraa_pwm_close(m_pwm);
|
||||
mraa_gpio_close(m_dir1);
|
||||
mraa_gpio_close(m_dir2);
|
||||
}
|
||||
}
|
||||
|
||||
void L298::setPeriodMS(int ms)
|
||||
{
|
||||
if (m_motor)
|
||||
{
|
||||
if (mraa_pwm_period_ms(m_pwm, ms) != MRAA_SUCCESS)
|
||||
cerr << __FUNCTION__ << ": period specified is not supported"
|
||||
<< endl;
|
||||
}
|
||||
}
|
||||
|
||||
void L298::enable(bool enable)
|
||||
{
|
||||
if (m_motor)
|
||||
{
|
||||
mraa_pwm_enable(m_pwm, ((enable) ? 1 : 0));
|
||||
}
|
||||
|
||||
if (m_stepper)
|
||||
{
|
||||
mraa_gpio_write(m_stepEnable, ((enable) ? 1 : 0));
|
||||
}
|
||||
}
|
||||
|
||||
void L298::setSpeed(int speed)
|
||||
{
|
||||
if (m_motor)
|
||||
{
|
||||
if (speed < 0)
|
||||
speed = 0;
|
||||
|
||||
if (speed > 100)
|
||||
speed = 100;
|
||||
|
||||
float percent = float(speed) / 100.0;
|
||||
|
||||
if (m_motor)
|
||||
{
|
||||
mraa_pwm_write(m_pwm, percent);
|
||||
}
|
||||
}
|
||||
|
||||
if (m_stepper)
|
||||
{
|
||||
m_stepDelay = 60 * 1000 / m_stepsPerRev / speed;
|
||||
}
|
||||
}
|
||||
|
||||
void L298::setDirection(L298_DIRECTION_T dir)
|
||||
{
|
||||
if (m_motor)
|
||||
{
|
||||
if (dir & 0x01)
|
||||
mraa_gpio_write(m_dir1, 1);
|
||||
else
|
||||
mraa_gpio_write(m_dir1, 0);
|
||||
|
||||
if (dir & 0x02)
|
||||
mraa_gpio_write(m_dir2, 1);
|
||||
else
|
||||
mraa_gpio_write(m_dir2, 0);
|
||||
}
|
||||
|
||||
if (m_stepper)
|
||||
{
|
||||
switch (dir)
|
||||
{
|
||||
case DIR_CW:
|
||||
m_stepDirection = 1;
|
||||
break;
|
||||
case DIR_CCW:
|
||||
m_stepDirection = -1;
|
||||
break;
|
||||
default: // default to 1 if DIR_NONE specified
|
||||
m_stepDirection = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void L298::stepperStep()
|
||||
{
|
||||
int step = m_currentStep % 4;
|
||||
|
||||
// Step I0 I1 I2 I3
|
||||
// 1 1 0 1 0
|
||||
// 2 0 1 1 0
|
||||
// 3 0 1 0 1
|
||||
// 4 1 0 0 1
|
||||
|
||||
switch (step)
|
||||
{
|
||||
case 0: // 1010
|
||||
mraa_gpio_write(m_stepI1, 1);
|
||||
mraa_gpio_write(m_stepI2, 0);
|
||||
mraa_gpio_write(m_stepI3, 1);
|
||||
mraa_gpio_write(m_stepI4, 0);
|
||||
break;
|
||||
case 1: // 0110
|
||||
mraa_gpio_write(m_stepI1, 0);
|
||||
mraa_gpio_write(m_stepI2, 1);
|
||||
mraa_gpio_write(m_stepI3, 1);
|
||||
mraa_gpio_write(m_stepI4, 0);
|
||||
break;
|
||||
case 2: //0101
|
||||
mraa_gpio_write(m_stepI1, 0);
|
||||
mraa_gpio_write(m_stepI2, 1);
|
||||
mraa_gpio_write(m_stepI3, 0);
|
||||
mraa_gpio_write(m_stepI4, 1);
|
||||
break;
|
||||
case 3: //1001
|
||||
mraa_gpio_write(m_stepI1, 1);
|
||||
mraa_gpio_write(m_stepI2, 0);
|
||||
mraa_gpio_write(m_stepI3, 0);
|
||||
mraa_gpio_write(m_stepI4, 1);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void L298::stepperSteps(unsigned int steps)
|
||||
{
|
||||
while (steps > 0)
|
||||
{
|
||||
if (getMillis() >= m_stepDelay)
|
||||
{
|
||||
// reset the clock
|
||||
initClock();
|
||||
|
||||
m_currentStep += m_stepDirection;
|
||||
|
||||
if (m_stepDirection == 1)
|
||||
{
|
||||
if (m_currentStep >= m_stepsPerRev)
|
||||
m_currentStep = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_currentStep <= 0)
|
||||
m_currentStep = m_stepsPerRev;
|
||||
}
|
||||
|
||||
steps--;
|
||||
stepperStep();
|
||||
// cerr << "STEPNUM: " << m_currentStep << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
184
src/l298/l298.h
Normal file
184
src/l298/l298.h
Normal file
@ -0,0 +1,184 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Copyright (c) 2015 Intel Corporation.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include <mraa/gpio.h>
|
||||
#include <mraa/pwm.h>
|
||||
|
||||
// in milliseconds
|
||||
#define L298_DEFAULT_PWM_PERIOD 4
|
||||
|
||||
namespace upm {
|
||||
|
||||
/**
|
||||
* @brief C++ API for the L298 Dual H-Bridge module
|
||||
* @defgroup l298 libupm-l298
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief UPM module for the L298 Dual H-Bridge
|
||||
*
|
||||
* It was developed using the RobotBase Dual H-Bridge module.
|
||||
*
|
||||
* This module can support 2 DC motors, or 1 2-phase stepper motor.
|
||||
* It requires 3 pins per DC motor (or h-bridge), or 4 pins for
|
||||
* the stepper (uses both h-bridges).
|
||||
*
|
||||
* @ingroup l298 pwm gpio
|
||||
* Example driving a DC motor
|
||||
* @snippet l298.cxx Interesting
|
||||
* Example driving a stepper motor
|
||||
* @snippet l298-stepper.cxx Interesting
|
||||
*/
|
||||
|
||||
|
||||
class L298 {
|
||||
public:
|
||||
|
||||
/**
|
||||
* Enum to specify the direction of a motor
|
||||
*/
|
||||
typedef enum {
|
||||
DIR_NONE = 0x00,
|
||||
DIR_CW = 0x01,
|
||||
DIR_CCW = 0x02
|
||||
} L298_DIRECTION_T;
|
||||
|
||||
/**
|
||||
* L298 constructor for DC motor(s) connected.
|
||||
*
|
||||
* @param pwm digital pin to use for DC motor - must be PWM capable
|
||||
* @param dir1 digital pin to use for motor direction pin 1
|
||||
* @param dir2 digital pin to use for motor direction pin 2
|
||||
*/
|
||||
L298(int pwm, int dir1, int dir2);
|
||||
|
||||
/**
|
||||
* L298 constructor for 4-wire stepper motor
|
||||
*
|
||||
* @param stepsPerRev number of steps per full revolution
|
||||
* @param en enable pin
|
||||
* @param i1 digital pin to use for stepper input 1
|
||||
* @param i2 digital pin to use for stepper input 2
|
||||
* @param i3 digital pin to use for stepper input 3
|
||||
* @param i4 digital pin to use for stepper input 4
|
||||
*/
|
||||
L298(int stepsPerRev, int en, int i1, int i2, int i3, int i4);
|
||||
|
||||
/**
|
||||
* L298 Destructor
|
||||
*/
|
||||
~L298();
|
||||
|
||||
/**
|
||||
* Return the number of milliseconds elapsed since initClock()
|
||||
* was last called.
|
||||
*
|
||||
* @return elapsed milliseconds
|
||||
*/
|
||||
uint32_t getMillis();
|
||||
|
||||
/**
|
||||
* Reset the Clock
|
||||
*
|
||||
*/
|
||||
void initClock();
|
||||
|
||||
/**
|
||||
* set the period in milliseconds
|
||||
*
|
||||
* @param ms period in milliseconds
|
||||
*/
|
||||
void setPeriodMS(int ms);
|
||||
|
||||
/**
|
||||
* enable PWM output for a motor
|
||||
*
|
||||
* @param enable enable PWM output if true, disable if false
|
||||
*/
|
||||
void enable(bool enable);
|
||||
|
||||
/**
|
||||
* set the speed of a DC or stepp motor. For a DC motor, Values
|
||||
* can range from 0 (off) to 100 (full speed). For a stepper
|
||||
* motor, specify the desired RPM.
|
||||
*
|
||||
* @param speed speed to set the motor to
|
||||
*/
|
||||
void setSpeed(int speed);
|
||||
|
||||
/**
|
||||
* set the direction of the motor, clockwise or counter clockwise
|
||||
*
|
||||
* @param dir direction to set the motor to
|
||||
*/
|
||||
void setDirection(L298_DIRECTION_T dir);
|
||||
|
||||
/**
|
||||
* step the stepper motor a specified number of steps
|
||||
*
|
||||
* @param steps number of steps to move the stepper motor
|
||||
*/
|
||||
void stepperSteps(unsigned int steps);
|
||||
|
||||
private:
|
||||
// DC motor mode enabled
|
||||
bool m_motor;
|
||||
// stepper mode enabled
|
||||
bool m_stepper;
|
||||
|
||||
struct timeval m_startTime;
|
||||
|
||||
// DC motor
|
||||
mraa_pwm_context m_pwm;
|
||||
mraa_gpio_context m_dir1;
|
||||
mraa_gpio_context m_dir2;
|
||||
|
||||
// stepper (4-wire)
|
||||
mraa_gpio_context m_stepEnable;
|
||||
mraa_gpio_context m_stepI1;
|
||||
mraa_gpio_context m_stepI2;
|
||||
mraa_gpio_context m_stepI3;
|
||||
mraa_gpio_context m_stepI4;
|
||||
|
||||
// steps per revolution
|
||||
int m_stepsPerRev;
|
||||
int m_currentStep;
|
||||
uint32_t m_stepDelay;
|
||||
|
||||
/**
|
||||
* step the motor one tick
|
||||
*
|
||||
*/
|
||||
void stepperStep();
|
||||
|
||||
// step direction - 1 = forward, -1 = backward
|
||||
int m_stepDirection;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
9
src/l298/pyupm_l298.i
Normal file
9
src/l298/pyupm_l298.i
Normal file
@ -0,0 +1,9 @@
|
||||
%module pyupm_l298
|
||||
%include "../upm.i"
|
||||
|
||||
%feature("autodoc", "3");
|
||||
|
||||
%include "l298.h"
|
||||
%{
|
||||
#include "l298.h"
|
||||
%}
|
||||
Reference in New Issue
Block a user