background:
I'm trying to control some geared DC servo motors with PID rather than the built in electronics and PWM. I'm hoping to achieve smoother and more controllable motion from a cheap hobby servo. I intend to use these hacked servos in a miniature pick and place delta robot. anyways, i've removed the control PCB, and soldered leads onto the motor and potentiometer. i'm driving the motor with an L293d dual Hbridge IC, and controlling everything with an arduino uno. I've written some simple code using the PID library by Brett Beauregard to drive the motor back and forth, pausing in between. so far this is all working as it should.
The problem:
tuning the PID values seems to be a tricky process, and I don't think i've got it really right, but I can achieve relatively smooth movement, and a stable landing without too much oscillation. probably this will get better as i add in some tighter timing constraints. the problem i've encountered is one of torque. when the motor is just holding position it's very strong, but when it's moving, it's very weak. if I attempt to slow it down it gets even weaker. I'm not sure if this is a tuning issue or something to do with my code... these motors are usually pretty strong with the original control, so i know they can do better.
I have a feeling it's my code.. at this point i'm just incrementing the pid setpoint each cycle to try to get smooth controllable motion. I think it never sends full power to the motor because it's always so close to the setpoint.
here's my code (i'm sorry it's kindof sloppy.. there's a bunch of stuff in there still from when i was sending setpoints directly through the serial monitor):
#include <PID_v1.h>
float speedFactor = .5; //actually just steps per tick
//feedback pins coming from motors (pots)
const int feedbackPin[3] = {A0, A1, A2};
//PWM pins to motor drivers
const int torquePin[3] = {10,11,12};
//direction pins to motor drivers
const int forwardsPin[3] = {3,5,7};
const int backwardsPin[3] = {4,6,8};
//for serial input
const byte numChars = 5;
char receivedChars[numChars];
float integerFromPC = 500;
boolean newData = false;
//PID tunings
float Kp1 =12;
float Ki1 = .00;
float Kd1 = 0.07;
float Kp2 = 9;
float Ki2 = .1;
float Kd2 = .1;
float Kp3 = 9;
float Ki3 = .1;
float Kd3 = .1;
// these are all 3 dimensional arrays because i'm going to have 3 motors. only one is connected at this point.
float setPoint[3] = {0,0,0};
float feedback[3] = {0,0,0};
float error[3] = {0,0,0};
float output[3] = {0,0,0};
float torque[3] = {0,0,0};
float currentPoint[3] = {0,0,0};
float nextPoint[3] = {0,0,0};
float currentGoal[3] = {50,50,50};
boolean whichWay[3] = {true,true,true};
boolean paused = false;
float pauseCount=0;
int i;
PID pid1(&error[0], &torque[0], 0, Kp1, Ki1 ,Kd1, REVERSE);
PID pid2(&error[1], &torque[1], 0, Kp2, Ki2,Kd2, REVERSE);
PID pid3(&error[2], &torque[2], 0, Kp3, Ki3,Kd3, REVERSE);
void setup() {
// i've been using the serial port for debugging, but it makes things run slow
// which affects the pid tuning, so i turn off the output function when testing.
Serial.begin(19200);
Serial.println("Ready");
get_feedback();
for (i=0; i<=2; i++){
setPoint[i]= feedback[i];
currentPoint[i]=feedback[i];
}
calc_error();
//set output pins
for (i=0; i<=2; i++){
pinMode(forwardsPin[i], OUTPUT);
pinMode(backwardsPin[i], OUTPUT);
pinMode(torquePin[i], OUTPUT);
}
pid1.SetSampleTime(2);
pid1.SetMode(AUTOMATIC);
pid2.SetSampleTime(2);
pid2.SetMode(AUTOMATIC);
pid3.SetSampleTime(2);
pid3.SetMode(AUTOMATIC);
}
void loop() {
get_feedback();
set_setPoint();
//check_input(); //right now we're using the serial port to change the setpoint, but this will be changed later
// display_data(); //comment out this line to improve performance once the motors are tuned
calc_error();
move_motors(); //this includes the PID calculations for each axis.. consider changing this to two separate functions to improve motor synchronization
//delay(100);
}
void get_feedback(){
for (i=0; i<=2; i++){
feedback[i] = analogRead(feedbackPin[i]);
}
}
void check_input() {
recvWithStartEndMarkers();
parseData() ;
storeNewData() ;
}
void calc_error(){
for(i=0; i<=2; i++){
error[i] = abs(setPoint[i] - feedback[i]);
}
}
void set_setPoint() {
for (i=0; i<=0; i++){
if (paused==false){
if (currentPoint[i]>=950) {
whichWay[i]=false;
paused=true;
pause();
}
if (currentPoint[i]<=50) {
whichWay[i]=true;
paused=true;
pause();
}
if (whichWay[i]==true) {
if ((abs(feedback[i]-setPoint[i]))<5){
setPoint[i]=(currentPoint[i]=currentPoint[i]+speedFactor);
}
}
else {
if ((abs(feedback[i]-setPoint[i]))<5){
setPoint[i]=(currentPoint[i]=currentPoint[i]-speedFactor);
}
}
}
else {
pause();
}
}
}
void pause(){
if (pauseCount==10000){
paused=false;
pauseCount=0;
}
else {
pauseCount++;
}
}
void recvWithStartEndMarkers() {
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = '<';
char endMarker = '>';
char rc;
// if (Serial.available() > 0) {
while (Serial.available() > 0 && newData == false) {
rc = Serial.read();
if (recvInProgress == true) {
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
recvInProgress = false;
ndx = 0;
newData = true;
}
}
else if (rc == startMarker) {
recvInProgress = true;
}
}
}
void storeNewData() {
if (newData == true) {
set_setPoint();
newData = false;
}
}
void parseData() {
integerFromPC = atoi(receivedChars); // convert this part to an integer
}
void display_data(){
for (i=0;i<=0; i++){
Serial.print(feedback[i]);
Serial.print(" -- ");
Serial.print(setPoint[i]);
Serial.print(" -- ");
Serial.print(whichWay[i]);
}
Serial.print("\n");
}
//trying to figure out how to make this more compact...
int move_motors(){
if(pid1.Compute()){
//Write enable pins
if (setPoint[0] - feedback[0] > 0){
digitalWrite(forwardsPin[0], LOW);
digitalWrite(backwardsPin[0], HIGH);
}
else if (setPoint[0] - feedback[0] < 0){
digitalWrite(forwardsPin[0], HIGH);
digitalWrite(backwardsPin[0], LOW);
}
analogWrite(torquePin[0], torque[0]);
}
}