Here are some examples from a project I am currently working on. Its a long way from finished But it got bit wise I/O etc and I am still writing/optimizing. Its from my motor controller project I am working on hooking 2 BTS7960 to 1 arduino nano and have that as a module that is controlled via I2C from another arduino - so you can control many motors from one arduino controller in effect 1 arduino (probably Mega) will take the role of what the roborio does on a robot and the controllers hopefully does what something like a talon srx does. Each motor will have limit switches and encoders etc and current control. So you will have commands like "run at 50% but dont exceed xx amps or run at … until the count reaches … ) etc. If the Nano chokes I probably port it to an esp32 Anyway here is some code - enjoy
#include <Wire.h>
#define BUFF_SIZE 40
char pbuff[BUFF_SIZE];
boolean toggle = false; // is a divide by 2 to get the 1khz output signal
/*
Sub class of BTS7960. Needs the following
PIN/Name on Module Description
3 and 4 L_EN R_EN Shorted together to PWM needs a PWM pin on the arduino
1 marked RPWM FWD pin on the Arduino
2 marked LPWM REV Pin on the Arduino
Truth table logic
PWM FWD REV Result
LR_EN in1 in2
pwm 1 0 Fwd controled speed by pwm
pwm 0 1 Rev "
1 0 0 Break - motor leads shorted
0 x x Coast - all off
IS1 and IS2 connect to an analog input pin and will read the current depending on which side is on
analogwrite(0) will turn it off and analogwrite(255) will turn it on
This One is for UNO and Nano only
We limit it for now to 6 motors max on a Mega and 2 on UNO and Nano
for each motor that is used the corresponding timer (1 or 2) is set to approx 4 KHz
Arduino UNO, Nano (Remember A4 and A5 are reserved for wire library - I2C)
Motor PWM Pin in1_fwd in2_rev, IS Enc Pins(Quadrature) LS suggestion
0 9 T1 8 11 A0 7,6 2,3
1 10 T1 12 13 A1 5,4 A2,A3
A4, A5 used for nano for now we leave the serial pins alone makes debugging and programming easier
MEGA 2560
*/
// DEFINE PINS AND PINREGISTERS We use pins and pinregisters for speed
#define P_M1_PWM 9 // Note if you change those you need also change the PCINT
#define P_M2_PWM 10
#define P_M1_IN1 8
#define P_M2_IN1 12
#define P_M1_IN2 11
#define P_M2_IN2 13
#define P_M1_QE1 7
#define P_M1_QE2 6
#define P_M2_QE1 5
#define P_M2_QE2 4
#define P_M1_LS1 3
#define P_M1_LS2 2
#define P_M2_LS1 A2
#define P_M2_LS2 A3
#define P_M1_CS A0
#define P_M2_CS A1
//flags
#define F_ERROR 0 // 0 no error 1 is error
#define F_ENABLE_LS1 1 // 1 = use limit switches use limitswitch ls1 tied to in1 (fwd)
#define F_ENABLE_LS2 2 // 1 = ls2 tied to in2(rev)
#define F_LS_OPEN 3 // Limit switch unactivated state either 0 or 1 depending on NO, NC and if tied to ground or VCC
#define F_LS1 4 // Current limit switch values
#define F_LS2 5
#define F_ENCODER 6 // Encoder is present or not
#define F_QUADRATURE 7 // Encoder if present is Quadrature (immaterial if F_ENCODER = 0
#define F_REVERSE 8 // Reverse the motor (0 - speed input) if 1 but leaves LS assignments alone
#define F_DIRECTION 9 // current direction reflection of in1(FWD) pin. Can be used to guess direction if not quadrature encoder
#define F_Brake 10 // Motor in Brake mode
#define F_COAST 11 // Motor in Coast mode (disabled)
#define F_ENABLE_CS 12 // Use Current control (limit to max current)
#define F_CS_NOW 13 // measure current sense now
// state register settings = bit positions corresponding with input bits that cause an interrupt like encoder and ls
class BTS7960_D {
public:
BTS7960_D(uint8_t motor);
void Start(); // needs to be run in setup before setting anything else - makes sure the timer1 is properly set
void Enable_CS();
void Disable_CS();
void GetLS();
void Set_ls(uint8_t ls1, uint8_t ls2, uint8_t open); // 0 = off, 1 = on, open shows if ls is inactive if you see a 0 or a 1
void Brake();
void Coast();
bool Drive(int16_t mspeed);
uint16_t getStatus(); //Returns flags
void Reverse(); //sets reverse flag
void Update(); // needs to be called periodically to check thingslike current, switches, encoders etc etc
void CheckCS(); //
// Make those variables available public for faster access
volatile uint16_t flags = 0; // 7 - enable, 6 - reverse, 5 - useLimits, 4 - usequadrature, 3 - useencoder, 2 - direction, 0 - error
//8 - LSforward, 9 - LSreverse, 10 - CS_measure, 11 CS_measure_now
volatile int16_t curr_Speed; // -255 to +255 0 will coast
volatile int16_t last_Speed; // -255 to +255 0 will coast
volatile uint32_t last_cs_read = 0; //holds millis for last current measure for now we try everyt 10 ms
volatile int16_t last_cs = 0 ; //Value of last current read
volatile int32_t enc_pos = 0; // Encoder Position
private:
uint8_t _motor = 0;
// pins
uint8_t _pwm; // pin for pw
uint8_t _in1; // pin for L_EN which is connecte internally to in_1
uint8_t _in2; // pinf for R_EN
int8_t _cs = -1; // Current sense 1 pin must be analog pin
int8_t _ls1 = -1; // Limit switch in the in1 direction
int8_t _ls2 = -1; // Limit swithc in the in2 direction
int8_t _enc1 = -1; //encoder input 1
int8_t _enc2 = -1; //encoder input 2
};
/*
Constructor do basic rudimentary setup
for now lets handle the pins hard coded and for now define it for UNO/Nano
F_Error for now to detect if we called with an invalid Motor.
/
BTS7960_D::BTS7960_D(uint8_t motor) {
if (motor < 1 || motor > 2) {
bitWrite(flags, F_ERROR, 1);
return; // only allow motor 1 and 2 on uno/nano
}
bitWrite(flags, F_ERROR, 0);
_motor = motor;
_pwm = (motor == 1) ? P_M1_PWM : P_M2_PWM;
_in1 = (motor == 1) ? P_M1_IN1 : P_M2_IN1;
_in2 = (motor == 1) ? P_M1_IN2 : P_M2_IN2;
pinMode(_pwm, OUTPUT);
pinMode(_in1, OUTPUT);
pinMode(_in2, OUTPUT);
digitalWrite(_pwm, 0);
digitalWrite(_in1, 0); // make sure everything is turned off
digitalWrite(_in2, 0);
//Setup pin interrupt for now we only accept them on PCINT 1 pin 9, 10 in the future we can expand here
}
void BTS7960_D::Start() {
cli();
//WGM 10 and 12 set FASTPMW 8 Bit to get them both to rise simultaneously and
//CS 11 gives us with that an 8kHz cycle
TCCR1B = 0B00001010; // sets CS11 and WGM12
TCCR1A = 0B00000001; // Sets WGM10
PCICR |= 0b00000001; // Enables Ports B Pin Change Interrupts
PCMSK0 |= (_motor == 10) ? 0b00000010 : 0b00000100; // PCINT1 or 2 - pin 9 and 10
TIMSK1 != 0b00000110; // set mask for both
sei();
}
// read Limit switches and set position if the pin = -1 (disabled) switch is set to 0
void BTS7960_D::GetLS () {
if (_ls1 >= 0) {
if (digitalRead(_ls1)) {
bitSet(flags, F_LS1);
} else {
bitClear(flags, F_LS1);
}
} else {
bitClear(flags, F_LS1);
}
if (_ls2 >= 0) {
if (digitalRead(_ls2)) {
bitSet(flags, F_LS2);
} else {
bitClear(flags, F_LS2);
}
} else {
bitClear(flags, F_LS2);
}
}
// see if its time to read the CS note that it will read aprox. every 10 ms or longer depending on how busy the system is
void BTS7960_D::CheckCS() {
uint32_t m = millis();
if (m < 10 || m - 10 > last_cs_read) {
bitSet(flags, F_CS_NOW);
}
}
void BTS7960_D::Update() {
//if we hit the limitswitch and the limitswitch is active then stop the motor
// if (bitRead(flags, F_ENABLE_LS1) == 1) {
// if (bitRead(flags, F_LS_OPEN) != digitalRead(_ls1)) {
// this->Brake();
// }
// }
}
//reverse motor direction
void BTS7960_D::Reverse() {
bitWrite(flags, F_REVERSE, 1);
}
/
Use currentcontrol
/
void BTS7960_D::Enable_CS() {
_cs = (_motor == 1) ? P_M1_CS : P_M2_CS;
bitWrite(flags, F_ENABLE_CS, 1);
bitWrite(flags, F_CS_NOW, 1);
last_cs = 0;
}
/
Disable Current Sense
*/
void BTS7960_D::Disable_CS() {
bitWrite(flags, F_ENABLE_CS, 0);
bitWrite(flags, F_CS_NOW, 0);
}
void BTS7960_D::Set_ls(uint8_t ls1, uint8_t ls2, uint8_t open) {
}
/*
get statusflags
/
uint16_t BTS7960_D::getStatus() {
return flags;
}
/
Put motor in coast by disabling the unit
/
void BTS7960_D::Coast() {
analogWrite(_pwm, 0); // will set the pin to 0
last_Speed = curr_Speed;
curr_Speed = 0;
}
/
Put the motor in Brake by turning both lower ends on and the upper ones off
*/
void BTS7960_D::Brake() {
analogWrite(_pwm, 255); // will set the pin to 1
digitalWrite(_in1, 0);
digitalWrite(_in2, 0);
last_Speed = curr_Speed;
curr_Speed = 0;
}
//Drive the motor positive = FWD negative = reverse
bool BTS7960_D::Drive(int16_t mspeed) {
if (bitRead(flags, F_REVERSE) != 0) {
mspeed = 0 - mspeed; // reverse it
}
if (mspeed == 0) {
analogWrite(_pwm, mspeed); // leave flags alone
} else {
if (mspeed > 0) // forward
{
if (bitRead(flags, F_DIRECTION) != 1) {
this->Brake();
}
bitWrite(flags, F_DIRECTION, 1); //Set direction
digitalWrite(_in1, 1);
digitalWrite(_in2, 0);
mspeed = (mspeed < 10) ? 10 : (mspeed > 245) ? 245 : mspeed;
analogWrite(_pwm, mspeed);
}
else {
if (bitRead(flags, F_DIRECTION) != 0) {
this->Brake();
}
bitWrite(flags, F_DIRECTION, 0);
digitalWrite(_in1, 0);
digitalWrite(_in2, 1);
mspeed = (mspeed > -10) ? -10 : (mspeed < -245) ? -245 : mspeed;
analogWrite(_pwm, abs(mspeed));
}
}
last_Speed = curr_Speed;
curr_Speed = mspeed;
return true;
}
// define motors
BTS7960_D motor1(1);
BTS7960_D motor2(2);
/*
On the wire there are always a set of bytes transmiteed
byte 1 = number of bytes in transmission after this 1-255
byte 2 = Command 1-255 - needs at least 1 command byte 0 would be NOP can be used as a heartbeat and will require a 0 back
byte 3-n parameters of command
Replies with either the required # of bytes or 0 = ok 1 = error
If it receives a command 1 (error) then it will reset the communication and expect a command next sending multiple 1 is ok
*/
#define WIRE_ID 1 // wire id of this device - change if you use multiple in a system
#define WIRE_BUFF 4 // #of bytes in data buffer
uint8_t wire_state = 0; // 0 = waiting for transmission 1 = count 2 cmd received 3 = parameters resets to 0 at end
uint8_t wire_wcnt = 0; // wait count of bytes still to be received currently limit to up to 4
uint8_t curr_cmd = 0;
uint8_t wbuff[WIRE_BUFF];
void clear_wbuff() {
for (uint8_t i = 0; i < WIRE_BUFF; i++) {
wbuff[i] = 0;
}
}
void receiveEvent(int howMany)
{
while (0 < Wire.available()) // loop through all but the last
{
uint8_t c = Wire.read(); // receive byte as a character
switch (wire_state) {
case 0: // byte is count
wire_wcnt = c;
wire_state++;
break;
case 1 : // command
wire_state = (–wire_wcnt == 0) ? 0 : wire_state++;
curr_cmd = c;
if (wire_wcnt <= 0) // no data
{
wire_state = 0; //make sure all flags are set properly
wire_wcnt = 0;
uint8_t *p; //pointer
switch (c) {
case 0: // heartbeat
Wire.write(0);
break;
case 1: // heartbeat
Wire.write(0);
break;
case 2: // get amp M1
p = (byte *)motor1.last_cs;
Wire.write(*p);
Wire.write(*(p+1));
break;
case 3: // get amp M2
p = (byte *)motor2.last_cs;
Wire.write(*p);
Wire.write(*(p+1));
break;
case 4: // get speed M1
p = (byte *)motor1.curr_Speed;
Wire.write(*p);
Wire.write(*(p+1));
break;
case 5: // get speed M2
p = (byte *)motor2.curr_Speed;
Wire.write(*p);
Wire.write(*(p+1));
break;
case 6: // get enc M1
p = (byte *)motor1.enc_pos;
Wire.write(*p);
Wire.write(*(p+1));
Wire.write(*(p + 2));
Wire.write(*(p+3));
break;
case 7: // get enc M2
p = (byte *)motor2.enc_pos;
Wire.write(*p);
Wire.write(*(p+1));
Wire.write(*(p + 2));
Wire.write(*(p+3));
break;
case 8: // get flags / LS M1
motor1.GetLS(); // read limit switches
p = (byte *)motor1.flags;
Wire.write(*p);
Wire.write(*(p+1));
break;
case 9: // get flags / LS M2
motor2.GetLS(); // read limit switches
p = (byte *)motor2.flags;
Wire.write(*p);
Wire.write(*(p+1));
break;
}
}
case 2: // data
wire_state = (--wire_wcnt == 0) ? 0 : wire_state++;
break;
default:
wire_wcnt--;
}
}
}
void setup() {
motor1.Start();
motor2.Start();
Wire.begin(WIRE_ID); // join i2c bus with address #4
Wire.onReceive(receiveEvent); // register event
uint32_t test = 0x11223344;
Serial.begin(115200);
uint8_t *p = (byte *)&test;
// uint8_t a = *p++ ;
// uint8_t b = *p++;
// uint8_t c = *p++;
// uint8_t d = *p++;
snprintf_P(pbuff, sizeof(pbuff), PSTR(“BTS79860 Test Boot up %i %i”), *p, *(p+1));
Serial.println(pbuff);
snprintf_P(pbuff, sizeof(pbuff), PSTR(“TCCR1A %i TCCR1B %i H/L %i %i”), TCCR1A, TCCR1B, *(p+2), *(p+3));
Serial.println(pbuff);
motor1.Drive(64);
motor2.Drive(145);
}
ISR(PCINT1_vect)
{
}
ISR(PCINT2_vect)
{
}
// interrupt for PCINT0 pins 8-12 OPTIMIZE!!! if possible
ISR(PCINT0_vect)
{
uint8_t sreg;
uint16_t tcnt; // tcnt = 16 bit for timer 1
uint8_t i ;
// if we dont do CS_NOW on either motor return right away - assumes both motors initialized
if ((motor1.flags | motor2.flags) & 1 << F_CS_NOW) {
/* Save global interrupt flag /
sreg = SREG;
/ Disable interrupts /
cli();
/ Read TCNT1 into tcnt /
tcnt = TCNT1;
/ Restore global interrupt flag */
SREG = sreg;
if (bitRead(motor1.flags, F_CS_NOW)) {
if (tcnt < OCR1A) {
motor1.last_cs = analogRead(P_M1_CS);
motor1.last_cs_read = millis(); //probably too slow TEST IT
bitClear(motor1.flags, F_CS_NOW); // we read it so lets clear it
}
}
if (bitRead(motor2.flags, F_CS_NOW)) {
if (tcnt < OCR1B) {
motor2.last_cs = analogRead(P_M2_CS);
motor2.last_cs_read = millis();
bitClear(motor2.flags, F_CS_NOW); // we read it so lets clear it
}
}
}
}
void loop() {
// put your main code here, to run repeatedly:
// snprintf_P(pbuff, sizeof(pbuff), PSTR("%i = %i"), millis(), cnt);
// check if we do CS and if its time
motor1.CheckCS();
motor2.CheckCS();
}
!