Control the Position of a Continuous Rotation Servo via WiFi using Arduino

Position control using wifi from a smartphone sounds too good to be true? Do it with MKR1000.

Things used in this project

Hardware components

Arduino MKR1000

Hand tools and fabrication machines

3D Printer (generic)

Story

Many position control use steppers because they are easy to deal with and are cheap. RC servos are easy and cheap too, but they are limited to half revolution. Continuous rotation servos can do more than half revolution but we cannot control position. This projects adds an inexpensive magnetic encoder that fits inside the servo to add the position sensing.

That means a single revolution now consists of 4096 steps or discrete angles, but any desired value of turns or steps can be achieved. Just remember these servos are not going to win any speed race.

But to make the project a bit more interesting for many of us, the use of the MKR1000 allows us to allow the control to happen wirelessly too.

I have a set of commands that are available both to USB communication or to UDP communication via WiFi. That means you can learn and test how it works connected to your computer USB and later you can move the module to another location and be accessed wirelessly.

For wireless communication I am using UDP protocol and port 23. The device IP address is reported to the serial terminal at the beginning so you can learn what IP is the board going to use. Please remember to insert in the source code two important values: your wifi network name (SSID) and password.

There is a really small circuit that can be done at a small SOIC-8 adapter board to be inserted inside the guts of the servo.

For accessing the controller wirelessly we need to know the IP address of it first, and for that we will need to use the Arduino IDE serial monitor:

Arduino IDE serial monitor

Once we know the IP address to connect to, we can use netcat (or nc) command to connect to the controller and send commands. I used telnet tool before but I realized better performance and stability was obtained using UDP instead.

In the image above three different commands have been issued. The first one ‘h’ is listing the help of available commands. Next is ‘q’ that reports the current set of values of PID controller that governs the closed-loop control of the servo motor. And third command ‘?’ reports the position of the output shaft, the controller output and the target location.

I used a continuous rotation servo for this project because it comes with its own power electronics, but this project can be easily adapted to other types of DC motors if you bring in an h-bridge control electronics and make a small change to the control code.

The most interesting command once everything is configured is the ‘X’ command that allows to send the servo to any desired location. Like X5000 for example will send it to the position 500 that, depending on your setup, will be a certain number of inches. Negative numbers are also allows but that will depend on where you set the zero of your motion platform.

Schematics

System diagram

Please note there is a magnet attached to the output shaft of the motor whose motion will be detected but the magnetic encoder chip. rect2985

Code

/* Miguel Sanchez 2106

   This program uses an Arduino MKR1000 for a closed-loop control of a 360servo.
   Motor motion is detected by a magnetic encoder.

   MKR1000 Pins used:
   I2C input is used for magnetic encoder SCL SDA
   Digital output 7 is servo control
   Please note PID gains kp, ki, kd need to be tuned to each different setup.
   Values can later stored to flash with W command for persistent storage
*/

#include <FlashStorage.h>  // for PID configuration storage
#include <PID_v1.h>
#include <Wire.h>         // support for I2C encoder
#include <Servo.h>

#include <WiFi101.h>
#include <WiFiUdp.h>

const char* ssid = "****";          // your home SSID
const char* password = "****";        // your wifi password

// Create an instance of the server
// specify the port to listen on as an argument
WiFiUDP client;



Servo servo;

byte pos[1000]; int p = 0;
double kp = .3, ki = 0, kd = 0.01;
double input = 0, output = 0, setpoint = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, REVERSE);
volatile long encoder0Pos = 0;
boolean auto1 = false, auto2 = false, counting = false;
long pos1;

long target1 = 0; // destination location at any moment

//for motor control ramps 1.4
bool newStep = false;
bool oldStep = false;
bool dir = false;
byte skip = 0;

word readTwoBytes()
{
  word retVal = -1;

  /* Read Low Byte */
  Wire.beginTransmission(0x36);
  Wire.write(0x0d);
  Wire.endTransmission();
  Wire.requestFrom(0x36, 1);
  while (Wire.available() == 0);
  int low = Wire.read();

  /* Read High Byte */
  Wire.beginTransmission(0x36);
  Wire.write(0x0c);
  Wire.endTransmission();
  Wire.requestFrom(0x36, 1);

  while (Wire.available() == 0);

  word high = Wire.read();

  high = high << 8;
  retVal = high | low;

  return retVal;
}

int angle, diff;
double before = 0;

void setup() {
  Serial.begin (115200);
  help();
  recoverPIDfromEEPROM();
  //Setup the pid
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(0, 180);
  Wire.begin(); // start I2C driver code D6 & D7
  servo.attach(7);
  servo.write(90);


  // Connect to WiFi network
  Serial.println();
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");

  // Start the server
  client.begin(23);
  Serial.println("UDP started");

  // Print the IP address
  IPAddress ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(ip);

}
void loop() {
  angle = readTwoBytes(); //analogRead(A0); // encoder0Pos;
  // process encoder rollover
  diff = angle - before;
  if (diff < -3500) pos1 += 4096;
  else if (diff > 3500) pos1 -= 4096;
  before = angle;
  input = pos1 + angle;


  //input = encoder0Pos;
  setpoint = target1;
  myPID.Compute();
  if (Serial.available()) process_line();
  if (client.parsePacket()) process_line_net();
  pwmOut(output);
  if (auto1) if (millis() % 3000 == 0) target1 = random(2000); // that was for self test with no input from main controller
  if (auto2) if (millis() % 1000 == 0) printPos();
  if (counting && abs(input - target1) < 15) counting = false;
  if (counting &&  (skip++ % 5) == 0 ) {
    pos[p] = input;
    if (p < 999) p++;
    else counting = false;
  }

}

// continuous servo output
void pwmOut(int out) {
  servo.write(out);
}

void process_line() {
  char cmd = Serial.read();
  if (cmd > 'Z') cmd -= 32;
  switch (cmd) {
    case 'P': kp = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break;
    case 'D': kd = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break;
    case 'I': ki = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break;
    case '?': printPos(); break;
    case 'X': target1 = Serial.parseInt(); counting = true; for (int i = 0; i < p; i++) pos[i] = 0; p = 0; break;
    case 'T': auto1 = !auto1; break;
    case 'A': auto2 = !auto2; break;
    case 'Q': Serial.print("P="); Serial.print(kp); Serial.print(" I="); Serial.print(ki); Serial.print(" D="); Serial.println(kd); break;
    case 'H': help(); break;
    case 'W': writetoEEPROM(); break;
    case 'K': //eedump(); break;
    case 'R': recoverPIDfromEEPROM() ; break;
    case 'S': for (int i = 0; i < p; i++) Serial.println(pos[i]); break;
    case 'M': pwmOut(Serial.parseInt()); break; // just ignore it unless you disable pwmOut in the main loop
  }
  while (Serial.read() != 10); // dump extra characters till LF is seen (you can use CRLF or just LF)
}


void process_line_net() {
  char cmd = client.read();
  if (cmd > 'Z') cmd -= 32;
  switch (cmd) {
    case 'P': kp = client.parseFloat(); myPID.SetTunings(kp, ki, kd); break;
    case 'D': kd = client.parseFloat(); myPID.SetTunings(kp, ki, kd); break;
    case 'I': ki = client.parseFloat(); myPID.SetTunings(kp, ki, kd); break;
    case '?': client.beginPacket(client.remoteIP(), client.remotePort()); client.print(F("Position=")); client.print(input); client.print(F(" PID_output=")); client.print(output); client.print(F(" Target=")); client.println(setpoint); client.endPacket(); break;
    case 'X': target1 = client.parseInt(); counting = true; for (int i = 0; i < p; i++) pos[i] = 0; p = 0; break;
    case 'T': auto1 = !auto1; break;
    case 'A': auto2 = !auto2; break;
    case 'Q': client.beginPacket(client.remoteIP(), client.remotePort()); client.print("P="); client.print(kp); client.print(" I="); client.print(ki); client.print(" D="); client.println(kd); client.endPacket(); break;
    case 'H': client.beginPacket(client.remoteIP(), client.remotePort()); help_net(); client.endPacket(); break;
    case 'W': writetoEEPROM(); break;
    case 'K': //eedump(); break;
    case 'R': recoverPIDfromEEPROM() ; break;
    case 'S': //for (int i = 0; i < p; i++) client.println(pos[i]); break;
    case 'M': pwmOut(client.parseInt()); break; // just ignore it unless you disable pwmOut in the main loop
  }
  while (client.read() != 10); // dump extra characters till LF is seen (you can use CRLF or just LF)
}
void printPos() {
  Serial.print(F("Position=")); Serial.print(input); Serial.print(F(" PID_output=")); Serial.print(output); Serial.print(F(" Target=")); Serial.println(setpoint);
}



void help() {
  Serial.println(F("\nPID DC motor controller and stepper interface emulator"));
  Serial.println(F("by misan"));
  Serial.println(F("Available serial commands: (lines end with CRLF or LF)"));
  Serial.println(F("P123.34 sets proportional term to 123.34"));
  Serial.println(F("I123.34 sets integral term to 123.34"));
  Serial.println(F("D123.34 sets derivative term to 123.34"));
  Serial.println(F("? prints out current encoder, output and setpoint values"));
  Serial.println(F("X123 sets the target destination for the motor to 123 encoder pulses"));
  Serial.println(F("T will start a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that"));
  Serial.println(F("Q will print out the current values of P, I and D parameters"));
  Serial.println(F("W will store current values of P, I and D parameters into EEPROM"));
  Serial.println(F("H will print this help message again"));
  Serial.println(F("A will toggle on/off showing regulator status every second\n"));
}


void help_net() {
  client.println(F("\nPID DC motor controller and stepper interface emulator"));
  client.println(F("by misan - porting cured by Exilaus"));
  client.println(F("Available serial commands: (lines end with CRLF or LF)"));
  client.println(F("P123.34 sets proportional term to 123.34"));
  client.println(F("I123.34 sets integral term to 123.34"));
  client.println(F("D123.34 sets derivative term to 123.34"));
  client.println(F("? prints out current encoder, output and setpoint values"));
  client.println(F("X123 sets the target destination for the motor to 123 encoder pulses"));
  client.println(F("T will start a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that"));
  client.println(F("Q will print out the current values of P, I and D parameters"));
  client.println(F("W will store current values of P, I and D parameters into EEPROM"));
  client.println(F("H will print this help message again"));
  client.println(F("A will toggle on/off showing regulator status every second\n"));
}