Hello everyone, we're trying to drive a mid size tank (industrial purposes, rough terrains) forwards and backwards initially and later on add sideways and rotations, but we've come to a roadblock, we've first tried using pulse_in but when reading from 3 channels there were timeouts and inconsistencies, so we've read that the best way is to use a digital interrupt, to kickstart the project fast, we found this code and tried to adopt it.
It works flawlessly even for our use case but only when we're printing the values, if we use writeMicroSeconds to the servos, then the values are jumping like crazy, it seems to me that writing to the servo is blocking the execution of the loop function.
If we need to add more details like schematics and what's being used, let me know, for starters we're using: Arduino Mega 2560, RadioLink AT10, 48v 3KW Brushless dc servo drivers x2 and two brushless dc motors.
#include <Servo.h>
Servo leftMotor;
Servo rightMotor;
// Set the port speed for host communication
#define SERIAL_PORT_SPEED 9600
// Set the size of the arrays (increase for more channels)
#define RC_NUM_CHANNELS 3
// Set up our receiver channels - these are the channels from the receiver
#define RC_CH1 0 // rotation
#define RC_CH2 1 // forward backwards
#define RC_CH3 2 // sideways
// ARDUINO MEGA 2560
#define RC_CH1_INPUT 19 // receiver pin 2
#define RC_CH2_INPUT 2 // receiver pin 5
#define RC_CH3_INPUT 3 // receiver pin 6
// Set up some arrays to store our pulse starts and widths
uint16_t RC_VALUES[RC_NUM_CHANNELS] = { 1508, 1508, 1508 };
uint32_t RC_START[RC_NUM_CHANNELS];
volatile uint16_t RC_SHARED[RC_NUM_CHANNELS];
// to the extent possible, all the variables are scaleable, you will need to add more values
// here if you use a higher number of channels
uint16_t RC_LOW[RC_NUM_CHANNELS] = { 1092, 1092, 1092 };
uint16_t RC_MID[RC_NUM_CHANNELS] = { 1508, 1508, 1508 };
uint16_t RC_HIGH[RC_NUM_CHANNELS] = { 1924, 1924, 1924 };
// The RC Channel mode helps us know how to use and refine the signal
// Settings are:
// 0 = a joystick with a centerpoint (deadzone in middle)
// 1 = a throttle that goes from low to high (deadzone at start)
// 2 = a a switch (either on or off)
uint16_t RC_CHANNEL_MODE[RC_NUM_CHANNELS] = { 0, 0, 0};
// a place to store our mapped values
float RC_TRANSLATED_VALUES[RC_NUM_CHANNELS] = { 1500, 1500, 1500};
// some boundaries for our mapped values
float RC_TRANSLATED_LOW[RC_NUM_CHANNELS] = { 2000, 2000, 2000 };
float RC_TRANSLATED_MID[RC_NUM_CHANNELS] = { 1500, 1500, 1500 };
float RC_TRANSLATED_HIGH[RC_NUM_CHANNELS] = { 1000, 1000, 1000 };
// What percentage deadzone is allowed? values in percent e.g. 10 = 10%
uint16_t RC_DZPERCENT[RC_NUM_CHANNELS] = { 30, 30, 5 };
// Setup our program
void setup() {
// Set the speed to communicate with the host PC
Serial.begin(SERIAL_PORT_SPEED);
leftMotor.attach(5);
rightMotor.attach(6);
// Set our pin modes to input for the pins connected to the receiver
pinMode(RC_CH1_INPUT, INPUT_PULLUP);
pinMode(RC_CH2_INPUT, INPUT_PULLUP);
pinMode(RC_CH3_INPUT, INPUT_PULLUP);
// Attach interrupts to our pins
attachInterrupt(digitalPinToInterrupt(RC_CH1_INPUT), READ_RC1, CHANGE);
attachInterrupt(digitalPinToInterrupt(RC_CH2_INPUT), READ_RC2, CHANGE);
attachInterrupt(digitalPinToInterrupt(RC_CH3_INPUT), READ_RC3, CHANGE);
}
unsigned long current = 0;
unsigned long prev = 0;
const unsigned long interval = 100000UL;
void loop() {
// read the values from our RC Receiver
rc_read_values();
rc_clip_values();
// map the radio values to the range we want
rc_translate_values();
driveBothMotors(RC_TRANSLATED_VALUES[1]);
// keep track of time
current = micros();
// This is our plotter Chart output, we only do it every so often or the plotter moves too fast
if (current - prev >= interval) {
prev += interval;
}
}
// Thee functions are called by the interrupts. We send them all to the same place to measure the pulse width
void READ_RC1() {
Read_Input(RC_CH1, RC_CH1_INPUT);
}
void READ_RC2() {
Read_Input(RC_CH2, RC_CH2_INPUT);
}
void READ_RC3() {
Read_Input(RC_CH3, RC_CH3_INPUT);
}
// This function reads the pulse starts and uses the time between rise and fall to set the value for pulse width
void Read_Input(uint8_t channel, uint8_t input_pin) {
if (digitalRead(input_pin) == HIGH) {
RC_START[channel] = micros();
} else {
uint16_t rc_compare = (uint16_t)(micros() - RC_START[channel]);
RC_SHARED[channel] = rc_compare;
}
}
// this function pulls the current values from our pulse arrays for us to use.
void rc_read_values() {
noInterrupts();
memcpy(RC_VALUES, (const void *)RC_SHARED, sizeof(RC_SHARED));
interrupts();
}
void rc_clip_values() {
// loop through the channels
if(RC_VALUES[0] < 1500) return;
if(RC_VALUES[1] < 1500) return;
if(RC_VALUES[2] < 1500) return;
for (int i = 0; i < RC_NUM_CHANNELS; i++) {
// a little clipping to make sure we dont go over or under the bounds
// clip the high range so it doesnt go over the max
if (RC_VALUES[i] > RC_HIGH[i]) {
RC_VALUES[i] = RC_HIGH[i];
}
// clip the low range so it doesnt go under the min
if (RC_VALUES[i] < RC_LOW[i]) {
RC_VALUES[i] = RC_LOW[i];
}
}
}
void rc_translate_values() {
// Loop through all our channels
for (int i = 0; i < RC_NUM_CHANNELS; i++) {
// translate the RC channel value into our new number range
RC_TRANSLATED_VALUES[i] = translateValueIntoNewRange((float)RC_VALUES[i], (float)RC_HIGH[i], (float)RC_LOW[i], RC_TRANSLATED_LOW[i], RC_TRANSLATED_HIGH[i]);
}
}
void driveBothMotors(long microSeconds){
//just printing the values are correct
Serial.println((String)"Forward and backwards "+microSeconds);
//when writing to both motors then the values that are read using interrupts are jumping, what's the problem?
leftMotor.writeMicroseconds(microSeconds);
rightMotor.writeMicroseconds(microSeconds);
}
float translateValueIntoNewRange(float currentvalue, float currentmax, float currentmin, float newmax, float newmin) {
// Use this formula to work out where we are in the new range
// NewValue = (((OldValue - OldMin) * (NewMax - NewMin)) / (OldMax - OldMin)) + NewMin
//
// this formula was lovingly stolen from https://stackoverflow.com/questions/929103/convert-a-number-range-to-another-range-maintaining-ratio
return (((currentvalue - currentmin) * (newmax - newmin)) / (currentmax - currentmin)) + newmin;
}