Circuit

Bomlist:

  • The servo I use is RDS3128 (28kgcm torque, 8V voltage, $20).
  • The DC motor driver is MC33926 from Pololu ($17).
  • And the MCU is an Arduino with Atmega32u4 on it(same configuration as Arduino Leonardo but smaller and cheaper $5)

I desoldered the original circuit board in the servo.

And I soldered 3 wires on the potentiometer and two wires on the DC motor.

Then I connected all the circuit on the bread board following the instruction on the Pololu MC33926 product page.

Torque sensing

Basically when I use my finger to stop it, the required torque increases which makes the current higher. The MC33926 has got current feedback pin, the higher the current, the higher the output voltage on its current feedback pin. I read the voltage on this pin, when it’s higher than the threshold I set, I make the servo runs to the opposite direction.

const byte INV_PIN = 14;
const byte D2PWM_PIN = 13;
const byte FB_PIN = 0;
const byte POT_PIN = 1;

int fb_value = 0;
int pot_value = 0;
int dir = 1;

void setup() {
  // put your setup code here, to run once:
  pinMode(D2PWM_PIN, OUTPUT);
  Serial.begin(9600);

  analogWrite(D2PWM_PIN, 20);
  digitalWrite(INV_PIN, dir);

}

void loop() {
  // put your main code here, to run repeatedly:
  //fb_value = analogRead(FB_PIN);
  Serial.print(fb_value);
  Serial.print(" ");
  pot_value = analogRead(POT_PIN);
  Serial.println(pot_value);

  fb_value = 0;
  for(int i = 0; i < 20; i ++){
      fb_value += analogRead(FB_PIN);  
  }
  fb_value = fb_value / 20;

  if(fb_value > 30){
    dir = 1 - dir;
    digitalWrite(INV_PIN, dir);
    delay(10);
  }

}

PID position control

It still oscillates too much. I will add another program on PC to set the PID parameters next time.

#include <PID_v1.h>

const byte INV_PIN = 14;
const byte D2PWM_PIN = 13;
const byte FB_PIN = 0;
const byte POT_PIN = 1;

const int POT_CALI = 543; // the pot reading when the servo is 90 degree
const double POT_V = 4.9;  // 4.9 unit of pot reading is 1 degree

int fb_value = 0;
int pot_value = 0;
int dir = 1;

double set_pos, input_pos, output_pwm;
double Ku = 0.8, Tu = 0.5;
double Kp = Ku * 0.6, Ki = Tu/2.0, Kd = Tu/8.0;
//double Kp = Ku * 0.6, Ki = 0, Kd = 0;
PID servo_pid(&input_pos, &output_pwm, &set_pos, Kp, Ki, Kd, DIRECT);

//serial stuff
int start_byte;
int is_PID; // if the parameters are for ku, tu or kp, ki, kd

double pot2angle(int pot){
  return ( ((double)(pot - POT_CALI)) / POT_V + 90.0);
}

int angle2pot(double angle){
  return (int)((angle - 90.0) * POT_V + POT_CALI);
}

void serialListen(){
  if(Serial.available() > 2){
    startbyte = Serial.parseInt();
    if(startbyte == 255){
      is_PID = Serial.parseInt();
      if(is_PID){
        int kp = Serial.parseInt();
        int ki = Serial.parseInt();
        int kd = Serial.parseInt();
        set_pos = Serial.parseInt(); 
        Kp = (double)kp / 100.0;
        Ki = (double)Ki / 100.0;
        Kd = (double)Kd / 100.0;
      }
      else{
        int ku = Serial.parseInt();
        int tu = Serial.parseInt();
        set_pos = Serial.parseInt(); 
        Kp = (double) Ku / 100.0 * 0.6;
        Ki = (double) Tu / 100.0 / 2.0;
        Kd = (double) Tu / 100.0 / 8.0;
      }
      servo_pid.SetTunings(Kp, Ki, Kd);
    }
  } 
}

void updatePos(){
  pot_value = analogRead(POT_PIN);
  //Serial.println(pot_value);
  input_pos = pot2angle(pot_value);

  servo_pid.Compute();

  if(output_pwm > 0){
    analogWrite(D2PWM_PIN, output_pwm);
    dir = 1;
    digitalWrite(INV_PIN, dir);
  }
  else if(output_pwm < 0){
    analogWrite(D2PWM_PIN, -output_pwm);
    dir = 0;
    digitalWrite(INV_PIN, dir);
  }
  else{
    analogWrite(D2PWM_PIN, 0);  
  }
}

void setup() {
  // put your setup code here, to run once:
  pinMode(D2PWM_PIN, OUTPUT);
  Serial.begin(9600);

  analogWrite(D2PWM_PIN, 0);
  //digitalWrite(INV_PIN, dir);

  set_pos = 90;

  servo_pid.SetOutputLimits(-255, 255);
  servo_pid.SetMode(AUTOMATIC);
}

void loop() {
  serialListen();
  updatePos();
}