Important changes to forums and questions
All forums and questions are now archived. To start a new conversation or read the latest updates go to forums.mbed.com.
Servo w/Encoder
Topic last updated 10 Feb 2012, by 
Trent Beckinger.
3
replies
Trent Beckinger.
3
replies
I'm new to mbed programming and will be working with an mbed LPC1768 for my college senior project per my advisor's suggestion. I will probably have a lot of questions in the future, but at the moment I have a question about controlling a servo that has an encoder. The servo and how it works can be seen at this site: http://www.robotroom.com/FaulhaberGearmotor.html
Has anybody worked with this or something similar? Mainly at this point I just need to know what pins and how many I will need (i.e. how many PWM pins, edge-detection pins, etc.).
I have worked with this motor before using an Atmel 8051 controller, so if somebody wanted I could post my code that I used to drive it with the 8051.
Thanks,
Trent
Here is the code I am attempting to use. The algorithm is the same as the 8051 that works perfectly, so I'm sure it's just the syntax of the mbed getting me. Can anybody point out any problems? While testing, it seems as if the only PWM p21 creates is while input = 0. The PWM it creates is 50% duty cycle. None of the other inputs create PWMs.
Thanks, Trent
#include "mbed.h"
InterruptIn EncoderB(p5);
DigitalIn EncoderA(p6);
DigitalOut Enable(p7);
DigitalOut Bridge2(p8);
PwmOut Bridge7(p21);
Timer t;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
unsigned int time_interval;
signed int Encoder_0_Count = 0;
signed int input = 0;
signed int Error = -70, lasterror;
signed int derv_error, integral_error;
#define kp 15
#define kd 15
#define ki 5
#define plus 1
#define minus 0
void Encoder_Count(void) {
if (EncoderA) {
Encoder_0_Count--;
} else {
Encoder_0_Count++;
}
}
void Motor(long speed) {
if (speed >= 0) {
if (speed > 255) speed = 255;
Bridge2 = plus; //set reference to high
} else {
speed = -speed; //compliment if speed is negative
if (speed > 255) speed = 255; //set speed to max value if greater than 255
speed = 255 - speed; //set speed
Bridge2 = minus; //set reference to low
}
Bridge7.write((float)(speed/255.0));
}
int main() {
Enable = 1;
EncoderB.rise(&Encoder_Count);
while (1) {
t.start();
while (t.read_ms() < 50); //sample every 50ms or 20 samples per second
t.reset();
time_interval++;
if (time_interval > 60) { //change position every three seconds
time_interval = 0;
led1 = 0; //LED's just used for testing
led2 = 0;
led3 = 0;
led4 = 0;
if (input == 0) { //motor has 141 counts per revolution
input = 35; //35 is 90 degrees from start
led1 = 1;
} else if (input == 35) {
input = 70; //70 is 180 degrees from start
led2 = 1;
} else if (input == 70) {
input = 106; //106 is 270 degrees from start
led3 = 1;
} else if (input == 106) {
input = 0; //back to start
led4 = 1;
}
}
lasterror = Error;
Error = input - Encoder_0_Count; // calculate error
derv_error = Error - lasterror; // calculate derivative error
integral_error = integral_error + (lasterror + Error)/2; // calculate integral error
if (Error < -10 || Error > 10) {
integral_error = 0;
Motor((long)(kp*Error + kd*derv_error)); // send k1 x error to motor controller
} else
Motor((long)(kp*Error + kd*derv_error +(ki*integral_error)/2));
}
}
Here is the code I am attempting to use. The algorithm is the same as the 8051 that works perfectly, so I'm sure it's just the syntax of the mbed getting me. Can anybody point out any problems? While testing, it seems as if the only PWM p21 creates is while input = 0. The PWM it creates is 50% duty cycle. None of the other inputs create PWMs.
Thanks,
Trent
<<code>>
#include "mbed.h"
InterruptIn EncoderB(p5);
DigitalIn EncoderA(p6);
DigitalOut Enable(p7);
DigitalOut Bridge2(p8);
PwmOut Bridge7(p21);
Timer t;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
unsigned int time_interval;
signed int Encoder_0_Count = 0;
signed int input = 0;
signed int Error = -70, lasterror;
signed int derv_error, integral_error;
#define kp 15
#define kd 15
#define ki 5
#define plus 1
#define minus 0
void Encoder_Count(void) {
if (EncoderA) {
Encoder_0_Count--;
} else {
Encoder_0_Count++;
}
}
void Motor(long speed) {
if (speed >= 0) {
if (speed > 255) speed = 255;
Bridge2 = plus; //set reference to high
} else {
speed = -speed; //compliment if speed is negative
if (speed > 255) speed = 255; //set speed to max value if greater than 255
speed = 255 - speed; //set speed
Bridge2 = minus; //set reference to low
}
Bridge7.write((float)(speed/255.0));
}
int main() {
Enable = 1;
EncoderB.rise(&Encoder_Count);
while (1) {
t.start();
while (t.read_ms() < 50); //sample every 50ms or 20 samples per second
t.reset();
time_interval++;
if (time_interval > 60) { //change position every three seconds
time_interval = 0;
led1 = 0; //LED's just used for testing
led2 = 0;
led3 = 0;
led4 = 0;
if (input == 0) { //motor has 141 counts per revolution
input = 35; //35 is 90 degrees from start
led1 = 1;
} else if (input == 35) {
input = 70; //70 is 180 degrees from start
led2 = 1;
} else if (input == 70) {
input = 106; //106 is 270 degrees from start
led3 = 1;
} else if (input == 106) {
input = 0; //back to start
led4 = 1;
}
}
lasterror = Error;
Error = input - Encoder_0_Count; // calculate error
derv_error = Error - lasterror; // calculate derivative error
integral_error = integral_error + (lasterror + Error)/2; // calculate integral error
if (Error < -10 || Error > 10) {
integral_error = 0;
Motor((long)(kp*Error + kd*derv_error)); // send k1 x error to motor controller
} else
Motor((long)(kp*Error + kd*derv_error +(ki*integral_error)/2));
}
}
<</code>>
Hi Trent,
Technical english isn't my favourite, sorry ;-)
May be not very helpful, but we are using similar motors like this one.
They are not "servo-driven"...without a Controller it is just a DC motor with gear and HSE-encoder. We are steering it with an SPS (for the encoder-signals vs. recepies) communicating with an pure analog Motordriver (Mattke). If you have a Controller for this motor it should use the encoder to steer the motor.
What do you want to do with this drive? For what u need the encoder-signals? Or do you want to build up a Servo-Controller with the mBed?
Maybe i have missunderstood your question....i'm very interested ;-)
Greetings, Steff
Hi Trent,
Technical english isn't my favourite, sorry ;-)
May be not very helpful, but we are using similar motors like this one.
They are not "servo-driven"...without a Controller it is just a DC motor with gear and HSE-encoder. We are steering it with an SPS (for the encoder-signals vs. recepies) communicating with an pure analog Motordriver (Mattke).
If you have a Controller for this motor it should use the encoder to steer the motor.
What do you want to do with this drive? For what u need the encoder-signals? Or do you want to build up a Servo-Controller with the mBed?
Maybe i have missunderstood your question....i'm very interested ;-)
Greetings,
Steff
Steff,
This motor will be used to rotate a platform on a robot. More specifically, I am building an autonomous Lawnmowing robot with an omniwheel chassis (robot does not spin to change direction). Therefore, I am spinning a GPS receiver that is mounted on a platform. The platform is spun by this motor. My desired destinations are the origin (0 degrees), 90 degrees, 180 degrees, and 270 degrees.
I found a few hidden errors in my design, but have gotten them all fixed. The following code accomplishes what I was looking for.
Thanks, Trent
#include "mbed.h"
///
///Functions and pins declarations
///
InterruptIn EncoderA(p5);
DigitalIn EncoderB(p6);
DigitalOut Enable(p7);
DigitalOut Bridge2(p8);
PwmOut Bridge7(p21);
Timer t;
///
///Declarations of variables
///
unsigned int time_interval; //used to change rotation destination for a certain time interval
signed int Encoder_0_Count = 0; //used to count tics of the Faulhaber encoder
signed int input = 0; //used to set rotation destination
signed int Error = 0, lasterror; //used to determine speed and direction motor needs to go
signed int derv_error = 0, integral_error = 0; //used for derivative and integral portions of controller
#define kp 15 //proportional constant
#define kd 10 //derivative constant
#define ki 1 //integral constant
#define plus 1 //forward motion of motor
#define minus 0 //reverse motion of motor
///
///Function called when a falling edge interrupt occurs on Port 5, or Encoder A of the Faulhaber
///
void Encoder_Count_Low(void) {
if (EncoderB) { //if Encoder B is high when Encoder A triggers an interrupt
Encoder_0_Count--; //decrment the count
} else {
Encoder_0_Count++; //otherwise increment the count
}
}
///
///Function called to send direction and speed to the motor through an H-bridge (SN754410)
///
void Motor(signed int speed) {
if (speed >= 0) {
if (speed > 255) speed = 255;
speed = 255 - speed; //set speed (bridge sinks signal when reference is high, so complement speed to get correct PWM)
Bridge2 = plus; //set reference to high
} else {
speed = -speed; //compliment if speed is negative
if (speed > 255) speed = 255; //set speed to max value if greater than 255
Bridge2 = minus; //set reference to low
}
Bridge7.period_us(50); //period of PWM set to 50 useconds
Bridge7.write((float)(speed/256.0)); //sets duty cycle of PWM
}
///
///Main function of code
///
int main() {
Enable = 1; //enable the H-bridge, thus enabling the motor
EncoderA.fall(&Encoder_Count_Low); //initialize Encoder A interrupt
while (1) {
t.start();
while (t.read_ms() < 50); //sample every 50ms or 20 samples per second
t.reset();
if (++time_interval > 200) { //change position every ten(200*50ms) seconds
time_interval = 0;
if (input == 0*8) //motor has 141 counts per revolution
input = 35*8; //35 is 90 degrees from start, times 8 to get 90 degree rotation with 8:1 gear ratio
else if (input == 35*8)
input = 70*8; //70 is 180 degrees from start, times 8 to get 180 degree rotation with 8:1 gear ratio
else if (input == 70*8)
input = 106*8; //106 is 270 degrees from start, times 8 to get 270 degree rotation with 8:1 gear ratio
else if (input == 106*8)
input = 0; //back to start
}
lasterror = Error;
Error = input - Encoder_0_Count; // calculate error
derv_error = Error - lasterror; // calculate derivative error
integral_error = integral_error + (lasterror + Error)/2; // calculate integral error
if (Error < -10 || Error > 10) { //if we are far away from our goal, reset integral error and exclude it from calculations
integral_error = 0;
Motor(kp*Error + kd*derv_error); // send proportional and derivative portions to motor controller
} else
Motor(kp*Error + kd*derv_error +(ki*integral_error)/2); //send entire PID to motor controller
}
}
Steff,
This motor will be used to rotate a platform on a robot. More specifically, I am building an autonomous Lawnmowing robot with an omniwheel chassis (robot does not spin to change direction). Therefore, I am spinning a GPS receiver that is mounted on a platform. The platform is spun by this motor. My desired destinations are the origin (0 degrees), 90 degrees, 180 degrees, and 270 degrees.
I found a few hidden errors in my design, but have gotten them all fixed. The following code accomplishes what I was looking for.
Thanks,
Trent
<<code>>
#include "mbed.h"
///
///Functions and pins declarations
///
InterruptIn EncoderA(p5);
DigitalIn EncoderB(p6);
DigitalOut Enable(p7);
DigitalOut Bridge2(p8);
PwmOut Bridge7(p21);
Timer t;
///
///Declarations of variables
///
unsigned int time_interval; //used to change rotation destination for a certain time interval
signed int Encoder_0_Count = 0; //used to count tics of the Faulhaber encoder
signed int input = 0; //used to set rotation destination
signed int Error = 0, lasterror; //used to determine speed and direction motor needs to go
signed int derv_error = 0, integral_error = 0; //used for derivative and integral portions of controller
#define kp 15 //proportional constant
#define kd 10 //derivative constant
#define ki 1 //integral constant
#define plus 1 //forward motion of motor
#define minus 0 //reverse motion of motor
///
///Function called when a falling edge interrupt occurs on Port 5, or Encoder A of the Faulhaber
///
void Encoder_Count_Low(void) {
if (EncoderB) { //if Encoder B is high when Encoder A triggers an interrupt
Encoder_0_Count--; //decrment the count
} else {
Encoder_0_Count++; //otherwise increment the count
}
}
///
///Function called to send direction and speed to the motor through an H-bridge (SN754410)
///
void Motor(signed int speed) {
if (speed >= 0) {
if (speed > 255) speed = 255;
speed = 255 - speed; //set speed (bridge sinks signal when reference is high, so complement speed to get correct PWM)
Bridge2 = plus; //set reference to high
} else {
speed = -speed; //compliment if speed is negative
if (speed > 255) speed = 255; //set speed to max value if greater than 255
Bridge2 = minus; //set reference to low
}
Bridge7.period_us(50); //period of PWM set to 50 useconds
Bridge7.write((float)(speed/256.0)); //sets duty cycle of PWM
}
///
///Main function of code
///
int main() {
Enable = 1; //enable the H-bridge, thus enabling the motor
EncoderA.fall(&Encoder_Count_Low); //initialize Encoder A interrupt
while (1) {
t.start();
while (t.read_ms() < 50); //sample every 50ms or 20 samples per second
t.reset();
if (++time_interval > 200) { //change position every ten(200*50ms) seconds
time_interval = 0;
if (input == 0*8) //motor has 141 counts per revolution
input = 35*8; //35 is 90 degrees from start, times 8 to get 90 degree rotation with 8:1 gear ratio
else if (input == 35*8)
input = 70*8; //70 is 180 degrees from start, times 8 to get 180 degree rotation with 8:1 gear ratio
else if (input == 70*8)
input = 106*8; //106 is 270 degrees from start, times 8 to get 270 degree rotation with 8:1 gear ratio
else if (input == 106*8)
input = 0; //back to start
}
lasterror = Error;
Error = input - Encoder_0_Count; // calculate error
derv_error = Error - lasterror; // calculate derivative error
integral_error = integral_error + (lasterror + Error)/2; // calculate integral error
if (Error < -10 || Error > 10) { //if we are far away from our goal, reset integral error and exclude it from calculations
integral_error = 0;
Motor(kp*Error + kd*derv_error); // send proportional and derivative portions to motor controller
} else
Motor(kp*Error + kd*derv_error +(ki*integral_error)/2); //send entire PID to motor controller
}
}
<</code>>
I'm new to mbed programming and will be working with an mbed LPC1768 for my college senior project per my advisor's suggestion. I will probably have a lot of questions in the future, but at the moment I have a question about controlling a servo that has an encoder. The servo and how it works can be seen at this site: http://www.robotroom.com/FaulhaberGearmotor.html Has anybody worked with this or something similar? Mainly at this point I just need to know what pins and how many I will need (i.e. how many PWM pins, edge-detection pins, etc.).
I have worked with this motor before using an Atmel 8051 controller, so if somebody wanted I could post my code that I used to drive it with the 8051.
Thanks, Trent