MeArm Robot Arm Controlled With RC Transmitter – Code Attempt 1

I first wrote a simple program using PulseIn to read the value from the rc reciever. The servo library was used to implement the servo control. Receiver values were mapped to the correct servo.

A problem with this code is that PulseIn interrupts the code significantly introducing errors. This effects the timing output to the servo. Since the High Pulse for the servo is between a 1000 microseconds and 2000 microseconds, timing is critical and the PulseIn dramatically effects this. All this causes the servos to jitter or move eratically since errors are being introduced from the changing pulse out to the servos.

Here is the code i wrote  (This reads 6 channels, For the MeArm only 4 are needed)

// This sketch will read all 6 channels of a RC reciever
// Display

// Programmed for the Arduino MEGA 2560

//——————————————————————
//——————- Servo Setup ———————————-
//——————————————————————

# include
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;

//——————————————————————
// —————— Reciever Setup ——————————-
//——————————————————————
// Define Variables for Reciever Input
const int chA = 22; // Constant variables relating to pin locations
const int chB = 24;
const int chC = 26;
const int chD = 28;
const int chE = 30;
const int chF = 32;

int ch1; //Variables to store and display the values of each channel
int ch2;
int ch3;
int ch4;
int ch5;
int ch6;

// The setup routine runs once when you press reset:
void setup(){

//——————————————————————
//——————- Servo Setup ———————————-
//——————————————————————

// Setting Servo Pins
servo1.attach(2);
servo2.attach(3);
servo3.attach(4);
servo4.attach(5);
servo5.attach(6);
servo6.attach(7);

//——————————————————————
// —————— Reciever Setup ——————————-
//——————————————————————
// initialize serial communication at 115200 bits per second:
Serial.begin(115200);
// Set input pins
pinMode(chA, INPUT);
pinMode(chB, INPUT);
pinMode(chC, INPUT);
pinMode(chD, INPUT);
pinMode(chE, INPUT);
pinMode(chF, INPUT);
}
// Main Program
void loop(){

//——————————————————————
// —————— Reciever Loop Setup ——————————-
//——————————————————————

// Read the input channels
ch1 = pulseIn(chA,HIGH); //Read and store channel 1
Serial.print(“Ch1:”); // Display text string on Serial Monitor to distinguise variables
Serial.print(ch1);
Serial.print(“|”);

ch2 = pulseIn(chB,HIGH);
Serial.print(“Ch2:”);
Serial.print(ch2);
Serial.print(“|”);

ch3 = pulseIn(chC,HIGH);
Serial.print(“Ch3:”);
Serial.print(ch3);
Serial.print(“|”);

ch4 = pulseIn(chD,HIGH);
Serial.print(“Ch4:”);
Serial.print(ch4);
Serial.print(“|”);

ch5 = pulseIn(chE,HIGH);
Serial.print(“Ch5:”);
Serial.print(ch5);
Serial.print(“|”);

ch6 = pulseIn(chF,HIGH);
Serial.print(“Ch6”);
Serial.print(ch6);
Serial.print(“|”);

//——————————————————————
//——————- Servo Loop Setup —————————–
//——————————————————————

// Rescaling the range taken from the reciever and suitable
// mapping them to the servos
ch1 = map(ch1, 900, 2000, 0, 180);
ch2 = map(ch2, 900, 2000, 0, 180);
ch3 = map(ch3, 900, 2000, 0, 180);
ch4 = map(ch4, 900, 2000, 0, 180);
ch5 = map(ch5, 900, 2000, 0, 180);
ch6 = map(ch6, 900, 2000, 0, 180);

// Serial printing values to show that it has been properly remapped

Serial.print(“Remap1: “);
Serial.print(ch1);

Serial.print(“Remap2: “);
Serial.print(ch2);

Serial.print(“Remap3: “);
Serial.print(ch3);

Serial.print(“Remap4: “);
Serial.print(ch4);

Serial.print(“Remap5: “);
Serial.print(ch5);

Serial.print(“Remap6: “);
Serial.println(ch6);

// Outputs to Servos
servo1.write(ch1);
servo2.write(ch2);
servo3.write(ch3);
servo4.write(ch4);
servo5.write(ch5);
servo6.write(ch6);

}

After searching the internet i found a great blog which have tutorials on arduino and rc control. http://rcarduino.blogspot.ie/

I modified code from this page http://rcarduino.blogspot.ie/2012/04/how-to-read-multiple-rc-channels-draft.html to work with 4 channels needed for the MeArm.  This program uses interrupts instead of using PulseIn to recognize when the pin in is High and shuts off when low. I am coding noob so check the blog link above for a proper tutorial on this. The program is modified to work with the mega 2560 which has 6 interrupts.

It works great very smooth movement from the servos. Thus should work with any standard rc reciever system, having a blast controlling the MeArm from a distance.

// MultiChannels
//
// rcarduino.blogspot.com
//
// A simple approach for reading three RC Channels using pin change interrupts
//
// See related posts –
// http://rcarduino.blogspot.co.uk/2012/01/how-to-read-rc-receiver-with.html
// http://rcarduino.blogspot.co.uk/2012/03/need-more-interrupts-to-read-more.html
// http://rcarduino.blogspot.co.uk/2012/01/can-i-control-more-than-x-servos-with.html
//
// rcarduino.blogspot.com
//

// include the pinchangeint library – see the links in the related topics section above for details
//#include

#include

// Assign your channel in pins
#define BASE_IN_PIN 2 // Pin 2 = Interrupt 0
#define Y_IN_PIN 3 // Pin 3 = Interrupt 1
#define X_IN_PIN 18 // Pin 18 = Interrupt 5
#define G_IN_PIN 19 // Pin 19 = Interrupt 4

// Assign Interrupt numbers, we will only use these in setup
#define BASE_IN_INTERRUPT 0 // Pin 2 = Interrupt 0
#define Y_IN_INTERRUPT 1 // Pin 3 = Interrupt 1
#define X_IN_INTERRUPT 5 // Pin 18 = Interrupt 5
#define G_IN_INTERRUPT 4 // Pin 19 = Interrupt 4

// Assign your channel out pins
#define BASE_OUT_PIN 5
#define Y_OUT_PIN 6
#define X_OUT_PIN 7
#define G_OUT_PIN 8

// Servo objects generate the signals expected by Electronic Speed Controllers and Servos
// We will use the objects to output the signals we read in
// this example code provides a straight pass through of the signal with no custom processing
Servo servoBase;
Servo servoY;
Servo servoX;
Servo servoG;

// These bit flags are set in bUpdateFlagsShared to indicate which
// channels have new signals
#define BASE_FLAG 1
#define Y_FLAG 2
#define X_FLAG 4
#define G_FLAG 4

// holds the update flags defined above
volatile uint8_t bUpdateFlagsShared;

// shared variables are updated by the ISR and read by loop.
// In loop we immediatley take local copies so that the ISR can keep ownership of the
// shared ones. To access these in loop
// we first turn interrupts off with noInterrupts
// we take a copy to use in loop and the turn interrupts back on
// as quickly as possible, this ensures that we are always able to receive new signals
volatile uint16_t unBaseInShared;
volatile uint16_t unYInShared;
volatile uint16_t unXInShared;
volatile uint16_t unGInShared;

// These are used to record the rising edge of a pulse in the calcInput functions
// They do not need to be volatile as they are only used in the ISR. If we wanted
// to refer to these in loop and the ISR then they would need to be declared volatile
uint32_t ulBaseStart;
uint32_t ulYStart;
uint32_t ulXStart;
uint32_t ulGStart;

void setup()
{
Serial.begin(9600);

Serial.println(“multiChannels”);

// attach servo objects, these will generate the correct
// pulses for driving Electronic speed controllers, servos or other devices
// designed to interface directly with RC Receivers
servoBase.attach(BASE_OUT_PIN);
servoY.attach(Y_OUT_PIN);
servoX.attach(X_OUT_PIN);
servoG.attach(G_OUT_PIN);

// using the PinChangeInt library, attach the interrupts
// used to read the channels
attachInterrupt(BASE_IN_INTERRUPT,calcBase,CHANGE);
attachInterrupt(Y_IN_INTERRUPT, calcY,CHANGE);
attachInterrupt(X_IN_INTERRUPT, calcX,CHANGE);
attachInterrupt(G_IN_INTERRUPT, calcG,CHANGE);
}

void loop()
{
// create local variables to hold a local copies of the channel inputs
// these are declared static so that thier values will be retained
// between calls to loop.
static uint16_t unBaseIn;
static uint16_t unYIn;
static uint16_t unXIn;
static uint16_t unGIn;

// local copy of update flags
static uint8_t bUpdateFlags;

// check shared update flags to see if any channels have a new signal
if(bUpdateFlagsShared)
{
noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables

// take a local copy of which channels were updated in case we need to use this in the rest of loop
bUpdateFlags = bUpdateFlagsShared;

// in the current code, the shared values are always populated
// so we could copy them without testing the flags
// however in the future this could change, so lets
// only copy when the flags tell us we can.

if(bUpdateFlags & BASE_FLAG)
{
unBaseIn = unBaseInShared;
}

if(bUpdateFlags & Y_FLAG)
{
unYIn = unYInShared;
}

if(bUpdateFlags & X_FLAG)
{
unXIn = unXInShared;
}
if(bUpdateFlags & G_FLAG)
{
unGIn= unGInShared;
}

// clear shared copy of updated flags as we have already taken the updates
// we still have a local copy if we need to use it in bUpdateFlags
bUpdateFlagsShared = 0;

interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on
// as soon as interrupts are back on, we can no longer use the shared copies, the interrupt
// service routines own these and could update them at any time. During the update, the
// shared copies may contain junk. Luckily we have our local copies to work with 🙂
}

// do any processing from here onwards
// only use the local values unAuxIn, unThrottleIn and unSteeringIn, the shared
// variables unAuxInShared, unThrottleInShared, unSteeringInShared are always owned by
// the interrupt routines and should not be used in loop

// the following code provides simple pass through
// this is a good initial test, the Arduino will pass through
// receiver input as if the Arduino is not there.
// This should be used to confirm the circuit and power
// before attempting any custom processing in a project.

// we are checking to see if the channel value has changed, this is indicated
// by the flags. For the simple pass through we don’t really need this check,
// but for a more complex project where a new signal requires significant processing
// this allows us to only calculate new values when we have new inputs, rather than
// on every cycle.
if(bUpdateFlags & BASE_FLAG)
{
if(servoBase.readMicroseconds() != unBaseIn)
{
servoBase.writeMicroseconds(unBaseIn);
}
}

if(bUpdateFlags & Y_FLAG)
{
if(servoY.readMicroseconds() != unYIn)
{
servoY.writeMicroseconds(unYIn);
}
}

if(bUpdateFlags & X_FLAG)
{
if(servoX.readMicroseconds() != unXIn)
{
servoX.writeMicroseconds(unXIn);
}
}

if(bUpdateFlags & G_FLAG)
{
if(servoG.readMicroseconds() != unGIn)
{
servoG.writeMicroseconds(unGIn);
}
}

bUpdateFlags = 0;
}

// simple interrupt service routine
void calcBase()
{
// if the pin is high, its a rising edge of the signal pulse, so lets record its value
if(digitalRead(BASE_IN_PIN) == HIGH)
{
ulBaseStart = micros();
}
else
{
// else it must be a falling edge, so lets get the time and subtract the time of the rising edge
// this gives use the time between the rising and falling edges i.e. the pulse duration.
unBaseInShared = (uint16_t)(micros() – ulBaseStart);
// use set the throttle flag to indicate that a new throttle signal has been received
bUpdateFlagsShared |= BASE_FLAG;
}
}

void calcY()
{
if(digitalRead(Y_IN_PIN) == HIGH)
{
ulYStart = micros();
}
else
{
unYInShared = (uint16_t)(micros() – ulYStart);
bUpdateFlagsShared |= Y_FLAG;
}
}

void calcX()
{
if(digitalRead(X_IN_PIN) == HIGH)
{
ulXStart = micros();
}
else
{
unXInShared = (uint16_t)(micros() – ulXStart);
bUpdateFlagsShared |= X_FLAG;
}
}

void calcG()
{
if(digitalRead(G_IN_PIN) == HIGH)
{
ulGStart = micros();
}
else
{
unGInShared = (uint16_t)(micros() – ulGStart);
bUpdateFlagsShared != G_FLAG;
}
}

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s