Skip to content

Instantly share code, notes, and snippets.

@reefwing
Created December 10, 2015 07:34
Show Gist options
  • Save reefwing/1589456f6fb2e359e8c7 to your computer and use it in GitHub Desktop.
Save reefwing/1589456f6fb2e359e8c7 to your computer and use it in GitHub Desktop.
Arduino Library for the Parallax HB-25 Motor Control
// HB-25 Library Test
//
// Version: 1.1
// Date: 10th December 2015
//
// Valid speed ranges for the forwardAtSpeed and reverseAtSpeed methods are
// 0 (stop) to 500 (maximum speed). For rampToSpeed and moveAtSpeed you can use from -500 (full
// reverse) to 500 (full forward). As before, a speed of 0 will stop the motor.
//
// Remember to call the begin() method in setup().
//
// Adapted from a sketch by zoomkat 10-22-11 serial servo test
#include <Servo.h> // You need to include Servo.h as it is used by the HB-25 Library
#include <HB25MotorControl.h>
const byte controlPin = 9; // Pin Definition
String readString;
HB25MotorControl motorControl(controlPin);
void setup() {
Serial.begin(9600);
Serial.println("HB-25 Motor Control Library Test");
Serial.println("Enter a speed between -500 and 500.");
Serial.println("A negative speed will reverse the motor direction. 0 will stop.");
motorControl.begin();
}
void loop() {
while (Serial.available()) {
char c = Serial.read(); // Gets one byte from the serial buffer
readString += c;
delay(2); // Slow looping to allow buffer to fill with the next character
}
if (readString.length() > 0) {
Serial.print("\nString entered: ");
Serial.print(readString); // Echo captured string
int n = readString.toInt(); // Convert readString into a number
n = constrain(n, -500, 500);
if (n > 0) {
Serial.print("Set Forward Speed: ");
} else if (n < 0) {
Serial.print("Set Reverse Speed: ");
} else {
Serial.print("Stop Motor. Speed: ");
}
Serial.println(n);
motorControl.moveAtSpeed(n);
readString = ""; // Empty string for the next input
}
}
#include <HB25MotorControl.h>
HB25MotorControl::HB25MotorControl(byte controlPin) {
_controlPin = controlPin;
}
// Private Methods
void HB25MotorControl::_checkHoldOffTime() {
// Check that time since the last command is greater than the HOLD_OFF_TIME.
//
// Note - Millis() returns the number of milliseconds since the Arduino board began running the current program.
// This number will overflow (go back to zero), after approximately 50 days. We will check for this overflow.
unsigned long timeSinceStartUp = millis();
if (timeSinceStartUp <= _timeOfLastCommand) { // Millis() Overflow
delay(HOLD_OFF_TIME);
}
unsigned long timeSinceLastCommand = timeSinceStartUp - _timeOfLastCommand;
if (timeSinceLastCommand < HOLD_OFF_TIME) {
delay(HOLD_OFF_TIME - timeSinceLastCommand);
}
}
// Public Methods
void HB25MotorControl::begin() {
_timeOfLastCommand = 0;
delay(5); // HB-25 initialisation time
pinMode(_controlPin, OUTPUT);
digitalWrite(_controlPin, LOW); // Set control pin low on power up
_servo.attach(_controlPin, 800, 2200); // Attach HB-25 to the control pin & set valid range
_servo.writeMicroseconds(STOP);
delay(20);
}
void HB25MotorControl::stop() {
_checkHoldOffTime();
_servo.writeMicroseconds(STOP);
_timeOfLastCommand = millis();
}
void HB25MotorControl::fullForward() {
_checkHoldOffTime();
_servo.writeMicroseconds(FORWARD);
_timeOfLastCommand = millis();
}
void HB25MotorControl::fullReverse() {
_checkHoldOffTime();
_servo.writeMicroseconds(REVERSE);
_timeOfLastCommand = millis();
}
void HB25MotorControl::rampToSpeed(int speed) {
_checkHoldOffTime();
speed = constrain(speed, -500, 500);
speed = STOP + speed;
if (speed > 0) {
for (int rampSpeed = STOP; rampSpeed < speed; rampSpeed++) {
_servo.writeMicroseconds(rampSpeed);
delay(150);
}
} else if (speed < 0) {
for (int rampSpeed = STOP; rampSpeed > speed; rampSpeed--) {
_servo.writeMicroseconds(rampSpeed);
delay(150);
}
} else {
_servo.writeMicroseconds(STOP);
}
}
void HB25MotorControl::moveAtSpeed(int speed) {
_checkHoldOffTime();
speed = constrain(speed, -500, 500); // Constrain speed to valid range
speed = STOP + speed;
_servo.writeMicroseconds(speed);
_timeOfLastCommand = millis();
}
void HB25MotorControl::forwardAtSpeed(int speed) {
_checkHoldOffTime();
speed = constrain(speed, 0, 500); // Constrain speed to valid range
speed = STOP + speed;
_servo.writeMicroseconds(speed);
_timeOfLastCommand = millis();
}
void HB25MotorControl::reverseAtSpeed(int speed) {
_checkHoldOffTime();
speed = constrain(speed, 0, 500); // Constrain speed to valid range
speed = STOP - speed;
_servo.writeMicroseconds(speed);
_timeOfLastCommand = millis();
}
/*
* Parallax HB-25 (Part Number: #29144)
* Motor Controller Library
*
* Written By: David Such (Reefwing Software - http://www.reefwing.com.au/)
* Version: 1.0
* Date: 11th October 2015
*
* Version History:
* 1.0 11/10/15 Original release
*
* Wiring:
* 1. If you are using the Parallax Motor Mount and wheel kits (part numbers
* #28962 - aluminium or #28963 - plastic) then the red cable should be
* connected to M1 on the HB-25 and the blue cable should be connected to
* M2. If these are reversed then the FORWARD and REVERSE commands will be
* reversed.
* 2. Make sure that the jumper is in place for mode 1 operation.
*
* Notes:
* 1. Library currently only supports mode 1 operation of the HB-25. In this
* mode, you need a separate digital output for each HB-25 that you want to
* control.
* 2. The HB-25 operates like a servo. You only need to send a single pulse (in mode 1)
* to change direction or speed. Pulse width determines the HB-25 output.
* 3. Valid pulse widths are 0.8 ms to 2.2 ms. If the HB-25 receives a pulse width
* which is outside this range, the motor will be stopped until it receives a
* valid pulse.
* 4. The minimum time between pulses (HOLD_OFF_TIME) is 5.25 ms + pulse time (max 2.2 ms).
* Thus the worst case hold off time needs to be 7.45 ms. We have used 8 ms.
* 5. The maximum time between pulse is unlimited, since a single pulse will be latched
* by the HB-25. An exception to this would be if the Communication Timeout feature of
* the HB-25 has been enabled. You can read more about this on the HB-25 data sheet
* (https://www.parallax.com/downloads/hb-25-motor-controller-product-documentation).
* 6. Regardless of the mode, the HB-25 signal pin should be brought low immediately upon
* power up. The Library does this when you instantiate a HB25MotorControl object.
* 7. Pulse Input (1 ms = 1000 microseconds):
* 1.0 ms Full Reverse
* 1.5 ms Neutral (STOP)
* 2.0 ms Full Forward
* 8. Valid speed ranges for the forwardAtSpeed and reverseAtSpeed methods are
* 0 (stop) to 500 (maximum speed). For moveAtSpeed & rampToSpeed you can use from -500 (full
* reverse) to 500 (full forward). As before, a speed of 0 will stop the motor.
*
*/
#ifndef HB25MotorControl_h
#define HB25MotorControl_h
#include <Arduino.h>
#include <Servo.h>
#define REVERSE 1000
#define STOP 1500
#define FORWARD 2000
#define HOLD_OFF_TIME 8
class HB25MotorControl
{
private:
unsigned long _timeOfLastCommand;
void _checkHoldOffTime();
Servo _servo;
byte _controlPin;
public:
HB25MotorControl(byte controlPin);
void begin();
void stop();
void fullForward();
void fullReverse();
void rampToSpeed(int speed);
void moveAtSpeed(int speed);
void forwardAtSpeed(int speed);
void reverseAtSpeed(int speed);
};
#endif
#####################################################################
# Syntax Colouring Map for the Parallax HB-25 (Part Number: #29144)
# Motor Controller Library
#####################################################################
#####################################################################
# Datatypes (KEYWORD1)
#####################################################################
HB25MotorControl KEYWORD1
#####################################################################
# Methods and Functions (KEYWORD2)
#####################################################################
begin KEYWORD2
stop KEYWORD2
fullForward KEYWORD2
fullReverse KEYWORD2
rampToSpeed KEYWORD2
moveAtSpeed KEYWORD2
forwardAtSpeed KEYWORD2
reverseAtSpeed KEYWORD2
#####################################################################
# END of Syntax Definitions
#####################################################################
@HancockTY
Copy link

HI, your program really help me a lot, now i am trying to control two parallax wheels in mode1 at the same time by assigning different digital pin. But anyway i can't use this library to to do that, how can i tell the Moveatspeed function in arduino code to know which signal come from which digital pin, please teach me how to control two wheel in mode 1

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment