Please provide better instructions on how to flash the firmware

[phretor]

I'm testing the PID control and the motors act weird. I'd like to make sure it's not the (old) firmware's fault (I'm running 0.0.6), but it's not very clear how to flash the carrier board.

[facchinm]

Hi @phretor,
firmware 0.0.6 has some bugs that has been solved in the meantime; to flash the a new firmware you simply need to:

• make sure that library is updated (1.0.3 right now)
• open the Flasher example
• upload it on the user-programmable MKR board (MKR1000 for example)
• briefly remove the power from both the Motor carrier and the user board
• open the MKR1000 serial port with any program (like Arduino IDE serial monitor)
• the new firmware will be automatically flashed and the new fw version will be reported if the flash completed successfully.

Let me know if the procedure works fine!

[phretor]

Hi @facchinm,

thanks for the quick answer. The flashing procedure did work: I'm now happily running 0.0.8. At some point I'll fork the repo and convert this issue into a wiki page as a courtesy to the next passenger.

I'm still experiencing a weird PID control action on the motors: not sure if it's the reading from the encoders that are not accurate, or the actuation. I've tried various Kp, Ki, Kd combos, with no significant improvements.

I'm using the DC motors with encoder that ship with the Arduino Engineering Kit, hooked up as M1 and M2, and encoders to Hall sensors HA1-HB1 and HA2-HB2, respectively. I've inverted the +/- so that a set point to, say, position X would mean "go forward" for both motors, and inverted the polarity of the encoder wirings accordingly.

Here's the code I'm playing with. I think you want to look at the moveBoth function, as the rest is mostly MQTT command parsing and connectivity:

#include <WiFi101.h>
#include <SPI.h>
#include <PubSubClient.h>

#include <MKRMotorCarrier.h>

#define PID_P 10.0
#define PID_I 0.0
#define PID_D 3.0
#define MIN_FORK 9
#define MAX_FORK 100
#define MAX_ACCEL 200
#define SPEED 25
#define MIN_SPEED 25
#define MAX_SPEED 70
#define PREFIX "/rover/command"

char ssid[] = "************";
char pass[] = "***********";

int status = WL_IDLE_STATUS;
float kp = PID_P;
float ki = PID_I;
float kd = PID_D;

WiFiClient wifiClient;
PubSubClient client(wifiClient);
IPAddress server(192, 168, 42, 225);

void commandSetGainP(float g) {
  kp = g;
}

void commandSetGainI(float g) {
  ki = g;
}

void commandSetGainD(float g) {
  kd = g;
}

void moveBoth(int target1, int target2) {
  Serial.print("Move to target: ");
  Serial.print(target1);
  Serial.print(", ");
  Serial.println(target2);

  // motor 1 = right
  // motor 2 = left
  
  encoder1.resetCounter(0);
  encoder2.resetCounter(0);
  
  pid1.setControlMode(CL_POSITION);
  pid2.setControlMode(CL_POSITION);

  pid1.setMaxAcceleration(MAX_ACCEL);
  pid2.setMaxAcceleration(MAX_ACCEL);

  pid1.setLimits(MIN_SPEED, MAX_SPEED);
  pid2.setLimits(MIN_SPEED, MAX_SPEED);
  
  pid1.setGains(kp, ki, kd);
  pid2.setGains(kp, ki, kd);

  pid1.setSetpoint(TARGET_POSITION, target1);
  pid2.setSetpoint(TARGET_POSITION, target2);
}

void commandGoto(int pos) {
  moveBoth(pos, pos);
}

void commandForward(int speed) {
  M1.setDuty(speed);
  M2.setDuty(speed);
}

void commandBackward(int speed) {
  M1.setDuty(-speed);
  M2.setDuty(-speed);
}

void commandStop() {
  M1.setDuty(0); 
  M2.setDuty(0);

  delay(10);

  encoder1.resetCounter(0);
  encoder2.resetCounter(0);
}

void commandTurn(int pos) {
  //moveBoth(RIGHT, LEFT);
  
  if (pos < 0) //turn left (left goes back, right goes ahead)
    moveBoth(abs(pos), -abs(pos));

  if (pos > 0) //turn right
    moveBoth(-abs(pos), abs(pos));
}

void commandFork(int pos) {
  if (pos < MIN_FORK)
    pos = MIN_FORK;

  if (pos > MAX_FORK)
    pos = MAX_FORK;

  servo1.setAngle(pos);
}

int payloadToInt(byte* payload, unsigned int length) {
  String payload_s = String();
  
  for (int i=0; i < length; i++)
    payload_s.concat((char)payload[i]);

  return payload_s.toInt();
}

int payloadToFloat(byte* payload, unsigned int length) {
  String payload_s = String();
  
  for (int i=0; i < length; i++)
    payload_s.concat((char)payload[i]);

  return payload_s.toFloat();
}

void parseMqttMessage(char* topic, byte* payload, unsigned int length) {
  String topic_s = String(topic);

  if (topic_s.startsWith("/rover/command/pid/p")) {
    commandSetGainP(payloadToFloat(payload, length));
    return;
  }
  
  if (topic_s.startsWith("/rover/command/pid/i")) {
    commandSetGainI(payloadToFloat(payload, length));
    return;
  }
  
  if (topic_s.startsWith("/rover/command/pid/d")) {
    commandSetGainP(payloadToFloat(payload, length));
    return;
  }
  
  int pos = payloadToInt(payload, length);

  if (topic_s.startsWith("/rover/command/stop")) {
    commandStop();
    return;
  }

  if (topic_s.startsWith("/rover/command/goto")) {
    commandGoto(pos);
    return;
  }
  
  if (topic_s.startsWith("/rover/command/forward")) {
    commandForward(pos);
    return;
  }
  
  if (topic_s.startsWith("/rover/command/backward")) {
    commandBackward(pos);
    return;
  }
  
  if (topic_s.startsWith("/rover/command/turn")) {
    commandTurn(pos);
    return;
  }
  
  if (topic_s.startsWith("/rover/command/fork")) {
    commandFork(pos);
    return;
  }
}

void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");

  String payload_s = String();
  
  for (int i=0; i < length; i++)
    payload_s.concat((char)payload[i]);

  Serial.println(payload_s);
  
  if (String(topic).startsWith(PREFIX))
    parseMqttMessage(topic, payload, length);
}

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("arduinoClient")) {
      Serial.println("connected");
      // Once connected, publish an announcement...
      client.publish("/rover/status","online");
      // ... and resubscribe
      client.subscribe("/rover/command/#");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

void printWifiData() {
  // print your WiFi shield's IP address:
  IPAddress ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(ip);
  Serial.println(ip);

  // print your MAC address:
  byte mac[6];
  WiFi.macAddress(mac);
  Serial.print("MAC address: ");
  Serial.print(mac[5], HEX);
  Serial.print(":");
  Serial.print(mac[4], HEX);
  Serial.print(":");
  Serial.print(mac[3], HEX);
  Serial.print(":");
  Serial.print(mac[2], HEX);
  Serial.print(":");
  Serial.print(mac[1], HEX);
  Serial.print(":");
  Serial.println(mac[0], HEX);

}

void printCurrentNet() {
  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());

  // print the MAC address of the router you're attached to:
  byte bssid[6];
  WiFi.BSSID(bssid);
  Serial.print("BSSID: ");
  Serial.print(bssid[5], HEX);
  Serial.print(":");
  Serial.print(bssid[4], HEX);
  Serial.print(":");
  Serial.print(bssid[3], HEX);
  Serial.print(":");
  Serial.print(bssid[2], HEX);
  Serial.print(":");
  Serial.print(bssid[1], HEX);
  Serial.print(":");
  Serial.println(bssid[0], HEX);

  // print the received signal strength:
  long rssi = WiFi.RSSI();
  Serial.print("signal strength (RSSI):");
  Serial.println(rssi);

  // print the encryption type:
  byte encryption = WiFi.encryptionType();
  Serial.print("Encryption Type:");
  Serial.println(encryption, HEX);
  Serial.println();
}

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

  delay(10);

  while (status != WL_CONNECTED)
  {
    Serial.print("Attempting to connect to WPA SSID: ");
    Serial.println(ssid);
    status = WiFi.begin(ssid, pass);
    Serial.println("Status: ");
    Serial.println(status);

    delay(500);
  }

  Serial.print("You're connected to the network");
  
  printCurrentNet();
  printWifiData();

  client.setServer(server, 1883);
  client.setCallback(callback);

  // Start communicationg with the motor shield
  if (controller.begin()) {
    Serial.print("MKR Motor Shield connected, firmware version ");
    Serial.println(controller.getFWVersion());
  } else {
    Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch");
    while (1);
  }

  // Reboot the motor controller; brings every value back to default
  controller.reboot();
}

void loop() {
  if (!client.connected()) {
    reconnect();
  }

  Serial.print(encoder1.getRawCount());
  Serial.print(" ");
  Serial.print(encoder2.getRawCount());
  Serial.print("\n");

  client.loop();
}

Here's a video that gives an idea of the issue I'm talking about, when issuing two commands:

1. go to 1000 position
2. go to -1000 position

Video is hosted here: https://www.dropbox.com/s/1sfr8br4hew7sz7/IMG_0307.MOV?dl=0

As you can see, the motors move a bit erratically, as if under the effect of bad control. If I simply set the duty cycle of the motors to, say, 50%, they will run smoothly. So, the erratic movement must be part of the control.

I can try with different PID parameters or do other tests, and I'm happy to contribute with the full working example once completed.

[facchinm]

Glad the firmware update worked smoothly!
I think the PID problem might be related with the missing call to controller.ping() .
Regularly calling that function is compulsory (at least once every three seconds), otherwise the motors will stop (this was added as a safety measure in case the controlling board stops responding). Simply add it before client.loop(); and it should be enough

[phretor]

Actually, nothing really changed. I had it there yesterday (not sure how it got removed), each 100ms.

I'm now calling it each 1000ms, but the twitching movement is still there.

[facchinm]

@ErnestoELC does it look familiar to you? Could you try to reproduce the issue?

[ErnestoELC]

Hi @phretor
I was able to reproduce the shaky behavior of the motors when using the PID. I am not sure what is the reason for it yet. It might be the way the PID is implemented or that value of the gains are not properly tuned.
We'll dig a bit more and come back to you.
Thanks!

[phretor]

Thanks for testing this out @ErnestoELC - makes me feel less lonely :-)

Should you need any tests on my side, just shoot a message here or in private.

[facchinm]

This should be fixed with latest 0.10 firmware. Reopen if needed