initial commit
This commit is contained in:
22
LICENSE.txt
Normal file
22
LICENSE.txt
Normal file
@@ -0,0 +1,22 @@
|
|||||||
|
Copyright (c) 2020 Willumpie82
|
||||||
|
|
||||||
|
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.
|
||||||
139
MC34933.cpp
Normal file
139
MC34933.cpp
Normal file
@@ -0,0 +1,139 @@
|
|||||||
|
/*
|
||||||
|
MC34933.h - Library for the Toshiba MC34933 motor driver.
|
||||||
|
Created by TheDIYGuy999 June - November 2016
|
||||||
|
Released into the public domain.
|
||||||
|
|
||||||
|
This is Version 1.2
|
||||||
|
|
||||||
|
The pin configuration was moved to the separate begin() function.
|
||||||
|
Change your existing programs in accordance with the provided example
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Arduino.h"
|
||||||
|
#include "MC34933.h"
|
||||||
|
|
||||||
|
// Member definition (code) ========================================================================
|
||||||
|
|
||||||
|
MC34933::MC34933()
|
||||||
|
{ // Constructor
|
||||||
|
_state = 0;
|
||||||
|
_previousMillis = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Begin function ************************************************************
|
||||||
|
|
||||||
|
// NOTE: The first pin must always be PWM capable, the second only, if the last parameter is set to "true"
|
||||||
|
// SYNTAX: IN1, IN2, min. input value, max. input value, neutral position width
|
||||||
|
// invert rotation direction, true = both pins are PWM capable
|
||||||
|
void MC34933::begin(int pin1, int pin2, int minInput, int maxInput, int neutralWidth, bool invert) {
|
||||||
|
_pin1 = pin1;
|
||||||
|
_pin2 = pin2;
|
||||||
|
_minInput = minInput;
|
||||||
|
_maxInput = maxInput;
|
||||||
|
_minNeutral = (_maxInput + _minInput) / 2 - (neutralWidth / 2);
|
||||||
|
_maxNeutral = (_maxInput + _minInput) / 2 + (neutralWidth / 2);
|
||||||
|
_controlValueRamp = (_minNeutral + _maxNeutral) / 2;
|
||||||
|
_invert = invert;
|
||||||
|
|
||||||
|
pinMode(_pin1, OUTPUT);
|
||||||
|
pinMode(_pin2, OUTPUT);
|
||||||
|
digitalWrite(_pin1, LOW);
|
||||||
|
digitalWrite(_pin2, LOW);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Drive function ************************************************************
|
||||||
|
|
||||||
|
// SYNTAX: Input value, max PWM, ramptime in ms per 1 PWM increment
|
||||||
|
// true = brake active, false = brake in neutral position inactive
|
||||||
|
bool MC34933::drive(int controlValue, int minPWM, int maxPWM, int rampTime, bool neutralBrake) {
|
||||||
|
_controlValue = controlValue;
|
||||||
|
_minPWM = minPWM;
|
||||||
|
_maxPWM = maxPWM;
|
||||||
|
_rampTime = rampTime;
|
||||||
|
_neutralBrake = neutralBrake;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if (_invert) {
|
||||||
|
_controlValue = map (_controlValue, _minInput, _maxInput, _maxInput, _minInput); // invert driving direction
|
||||||
|
}
|
||||||
|
|
||||||
|
// Fader (allows to ramp the motor speed slowly up & down) --------------------
|
||||||
|
if (_rampTime >= 1) {
|
||||||
|
unsigned long _currentMillis = millis();
|
||||||
|
if (_currentMillis - _previousMillis >= (unsigned int)_rampTime) {
|
||||||
|
// Increase
|
||||||
|
if (_controlValue > _controlValueRamp && _controlValueRamp < _maxInput) {
|
||||||
|
_controlValueRamp++;
|
||||||
|
_upwards = true;
|
||||||
|
_downwards = false;
|
||||||
|
}
|
||||||
|
// Decrease
|
||||||
|
else if (_controlValue < _controlValueRamp && _controlValueRamp > _minInput) {
|
||||||
|
_controlValueRamp--;
|
||||||
|
_upwards = false;
|
||||||
|
_downwards = true;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
_upwards = false;
|
||||||
|
_downwards = false;
|
||||||
|
}
|
||||||
|
_previousMillis = _currentMillis;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
_controlValueRamp = _controlValue;
|
||||||
|
_upwards = false;
|
||||||
|
_downwards = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// H bridge controller -------------------
|
||||||
|
if (_controlValueRamp >= _maxNeutral) { // Forward
|
||||||
|
digitalWrite(_pin1, HIGH);
|
||||||
|
//digitalWrite(_pin2, LOW);
|
||||||
|
analogWrite(_pin2, map(_controlValueRamp, _maxNeutral, _maxInput, _minPWM, _maxPWM));
|
||||||
|
_forward = true;
|
||||||
|
_reverse = false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
else if (_controlValueRamp <= _minNeutral) { // Reverse
|
||||||
|
digitalWrite(_pin1, LOW);
|
||||||
|
//digitalWrite(_pin2, HIGH);
|
||||||
|
analogWrite(_pin2, map(_controlValueRamp, _minNeutral, _minInput, _minPWM, _maxPWM));
|
||||||
|
_forward = false;
|
||||||
|
_reverse = true;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
else { // Neutral
|
||||||
|
if (_neutralBrake) {
|
||||||
|
digitalWrite(_pin1, LOW); // Brake in neutral position active
|
||||||
|
digitalWrite(_pin2, LOW);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
digitalWrite(_pin1, HIGH); // Brake in neutral position inactive
|
||||||
|
digitalWrite(_pin2, HIGH);
|
||||||
|
}
|
||||||
|
_forward = false;
|
||||||
|
_reverse = false;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Brakelight detection function ************************************************************
|
||||||
|
bool MC34933::brakeActive() {
|
||||||
|
|
||||||
|
unsigned long _currentMillisBrake = millis();
|
||||||
|
|
||||||
|
// Reset delay timer, if vehicle isn't decelerating
|
||||||
|
if ( (!(_upwards && _reverse)) && (!(_downwards && _forward)) ) {
|
||||||
|
_previousMillisBrake = _currentMillisBrake;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_currentMillisBrake - _previousMillisBrake >= 100) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
57
MC34933.h
Normal file
57
MC34933.h
Normal file
@@ -0,0 +1,57 @@
|
|||||||
|
/*
|
||||||
|
MC34933.h - Library for the Toshiba MC34933 motor driver.
|
||||||
|
Created by TheDIYGuy999 June - November 2016
|
||||||
|
Released into the public domain.
|
||||||
|
|
||||||
|
This is Version 1.2
|
||||||
|
|
||||||
|
Change history:
|
||||||
|
|
||||||
|
V1.1:
|
||||||
|
The pin configuration was moved to the separate begin() function.
|
||||||
|
Change your existing programs in accordance with the provided example
|
||||||
|
|
||||||
|
V1.2:
|
||||||
|
minPWM input variable added to the drive() function.
|
||||||
|
Allows to eliminate the backlash in self balancing applications
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef MC34933H
|
||||||
|
#define MC34933H
|
||||||
|
|
||||||
|
#include "Arduino.h"
|
||||||
|
|
||||||
|
// Class definition (header) ========================================================================
|
||||||
|
class MC34933 {
|
||||||
|
public:
|
||||||
|
MC34933();
|
||||||
|
void begin(int pin1, int pin2, int minInput, int maxInput, int neutralWidth, bool invert);
|
||||||
|
bool drive(int controlValue, int minPWM, int maxPWM, int rampTime, bool neutralBrake);
|
||||||
|
bool brakeActive();
|
||||||
|
|
||||||
|
private:
|
||||||
|
int _pin1;
|
||||||
|
int _pin2;
|
||||||
|
int _pwmPin;
|
||||||
|
int _minInput;
|
||||||
|
int _maxInput;
|
||||||
|
int _minNeutral;
|
||||||
|
int _maxNeutral;
|
||||||
|
int _controlValue;
|
||||||
|
int _controlValueRamp;
|
||||||
|
int _minPWM;
|
||||||
|
int _maxPWM;
|
||||||
|
int _rampTime;
|
||||||
|
bool _neutralBrake;
|
||||||
|
bool _invert;
|
||||||
|
unsigned long _previousMillis = 0;
|
||||||
|
unsigned long _previousMillisBrake = 0;
|
||||||
|
byte _state = 0;
|
||||||
|
byte _forward;
|
||||||
|
byte _reverse;
|
||||||
|
byte _upwards;
|
||||||
|
byte _downwards;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif //MC34933H
|
||||||
102
MC34933/MC34933.ino
Normal file
102
MC34933/MC34933.ino
Normal file
@@ -0,0 +1,102 @@
|
|||||||
|
// This example was cuccessfully tested on an Sparkfun Pro Micro in 8MHz version
|
||||||
|
// It also works on a Pro Mini, but only, if you don't change the PWM frequency
|
||||||
|
|
||||||
|
// Version 1.2
|
||||||
|
|
||||||
|
//
|
||||||
|
// =======================================================================================================
|
||||||
|
// INCLUDE LIRBARIES
|
||||||
|
// =======================================================================================================
|
||||||
|
//
|
||||||
|
#include <MC34933.h> // https://github.com/TheDIYGuy999/MC34933
|
||||||
|
|
||||||
|
// Optional for motor PWM frequency adjustment:
|
||||||
|
//#include <PWMFrequency.h> // https://github.com/kiwisincebirth/Arduino/tree/master/libraries/PWMFrequency
|
||||||
|
|
||||||
|
//
|
||||||
|
// =======================================================================================================
|
||||||
|
// CREATE MOTOR OBJECTS
|
||||||
|
// =======================================================================================================
|
||||||
|
//
|
||||||
|
|
||||||
|
// define motor pin numbers
|
||||||
|
#define motor1_in1 2
|
||||||
|
#define motor1_in2 4
|
||||||
|
|
||||||
|
#define motor2_in1 7
|
||||||
|
#define motor2_in2 8
|
||||||
|
|
||||||
|
// Create motor objects
|
||||||
|
MC34933 Motor1; // Motor 1
|
||||||
|
MC34933 Motor2; // Motor 2
|
||||||
|
|
||||||
|
//
|
||||||
|
// =======================================================================================================
|
||||||
|
// LIGHTS
|
||||||
|
// =======================================================================================================
|
||||||
|
//
|
||||||
|
|
||||||
|
void lights() {
|
||||||
|
if (Motor1.brakeActive()) { // if braking detected from MC34933 motor driver
|
||||||
|
//digitalWrite(BRAKELIGHTS, HIGH);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
//digitalWrite(BRAKELIGHTS, LOW);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
// =======================================================================================================
|
||||||
|
// MAIN ARDUINO SETUP (1x during startup)
|
||||||
|
// =======================================================================================================
|
||||||
|
//
|
||||||
|
void setup() {
|
||||||
|
|
||||||
|
// Motor pin setup
|
||||||
|
// SYNTAX: IN1, IN2, PWM, min. input value, max. input value, neutral position width
|
||||||
|
// invert rotation direction true or false
|
||||||
|
Motor1.begin(motor1_in1, motor1_in2, 0, 1023, 60, false); // Motor 1
|
||||||
|
Motor2.begin(motor2_in1, motor2_in2, 0, 1023, 60, false); // Motor 2
|
||||||
|
|
||||||
|
// Optional: Motor PWM frequency (Requires the PWMFrequency.h library)
|
||||||
|
/*setPWMPrescaler(motor1_pwm, 1); // 123Hz = 256, 492Hz = 64, 3936Hz = 8, 31488Hz = 1
|
||||||
|
setPWMPrescaler(motor2_pwm, 1);*/
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
// =======================================================================================================
|
||||||
|
// DRIVE MOTOR
|
||||||
|
// =======================================================================================================
|
||||||
|
//
|
||||||
|
|
||||||
|
void driveMotor() {
|
||||||
|
|
||||||
|
int speed1 = 512;
|
||||||
|
int speed2 = 512;
|
||||||
|
|
||||||
|
// Read Potentiometer
|
||||||
|
speed1 = analogRead(A0); // 0 - 1023
|
||||||
|
speed2 = analogRead(A1); // 0 - 1023
|
||||||
|
|
||||||
|
// ***************** Note! The ramptime is intended to protect the gearbox! *******************
|
||||||
|
// SYNTAX: Input value, min PWM, max PWM, ramptime in ms per 1 PWM increment
|
||||||
|
// false = brake in neutral position inactive
|
||||||
|
if (Motor1.drive(speed1, 0, 255, 7, false) ) { // Motor 1
|
||||||
|
// returns true, if motor is running, so we can do other stuff here!
|
||||||
|
}
|
||||||
|
Motor2.drive(speed2, 0, 255, 1, false); // Motor 2
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
// =======================================================================================================
|
||||||
|
// MAIN LOOP
|
||||||
|
// =======================================================================================================
|
||||||
|
//
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
// Drive the main motor
|
||||||
|
driveMotor();
|
||||||
|
|
||||||
|
//Light control
|
||||||
|
lights();
|
||||||
|
}
|
||||||
36
README.md
Normal file
36
README.md
Normal file
@@ -0,0 +1,36 @@
|
|||||||
|
# MC34933 modification
|
||||||
|
This lib is forked from: https://github.com/TheDIYGuy999/TB6612FNG.git
|
||||||
|
|
||||||
|
- EXPERIMENTAL!
|
||||||
|
- Modifed code to support MC34933 (will make a nice switch when I have time)
|
||||||
|
- Removed dedicated PWM pin (this controller an PWM the input pins)
|
||||||
|
|
||||||
|
# Advanced Arduino H-Bridge control library
|
||||||
|
|
||||||
|
This is an Arduino library for the Toshiba MC34933 DC motor driver
|
||||||
|
|
||||||
|
## Features:
|
||||||
|
- Adjustable fader for smooth speed changes
|
||||||
|
- brake active or not in neutral mode
|
||||||
|
- selectable input signal range (e.g. 0 - 100, 0 - 1023, -255 - 255 etc.)
|
||||||
|
- selectable neutral position width. This allows you to optimize it for your joystick
|
||||||
|
- the motor rotation direction is reversible in software, so you don't have to switch your motor wires, if the direction is reversed
|
||||||
|
- The end-speed is adjustable during runtime. This allows you to simulate different gear ratios
|
||||||
|
- drive function returns true while driving or false while in neutral
|
||||||
|
- brakeActive function returns true, while the vehicle is getting slower. Used to control brake lights
|
||||||
|
- Backlash compensation for self balancing applications
|
||||||
|
|
||||||
|
New in V 1.1:
|
||||||
|
- Pin configuration moved to begin() function. This change was necessary due to new requirements in my new "Micro RC Receiver" V1.7 software revision
|
||||||
|
- If you've used the previous version in a project, you will have to change the pin configuration in accordance with the provided example. The archived V1.0 is enclosed and will not be maintained anymore.
|
||||||
|
|
||||||
|
New in V1.2:
|
||||||
|
- minPWM input variable added to the drive() function. Allows to eliminate the backlash in self balancing applications
|
||||||
|
- IMPORTANT!! You have to add this new input variable to your existing sketches! Ohtherwise it will not compile.
|
||||||
|
|
||||||
|
|
||||||
|
## Usage
|
||||||
|
|
||||||
|
TODO
|
||||||
|
|
||||||
|
(c) 2020 - Willumpie82
|
||||||
17
keywords.txt
Normal file
17
keywords.txt
Normal file
@@ -0,0 +1,17 @@
|
|||||||
|
#######################################
|
||||||
|
# Syntax Coloring Map For MC34933
|
||||||
|
#######################################
|
||||||
|
|
||||||
|
#######################################
|
||||||
|
# Datatypes (KEYWORD1)
|
||||||
|
#######################################
|
||||||
|
|
||||||
|
MC34933 KEYWORD
|
||||||
|
|
||||||
|
#######################################
|
||||||
|
# Methods and Functions (KEYWORD2)
|
||||||
|
#######################################
|
||||||
|
|
||||||
|
begin KEYWORD2
|
||||||
|
drive KEYWORD2
|
||||||
|
brakeActive KEYWORD2
|
||||||
Reference in New Issue
Block a user